Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
391 Brean 1
#include <Parser.h>
2
 
3
// Base64 Decoder
4
// see Parser.h for details about sRxData
396 Brean 5
bool Parser::decode64(sRxData &rx)
391 Brean 6
{
393 Brean 7
    int length = rx.str.size();
391 Brean 8
    unsigned char a,b,c,d;
9
    unsigned char ptr = 0;
10
    unsigned char x,y,z;
11
    int ptrOut[150];
12
 
13
    int ptrIn = 3;
14
    int max = length;
15
    int len = length;
16
    int decLen = 0;
17
 
396 Brean 18
    if (rx.input[ptrIn] == 0) {
19
        return false;
20
        //TODO: catch error to show that something went wrong during the decode process
21
        //throw "Nothing received";
391 Brean 22
    }
23
 
24
    while(len != 0) {
393 Brean 25
        a = rx.input[ptrIn++] - '=';
26
        b = rx.input[ptrIn++] - '=';
27
        c = rx.input[ptrIn++] - '=';
28
        d = rx.input[ptrIn++] - '=';
391 Brean 29
 
30
        if(ptrIn > max - 2) break;
31
 
32
        x = (a << 2) | (b >> 4);
33
        y = ((b & 0x0f) << 4) | (c >> 2);
34
        z = ((c & 0x03) << 6) | d;
35
 
36
        if(len--) ptrOut[ptr++] = x; else break;
37
        if(len--) ptrOut[ptr++] = y; else break;
38
        if(len--) ptrOut[ptr++] = z; else break;
39
    }
40
 
41
    for (int a=0; a<ptr; a++) {
396 Brean 42
        if (length == false) {
391 Brean 43
            int b1, b2, b3;
44
 
45
            b1 = ptrOut[a++];
46
            b2 = ptrOut[a];
47
 
48
            b3 = (b2 << 8) | b1;
49
 
50
            if (b3 > 32767)
51
                b3 = b3 - 65536;
52
 
396 Brean 53
            rx.decode[decLen] = b3;
54
            decLen++;
391 Brean 55
        } else {
396 Brean 56
            rx.decode[decLen] = ptrOut[a];
57
            decLen++;
391 Brean 58
        }
396 Brean 59
        rx.decLen = decLen;
391 Brean 60
    }
396 Brean 61
    return true;
391 Brean 62
}
63
 
64
// base64 encoder
65
string Parser::encode64(char data[150],unsigned int length)
66
{
67
    unsigned int pt = 0;
68
    unsigned char a,b,c;
69
    unsigned char ptr = 0;
70
 
71
    char tx_buff[150];
72
 
396 Brean 73
    while(length > 0)
391 Brean 74
    {
396 Brean 75
        if(length) { a = data[ptr++]; length--;} else a = 0;
76
        if(length) { b = data[ptr++]; length--;} else b = 0;
77
        if(length) { c = data[ptr++]; length--;} else c = 0;
391 Brean 78
 
79
        tx_buff[pt++] = '=' + (a >> 2);
80
        tx_buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
81
        tx_buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
82
        tx_buff[pt++] = '=' + ( c & 0x3f);
83
    }
84
    tx_buff[pt] = 0;
85
 
86
    return string(tx_buff);
87
}
88
 
89
// Datensatz nach 8bit Integer
90
int Parser::dataToChar(int *data , int start, bool is_signed) {
91
    int out = (data[start]);
92
 
93
    if ((out > 128) && (is_signed))
94
      out = out - 256;
95
 
96
    return out;
97
}
98
 
99
// received char to 8 bit integer
100
int Parser::charToData(int data) {
101
    if (data < 0)
102
        return data + 256;
103
    return data;
104
}
105
 
106
// convert data to 16bit Integer
107
int Parser::dataToInt(int *Data , int Start, bool is_signed)
108
{
109
    int Out = (Data[Start+1]<<8) | (Data[Start+0]);
110
 
111
    if ((Out > 32767) && (is_signed))
112
      Out = Out - 65536;
113
 
114
    return Out;
115
 
116
}
117
 
118
// convert data to 32bit Long
119
long Parser::dataToLong(int *Data , int Start, bool is_signed)
120
{
121
    long Out = (Data[Start+3]<<24) | (Data[Start+2]<<16) | (Data[Start+1]<<8) | (Data[Start+0]);
122
 
123
    if ((Out > 32767) && (is_signed))
124
      Out = Out;
125
 
126
    return Out;
127
}
128
 
129
float Parser::getFloat(long value, int count)
130
{
396 Brean 131
    long num = pow(10, count);
391 Brean 132
 
133
    float temp = value;
134
 
396 Brean 135
    return temp / num;
391 Brean 136
}
137
 
138
string Parser::dataToString(int Data[150], int Start, int End)
139
{
140
    char String[150];
141
    for (int a = Start; a < End; a++)
142
    {
143
        String[a - Start] = Data[a];
144
    }
145
    String[End - Start] = '\0';
146
 
147
    return string(String);
148
}
149
 
150
// check CRC
396 Brean 151
bool Parser::check_CRC(string RXstr)
391 Brean 152
{
396 Brean 153
    int length = RXstr.size();
391 Brean 154
    int CRC = 0;
396 Brean 155
    char *RX = (char *)RXstr.c_str();
391 Brean 156
 
157
    if (RX[1] == 127)
158
    {
159
        RX[1] = 0;
160
    }
161
 
162
    for(int i=0; i < length-2; i++)
163
    {
164
            CRC+=RX[i];
165
    }
166
 
167
    CRC = CRC % 4096;
168
 
169
    if(RX[length - 2] != ('=' + (CRC / 64)))
170
    {
171
        return false;
172
    }
173
 
174
    if(RX[length - 1] != ('=' + CRC % 64))
175
    {
176
        return false;
177
    }
178
 
179
    return true;
180
}
181
 
182
// add CRC
396 Brean 183
string Parser::add_CRC(string TX)
391 Brean 184
{
396 Brean 185
    int length = TX.size();
391 Brean 186
    unsigned int tmpCRC = 0;
187
 
188
    char CRC[2];
189
 
190
    for(int i = 0; i < length; i++)
191
    {
192
        tmpCRC += TX[i];
193
    }
194
 
195
    tmpCRC %= 4096;
196
 
197
    CRC[0] = '=' + tmpCRC / 64;
198
    CRC[1] = '=' + tmpCRC % 64;
199
    CRC[2] = '\0';
200
 
201
    return string(TX) + string(CRC);
202
}
203