Rev 450 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 450 | Rev 513 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <Parser.h> |
1 | #include <Parser.h> |
| - | 2 | #include <iostream> |
|
| Line 2... | Line 3... | ||
| 2 | 3 | ||
| 3 | /** |
4 | /** |
| 4 | * create a frame that can be send to the MK using the |
5 | * create a frame that can be send to the MK using the |
| 5 | * connection class. |
6 | * connection class. |
| 6 | * see http://www.mikrokopter.com/ucwiki/en/SerialProtocol |
7 | * see http://www.mikrokopter.com/ucwiki/en/SerialProtocol |
| 7 | * how the protocol is encoded and |
8 | * how the protocol is encoded and |
| 8 | * see http://www.mikrokopter.com/ucwiki/en/SerialCommands |
9 | * see http://www.mikrokopter.com/ucwiki/en/SerialCommands |
| 9 | * to look at the possible commands that are already coded |
10 | * to look at the possible commands that are already coded |
| 10 | * in data |
11 | * in data |
| 11 | */ |
12 | */ |
| 12 | void Parser::create_frame(char cmd, int address, char * data, unsigned int length) { |
13 | void Parser::create_frame(char * send_data, char cmd, int address, char * data, unsigned int length) { |
| 13 | //if # is cmd we do not touch anything, because |
14 | //if # is cmd we do not touch anything, because |
| 14 | // the command is already encoded in data |
15 | // the command is already encoded in data |
| 15 | if (cmd != '#') { |
16 | if (cmd != '#') { |
| 16 | /* |
17 | /* |
| Line 21... | Line 22... | ||
| 21 | 22 | ||
| 22 | //allociate memory for the data we want to send |
23 | //allociate memory for the data we want to send |
| 23 | char * send_data = (char *)malloc(buf_len); |
24 | char * send_data = (char *)malloc(buf_len); |
| 24 | */ |
25 | */ |
| 25 | Parser::encode64(data, length); |
- | |
| - | 26 | Parser::encode64(data, length); |
|
| 26 | char send_data[150]; |
27 | |
| 27 | send_data[0]='#'; |
28 | send_data[0]='#'; |
| 28 | send_data[1]=(char)address+'a'; |
29 | send_data[1]=(char)address+'a'; |
| 29 | send_data[2]=cmd; |
30 | send_data[2]=cmd; |
| 30 | for (int i = 0; i < length; i++) |
31 | for (int i = 0; i < length; i++) |
| Line 48... | Line 49... | ||
| 48 | unsigned char a,b,c,d; |
49 | unsigned char a,b,c,d; |
| 49 | unsigned char ptr = 0; |
50 | unsigned char ptr = 0; |
| 50 | unsigned char x,y,z; |
51 | unsigned char x,y,z; |
| Line 51... | Line 52... | ||
| 51 | 52 | ||
| 52 | int decLen = 0; |
53 | int decLen = 0; |
| - | 54 | ||
| 53 | /* |
55 | std::cout << "decode64" << offset << " " << len << std::endl; |
| 54 | //FIXME: dies wieder einklammern! |
56 | FlightLog::log_data(data, len); |
| 55 | if (data[ptrIn] == 0) { |
57 | if (data[offset] == 0) { |
| 56 | return -1; |
58 | //return -1; |
| 57 | //TODO: catch error to show that something went wrong during the decode process |
59 | //TODO: catch error to show that something went wrong during the decode process |
| - | 60 | //throw "Nothing received"; |
|
| 58 | //throw "Nothing received"; |
61 | FlightLog::warning("incorrect data received"); |
| 59 | } |
- | |
| 60 | */ |
62 | } |
| 61 | //decode data |
63 | //decode data |
| 62 | while(len) { |
64 | while(len) { |
| 63 | a = data[offset++] - '='; |
65 | a = data[offset++] - '='; |
| 64 | b = data[offset++] - '='; |
66 | b = data[offset++] - '='; |
| Line 197... | Line 199... | ||
| 197 | bool Parser::check_CRC(char * rx, int length) |
199 | bool Parser::check_CRC(char * rx, int length) |
| 198 | { |
200 | { |
| 199 | int CRC = 0; |
201 | int CRC = 0; |
| Line 200... | Line 202... | ||
| 200 | 202 | ||
| 201 | if (rx[1] == 127) |
- | |
| 202 | { |
203 | if (rx[1] == 127) |
| 203 | rx[1] = 0; |
- | |
| Line 204... | Line 204... | ||
| 204 | } |
204 | rx[1] = 0; |
| 205 | - | ||
| 206 | for(int i=0; i < length-2; i++) |
205 | |
| 207 | { |
- | |
| Line 208... | Line 206... | ||
| 208 | CRC+=rx[i]; |
206 | for(int i=0; i < length-2; i++) |
| Line 209... | Line 207... | ||
| 209 | } |
207 | CRC+=rx[i]; |
| 210 | - | ||
| 211 | CRC = CRC % 4096; |
208 | |
| 212 | - | ||
| Line 213... | Line 209... | ||
| 213 | if(rx[length - 2] != ('=' + (CRC / 64))) |
209 | CRC = CRC % 4096; |
| 214 | { |
- | |
| 215 | return false; |
210 | |
| 216 | } |
- | |
| Line 217... | Line 211... | ||
| 217 | 211 | if(rx[length - 2] != ('=' + (CRC / 64))) |
|
| 218 | if(rx[length - 1] != ('=' + CRC % 64)) |
212 | return false; |
| Line 219... | Line 213... | ||
| 219 | { |
213 | |
| Line 229... | Line 223... | ||
| 229 | void Parser::add_CRC(char * tx, int length) |
223 | void Parser::add_CRC(char * tx, int length) |
| 230 | { |
224 | { |
| 231 | unsigned int tmpCRC = 0; |
225 | unsigned int tmpCRC = 0; |
| Line 232... | Line 226... | ||
| 232 | 226 | ||
| 233 | for(int i = 0; i < length; i++) |
- | |
| 234 | { |
227 | for(int i = 0; i < length; i++) |
| 235 | tmpCRC += tx[i]; |
- | |
| Line 236... | Line 228... | ||
| 236 | } |
228 | tmpCRC += tx[i]; |
| Line 237... | Line 229... | ||
| 237 | 229 | ||
| 238 | tmpCRC %= 4096; |
230 | tmpCRC %= 4096; |
| 239 | 231 | ||
| 240 | tx[length] = '=' + tmpCRC / 64; |
232 | tx[length] = '=' + tmpCRC / 64; |
| 241 | tx[length+1] = '=' + tmpCRC % 64; |
- | |