AruFraming.cpp

00001 #include "mbed.h"
00002 #include "AruFraming.h"
00003 //#include <stdio.h>
00004 //#include <algorithm>
00005 
00006 AruFraming::AruFraming()
00007 {
00008 
00009 }
00010 
00011 AruFraming::~AruFraming()
00012 {
00013 
00014 }
00015 
00016 std::string ConvertInt(int data)
00017 {
00018     char c[8];
00019     sprintf(c,"%X", data);
00020     return c;
00021 }
00022 
00023 void AruFraming::ProcessFromAru(const std::vector<int> &stream)
00024 {
00025     AruFrm f;
00026     if(stream[0] != 0x02)return;
00027 
00028     f.Identifier = stream[1];
00029 
00030     switch(f.Identifier)
00031     {
00032         case 'e':
00033         case 'i':
00034         case 'r':
00035         case 'v':
00036         case 'n': {
00037             unsigned int byteCnt = find(stream.begin(), stream.end(), /*'*'*/0x03) - stream.begin();
00038 
00039             if (byteCnt < stream.size())
00040             {
00041                 for(unsigned int i=2; i<byteCnt; i++)
00042                 {
00043                     f.Data.push_back(stream[i]);
00044                 }
00045             }
00046             else
00047             {
00048                 return;
00049             }
00050             break;
00051         }
00052         case 'a':
00053         case 'c':
00054         case 't':
00055         case 's':{
00056             f.Data.clear();
00057             break;
00058         }
00059         default :
00060             return;
00061     }
00062     RCV_QUE.push(f);
00063 }
00064 
00065 std::vector<int> AruFraming::CreateFrame(const AruFrm &f)
00066 {
00067     std::vector<int> result;
00068 
00069     result.push_back(0x02);
00070     result.push_back(f.Identifier);
00071 
00072     int sz = f.Data.size();
00073     if(sz > 0)
00074     {
00075         for(int i=0; i<sz; i++)
00076         {
00077             result.push_back(f.Data[i] & 0x0FF);
00078         }
00079     }
00080 
00081     result.push_back(0x03);
00082 
00083     unsigned int LRC = CRC16(result);
00084     std::string ret = ConvertInt(LRC);
00085     std::string res = ConvertInt(LRC);
00086 
00087     result.push_back((int)res[2] & 0x0FF);
00088     result.push_back((int)res[3] & 0x0FF);
00089     result.push_back((int)res[0] & 0x0FF);
00090     result.push_back((int)res[1] & 0x0FF);
00091 
00092     return result;
00093 }
00094 
00095 /* Table of CRC values for high–order byte  */
00096 unsigned int AruFraming::auchCRCHi[] = {
00097 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
00098 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
00099 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
00100 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
00101 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
00102 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
00103 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
00104 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
00105 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
00106 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
00107 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
00108 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
00109 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
00110 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
00111 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
00112 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
00113 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
00114 0x40
00115 } ;
00116 
00117 /* Table of CRC values for low–order byte  */
00118 unsigned int AruFraming::auchCRCLo[] = {
00119 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4,
00120 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
00121 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD,
00122 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
00123 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7,
00124 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
00125 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE,
00126 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
00127 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2,
00128 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
00129 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB,
00130 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
00131 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91,
00132 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
00133 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88,
00134 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
00135 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80,
00136 0x40
00137 } ;
00138 
00139 unsigned int AruFraming::CRC16(const std::vector<int> &puchMsg)
00140 {
00141     unsigned int uchCRCHi = 0x0FF ; /* high byte of CRC initialized */
00142     unsigned int uchCRCLo = 0x0FF ; /* low byte of CRC initialized */
00143     unsigned int uIndex ; /* will index into CRC lookup table */
00144 
00145     int ptr = 0;
00146     int usDataLen = (int)puchMsg.size();
00147     while (usDataLen > 0) /* pass through message buffer */
00148     {
00149         usDataLen--;
00150 
00151         uIndex = uchCRCHi ^ (unsigned int)puchMsg[ptr++] ; /* calculate the CRC */
00152         uchCRCHi = uchCRCLo ^ auchCRCHi[uIndex];
00153         uchCRCLo = auchCRCLo[uIndex];
00154     }
00155     return (uchCRCHi << 8 | uchCRCLo);
00156 }