Subversion Repositories Projects

#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);

}