Subversion Repositories Projects

Rev

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>
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
        /*
17
        //calculate buffer length
18
        //calculate buffer length
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++)
31
            send_data[i+3] = data[i];
32
            send_data[i+3] = data[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;
51
 
52
 
52
    int decLen = 0;
53
    int decLen = 0;
53
/*
54
 
-
 
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
58
        //throw "Nothing received";
60
        //throw "Nothing received";
-
 
61
        FlightLog::warning("incorrect data received");
59
    }
62
    }
60
*/
-
 
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++] - '=';
65
        c = data[offset++] - '=';
67
        c = 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;
200
 
202
 
201
    if (rx[1] == 127)
203
    if (rx[1] == 127)
202
    {
-
 
203
        rx[1] = 0;
204
        rx[1] = 0;
204
    }
-
 
205
 
205
 
206
    for(int i=0; i < length-2; i++)
206
    for(int i=0; i < length-2; i++)
207
    {
-
 
208
            CRC+=rx[i];
207
        CRC+=rx[i];
209
    }
-
 
210
 
208
 
211
    CRC = CRC % 4096;
209
    CRC = CRC % 4096;
212
 
210
 
213
    if(rx[length - 2] != ('=' + (CRC / 64)))
211
    if(rx[length - 2] != ('=' + (CRC / 64)))
214
    {
-
 
215
        return false;
212
        return false;
216
    }
-
 
217
 
213
 
218
    if(rx[length - 1] != ('=' + CRC % 64))
214
    if(rx[length - 1] != ('=' + CRC % 64))
219
    {
-
 
220
        return false;
215
        return false;
221
    }
-
 
222
 
216
 
223
    return true;
217
    return true;
224
}
218
}
225
 
219
 
226
/**
220
/**
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;
232
 
226
 
233
    for(int i = 0; i < length; i++)
227
    for(int i = 0; i < length; i++)
234
    {
-
 
235
        tmpCRC += tx[i];
228
        tmpCRC += tx[i];
236
    }
-
 
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;
233
    tx[length+1] = '=' + tmpCRC % 64;
242
    tx[length+2] = '\0';
234
    tx[length+2] = '\0';
243
}
235
}
244
 
-