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