0,0 → 1,203 |
#include <Parser.h> |
|
// Base64 Decoder |
// see Parser.h for details about sRxData |
bool Parser::decode64(sRxData &rx) |
{ |
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; |
//TODO: catch error to show that something went wrong during the decode process |
//throw "Nothing received"; |
} |
|
while(len != 0) { |
a = rx.input[ptrIn++] - '='; |
b = rx.input[ptrIn++] - '='; |
c = rx.input[ptrIn++] - '='; |
d = rx.input[ptrIn++] - '='; |
|
if(ptrIn > max - 2) break; |
|
x = (a << 2) | (b >> 4); |
y = ((b & 0x0f) << 4) | (c >> 2); |
z = ((c & 0x03) << 6) | d; |
|
if(len--) ptrOut[ptr++] = x; else break; |
if(len--) ptrOut[ptr++] = y; else break; |
if(len--) ptrOut[ptr++] = z; else break; |
} |
|
for (int a=0; a<ptr; a++) { |
if (length == false) { |
int b1, b2, b3; |
|
b1 = ptrOut[a++]; |
b2 = ptrOut[a]; |
|
b3 = (b2 << 8) | b1; |
|
if (b3 > 32767) |
b3 = b3 - 65536; |
|
rx.decode[decLen] = b3; |
decLen++; |
} else { |
rx.decode[decLen] = ptrOut[a]; |
decLen++; |
} |
rx.decLen = decLen; |
} |
return true; |
} |
|
// base64 encoder |
string Parser::encode64(char data[150],unsigned int length) |
{ |
unsigned int pt = 0; |
unsigned char a,b,c; |
unsigned char ptr = 0; |
|
char tx_buff[150]; |
|
while(length > 0) |
{ |
if(length) { a = data[ptr++]; length--;} else a = 0; |
if(length) { b = data[ptr++]; length--;} else b = 0; |
if(length) { c = data[ptr++]; length--;} else c = 0; |
|
tx_buff[pt++] = '=' + (a >> 2); |
tx_buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
tx_buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
tx_buff[pt++] = '=' + ( c & 0x3f); |
} |
tx_buff[pt] = 0; |
|
return string(tx_buff); |
} |
|
// Datensatz nach 8bit Integer |
int Parser::dataToChar(int *data , int start, bool is_signed) { |
int out = (data[start]); |
|
if ((out > 128) && (is_signed)) |
out = out - 256; |
|
return out; |
} |
|
// received char to 8 bit integer |
int Parser::charToData(int data) { |
if (data < 0) |
return data + 256; |
return data; |
} |
|
// convert data to 16bit Integer |
int Parser::dataToInt(int *Data , int Start, bool is_signed) |
{ |
int Out = (Data[Start+1]<<8) | (Data[Start+0]); |
|
if ((Out > 32767) && (is_signed)) |
Out = Out - 65536; |
|
return Out; |
|
} |
|
// 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]); |
|
if ((Out > 32767) && (is_signed)) |
Out = Out; |
|
return Out; |
} |
|
float Parser::getFloat(long value, int count) |
{ |
long num = pow(10, count); |
|
float temp = value; |
|
return temp / num; |
} |
|
string Parser::dataToString(int Data[150], int Start, int End) |
{ |
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) |
{ |
RX[1] = 0; |
} |
|
for(int i=0; i < length-2; i++) |
{ |
CRC+=RX[i]; |
} |
|
CRC = CRC % 4096; |
|
if(RX[length - 2] != ('=' + (CRC / 64))) |
{ |
return false; |
} |
|
if(RX[length - 1] != ('=' + CRC % 64)) |
{ |
return false; |
} |
|
return true; |
} |
|
// add CRC |
string Parser::add_CRC(string TX) |
{ |
int length = TX.size(); |
unsigned int tmpCRC = 0; |
|
char CRC[2]; |
|
for(int i = 0; i < length; i++) |
{ |
tmpCRC += TX[i]; |
} |
|
tmpCRC %= 4096; |
|
CRC[0] = '=' + tmpCRC / 64; |
CRC[1] = '=' + tmpCRC % 64; |
CRC[2] = '\0'; |
|
return string(TX) + string(CRC); |
} |
|