| 1,34 → 1,73 |
|---|
| #include <Parser.h> |
| |
| // Base64 Decoder |
| // see Parser.h for details about sRxData |
| bool Parser::decode64(sRxData &rx) |
| /** |
| * create a frame that can be send to the MK using the |
| * connection class. |
| * see http://www.mikrokopter.com/ucwiki/en/SerialProtocol |
| * how the protocol is encoded and |
| * see http://www.mikrokopter.com/ucwiki/en/SerialCommands |
| * to look at the possible commands that are already coded |
| * in data |
| */ |
| void Parser::create_frame(char cmd, int address, char * data, unsigned int length) { |
| //if # is cmd we do not touch anything, because |
| // the command is already encoded in data |
| if (cmd != '#') { |
| /* |
| //calculate buffer length |
| //(4 Bytes for 1-byte '#', 1-byte address, and 2-byte crc) |
| //(the rest for the data length - see encode how this is calculated) |
| int buff_len = 4+(length/3 + (length%3==0?0:1) )*4; |
| |
| //allociate memory for the data we want to send |
| char * send_data = (char *)malloc(buf_len); |
| */ |
| char send_data[150]; |
| send_data[0]='#'; |
| send_data[1]=(char)address; |
| send_data[2]=cmd; |
| for (int i = 0; i < length; i++) |
| send_data[i+3] = data[i]; |
| //TODO: abgleich mit MKCommunication::send_command |
| Parser::encode64(send_data, length); |
| address = 'a' + address; |
| |
| } |
| } |
| |
| |
| /** |
| * Base64 Decoder |
| * see Parser.h for details about sRxData |
| * data = data that will be decoded |
| * len = length of data |
| * ptrOut = pointer to decoded data |
| * offset = offset in data |
| */ |
| int Parser::decode64(char * data, int len, unsigned char *ptrOut, int offset) |
| { |
| int length = rx.str.size(); |
| unsigned char a,b,c,d; |
| unsigned char ptr = 0; |
| unsigned char x,y,z; |
| int ptrOut[150]; |
| |
| int ptrIn = 3; |
| int max = length; |
| int len = length; |
| int decLen = 0; |
| |
| if (rx.input[ptrIn] == 0) { |
| return false; |
| /* |
| //FIXME: dies wieder einklammern! |
| if (data[ptrIn] == 0) { |
| return -1; |
| //TODO: catch error to show that something went wrong during the decode process |
| //throw "Nothing received"; |
| } |
| */ |
| //decode data |
| while(len) { |
| a = data[offset++] - '='; |
| b = data[offset++] - '='; |
| c = data[offset++] - '='; |
| d = data[offset++] - '='; |
| |
| while(len != 0) { |
| a = rx.input[ptrIn++] - '='; |
| b = rx.input[ptrIn++] - '='; |
| c = rx.input[ptrIn++] - '='; |
| d = rx.input[ptrIn++] - '='; |
| //if(offset > max - 2) break; |
| |
| if(ptrIn > max - 2) break; |
| |
| x = (a << 2) | (b >> 4); |
| y = ((b & 0x0f) << 4) | (c >> 2); |
| z = ((c & 0x03) << 6) | d; |
| 37,9 → 76,13 |
|---|
| if(len--) ptrOut[ptr++] = y; else break; |
| if(len--) ptrOut[ptr++] = z; else break; |
| } |
| |
| //decoded data |
| unsigned char * decData; |
| for (int a=0; a<ptr; a++) { |
| if (length == false) { |
| if (len) { |
| decData[decLen] = ptrOut[a]; |
| decLen++; |
| } else { |
| int b1, b2, b3; |
| |
| b1 = ptrOut[a++]; |
| 50,19 → 93,18 |
|---|
| if (b3 > 32767) |
| b3 = b3 - 65536; |
| |
| rx.decode[decLen] = b3; |
| decData[decLen] = b3; |
| decLen++; |
| } else { |
| rx.decode[decLen] = ptrOut[a]; |
| decLen++; |
| } |
| rx.decLen = decLen; |
| } |
| return true; |
| ptrOut = decData; |
| return decLen; |
| } |
| |
| // base64 encoder |
| string Parser::encode64(char data[150],unsigned int length) |
| /** |
| * base64 encoder |
| */ |
| void Parser::encode64(char data[150],unsigned int length) |
| { |
| unsigned int pt = 0; |
| unsigned char a,b,c; |
| 83,7 → 125,8 |
|---|
| } |
| tx_buff[pt] = 0; |
| |
| return string(tx_buff); |
| //move pointer of tx_buff to data |
| data = tx_buff; |
| } |
| |
| // Datensatz nach 8bit Integer |
| 96,7 → 139,9 |
|---|
| return out; |
| } |
| |
| // received char to 8 bit integer |
| /** |
| * received char to 8 bit integer |
| */ |
| int Parser::charToData(int data) { |
| if (data < 0) |
| return data + 256; |
| 103,7 → 148,9 |
|---|
| return data; |
| } |
| |
| // convert data to 16bit Integer |
| /** |
| * convert data to 16bit Integer |
| */ |
| int Parser::dataToInt(int *Data , int Start, bool is_signed) |
| { |
| int Out = (Data[Start+1]<<8) | (Data[Start+0]); |
| 115,7 → 162,9 |
|---|
| |
| } |
| |
| // convert data to 32bit Long |
| /** |
| * convert data to 32bit Long |
| */ |
| long Parser::dataToLong(int *Data , int Start, bool is_signed) |
| { |
| long Out = (Data[Start+3]<<24) | (Data[Start+2]<<16) | (Data[Start+1]<<8) | (Data[Start+0]); |
| 135,43 → 184,31 |
|---|
| return temp / num; |
| } |
| |
| string Parser::dataToString(int Data[150], int Start, int End) |
| /** |
| * check CRC |
| */ |
| bool Parser::check_CRC(char * rx, int length) |
| { |
| char String[150]; |
| for (int a = Start; a < End; a++) |
| { |
| String[a - Start] = Data[a]; |
| } |
| String[End - Start] = '\0'; |
| |
| return string(String); |
| } |
| |
| // check CRC |
| bool Parser::check_CRC(string RXstr) |
| { |
| int length = RXstr.size(); |
| int CRC = 0; |
| char *RX = (char *)RXstr.c_str(); |
| |
| if (RX[1] == 127) |
| if (rx[1] == 127) |
| { |
| RX[1] = 0; |
| rx[1] = 0; |
| } |
| |
| for(int i=0; i < length-2; i++) |
| { |
| CRC+=RX[i]; |
| CRC+=rx[i]; |
| } |
| |
| CRC = CRC % 4096; |
| |
| if(RX[length - 2] != ('=' + (CRC / 64))) |
| if(rx[length - 2] != ('=' + (CRC / 64))) |
| { |
| return false; |
| } |
| |
| if(RX[length - 1] != ('=' + CRC % 64)) |
| if(rx[length - 1] != ('=' + CRC % 64)) |
| { |
| return false; |
| } |
| 179,25 → 216,22 |
|---|
| return true; |
| } |
| |
| // add CRC |
| string Parser::add_CRC(string TX) |
| /** |
| * create CRC and add it to tx |
| */ |
| void Parser::add_CRC(char * tx, int length) |
| { |
| int length = TX.size(); |
| unsigned int tmpCRC = 0; |
| |
| char CRC[2]; |
| |
| for(int i = 0; i < length; i++) |
| { |
| tmpCRC += TX[i]; |
| tmpCRC += tx[i]; |
| } |
| |
| tmpCRC %= 4096; |
| |
| CRC[0] = '=' + tmpCRC / 64; |
| CRC[1] = '=' + tmpCRC % 64; |
| CRC[2] = '\0'; |
| |
| return string(TX) + string(CRC); |
| tx[length-2] = '=' + tmpCRC / 64; |
| tx[length-1] = '=' + tmpCRC % 64; |
| tx[length] = '\0'; |
| } |
| |