Subversion Repositories Projects

Rev

Rev 450 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 450 Rev 513
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
18
        //(4 Bytes for 1-byte '#', 1-byte address, and 2-byte crc)
19
        //(4 Bytes for 1-byte '#', 1-byte address, and 2-byte crc)
19
        //(the rest for the data length - see encode how this is calculated)
20
        //(the rest for the data length - see encode how this is calculated)
20
        int buff_len = 4+(length/3 + (length%3==0?0:1) )*4;
21
        int buff_len = 4+(length/3 + (length%3==0?0:1) )*4;
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];
32
        add_CRC(send_data, length+3);
33
        add_CRC(send_data, length+3);
33
        data = send_data;
34
        data = send_data;
34
    }
35
    }
35
}
36
}
36
 
37
 
37
 
38
 
38
/**
39
/**
39
 * Base64 Decoder
40
 * Base64 Decoder
40
 * see Parser.h for details about sRxData
41
 * see Parser.h for details about sRxData
41
 * data = data that will be decoded
42
 * data = data that will be decoded
42
 * len = length of data
43
 * len = length of data
43
 * ptrOut = pointer to decoded data
44
 * ptrOut = pointer to decoded data
44
 * offset = offset in data
45
 * offset = offset in data
45
 */
46
 */
46
int Parser::decode64(char * data, int len, unsigned char *ptrOut, int offset)
47
int Parser::decode64(char * data, int len, unsigned char *ptrOut, int offset)
47
{
48
{
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++] - '=';
66
        d = data[offset++] - '=';
68
        d = data[offset++] - '=';
67
 
69
 
68
        //if(offset > max - 2) break;
70
        //if(offset > max - 2) break;
69
 
71
 
70
        x = (a << 2) | (b >> 4);
72
        x = (a << 2) | (b >> 4);
71
        y = ((b & 0x0f) << 4) | (c >> 2);
73
        y = ((b & 0x0f) << 4) | (c >> 2);
72
        z = ((c & 0x03) << 6) | d;
74
        z = ((c & 0x03) << 6) | d;
73
 
75
 
74
        if(len--) ptrOut[ptr++] = x; else break;
76
        if(len--) ptrOut[ptr++] = x; else break;
75
        if(len--) ptrOut[ptr++] = y; else break;
77
        if(len--) ptrOut[ptr++] = y; else break;
76
        if(len--) ptrOut[ptr++] = z; else break;
78
        if(len--) ptrOut[ptr++] = z; else break;
77
    }
79
    }
78
    //decoded data
80
    //decoded data
79
    unsigned char * decData;
81
    unsigned char * decData;
80
    for (int a=0; a<ptr; a++) {
82
    for (int a=0; a<ptr; a++) {
81
        if (len) {
83
        if (len) {
82
            decData[decLen] = ptrOut[a];
84
            decData[decLen] = ptrOut[a];
83
            decLen++;
85
            decLen++;
84
        } else {
86
        } else {
85
            int b1, b2, b3;
87
            int b1, b2, b3;
86
 
88
 
87
            b1 = ptrOut[a++];
89
            b1 = ptrOut[a++];
88
            b2 = ptrOut[a];
90
            b2 = ptrOut[a];
89
 
91
 
90
            b3 = (b2 << 8) | b1;
92
            b3 = (b2 << 8) | b1;
91
 
93
 
92
            if (b3 > 32767)
94
            if (b3 > 32767)
93
                b3 = b3 - 65536;
95
                b3 = b3 - 65536;
94
 
96
 
95
            decData[decLen] = b3;
97
            decData[decLen] = b3;
96
            decLen++;
98
            decLen++;
97
        }
99
        }
98
    }
100
    }
99
    ptrOut = decData;
101
    ptrOut = decData;
100
    return decLen;
102
    return decLen;
101
}
103
}
102
 
104
 
103
/**
105
/**
104
 * base64 encoder
106
 * base64 encoder
105
 */
107
 */
106
void Parser::encode64(char data[150],unsigned int length)
108
void Parser::encode64(char data[150],unsigned int length)
107
{
109
{
108
    unsigned int pt = 0;
110
    unsigned int pt = 0;
109
    unsigned char a,b,c;
111
    unsigned char a,b,c;
110
    unsigned char ptr = 0;
112
    unsigned char ptr = 0;
111
 
113
 
112
    char tx_buff[150];
114
    char tx_buff[150];
113
 
115
 
114
    while(length > 0)
116
    while(length > 0)
115
    {
117
    {
116
        if(length) { a = data[ptr++]; length--;} else a = 0;
118
        if(length) { a = data[ptr++]; length--;} else a = 0;
117
        if(length) { b = data[ptr++]; length--;} else b = 0;
119
        if(length) { b = data[ptr++]; length--;} else b = 0;
118
        if(length) { c = data[ptr++]; length--;} else c = 0;
120
        if(length) { c = data[ptr++]; length--;} else c = 0;
119
 
121
 
120
        tx_buff[pt++] = '=' + (a >> 2);
122
        tx_buff[pt++] = '=' + (a >> 2);
121
        tx_buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
123
        tx_buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
122
        tx_buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
124
        tx_buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
123
        tx_buff[pt++] = '=' + ( c & 0x3f);
125
        tx_buff[pt++] = '=' + ( c & 0x3f);
124
    }
126
    }
125
    tx_buff[pt] = 0;
127
    tx_buff[pt] = 0;
126
 
128
 
127
    //move pointer of tx_buff to data
129
    //move pointer of tx_buff to data
128
    data = tx_buff;
130
    data = tx_buff;
129
}
131
}
130
 
132
 
131
// Datensatz nach 8bit Integer
133
// Datensatz nach 8bit Integer
132
int Parser::dataToChar(char *data , int start, bool is_signed) {
134
int Parser::dataToChar(char *data , int start, bool is_signed) {
133
    int out = (data[start]);
135
    int out = (data[start]);
134
 
136
 
135
    if ((out > 128) && (is_signed))
137
    if ((out > 128) && (is_signed))
136
      out = out - 256;
138
      out = out - 256;
137
 
139
 
138
    return out;
140
    return out;
139
}
141
}
140
 
142
 
141
/**
143
/**
142
 * received char to 8 bit integer
144
 * received char to 8 bit integer
143
 */
145
 */
144
int Parser::charToData(int data) {
146
int Parser::charToData(int data) {
145
    if (data < 0)
147
    if (data < 0)
146
        return data + 256;
148
        return data + 256;
147
    return data;
149
    return data;
148
}
150
}
149
 
151
 
150
/**
152
/**
151
 * convert data to 16bit Integer
153
 * convert data to 16bit Integer
152
 */
154
 */
153
int Parser::dataToInt(char *Data , int Start, bool is_signed)
155
int Parser::dataToInt(char *Data , int Start, bool is_signed)
154
{
156
{
155
    int Out = (Data[Start+1]<<8) | (Data[Start+0]);
157
    int Out = (Data[Start+1]<<8) | (Data[Start+0]);
156
 
158
 
157
    if ((Out > 32767) && (is_signed))
159
    if ((Out > 32767) && (is_signed))
158
      Out = Out - 65536;
160
      Out = Out - 65536;
159
 
161
 
160
    return Out;
162
    return Out;
161
 
163
 
162
}
164
}
163
 
165
 
164
/**
166
/**
165
 * convert data to 32bit Long
167
 * convert data to 32bit Long
166
 */
168
 */
167
long Parser::dataToLong(char *Data , int Start, bool is_signed)
169
long Parser::dataToLong(char *Data , int Start, bool is_signed)
168
{
170
{
169
    long Out = (Data[Start+3]<<24) | (Data[Start+2]<<16) | (Data[Start+1]<<8) | (Data[Start+0]);
171
    long Out = (Data[Start+3]<<24) | (Data[Start+2]<<16) | (Data[Start+1]<<8) | (Data[Start+0]);
170
 
172
 
171
    if ((Out > 32767) && (is_signed))
173
    if ((Out > 32767) && (is_signed))
172
      Out = Out;
174
      Out = Out;
173
 
175
 
174
    return Out;
176
    return Out;
175
}
177
}
176
 
178
 
177
std::string Parser::dataToString(char * data, int start, int end)
179
std::string Parser::dataToString(char * data, int start, int end)
178
{
180
{
179
    char tmp[MAX_DATA_SIZE];
181
    char tmp[MAX_DATA_SIZE];
180
    for (int i = start; i < end; i++)
182
    for (int i = start; i < end; i++)
181
        tmp[i-start] = data[i];
183
        tmp[i-start] = data[i];
182
    return std::string(tmp);
184
    return std::string(tmp);
183
}
185
}
184
 
186
 
185
float Parser::getFloat(long value, int count)
187
float Parser::getFloat(long value, int count)
186
{
188
{
187
    long num = pow(10, count);
189
    long num = pow(10, count);
188
 
190
 
189
    float temp = value;
191
    float temp = value;
190
 
192
 
191
    return temp / num;
193
    return temp / num;
192
}
194
}
193
 
195
 
194
/**
196
/**
195
 * check CRC
197
 * check CRC
196
 */
198
 */
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++)
-
 
207
    {
206
    for(int i=0; i < length-2; i++)
208
            CRC+=rx[i];
-
 
209
    }
207
        CRC+=rx[i];
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))
-
 
219
    {
214
    if(rx[length - 1] != ('=' + CRC % 64))
220
        return false;
-
 
221
    }
215
        return false;
222
 
216
 
223
    return true;
217
    return true;
224
}
218
}
225
 
219
 
226
/**
220
/**
227
 * create CRC and add it to tx
221
 * create CRC and add it to tx
228
 */
222
 */
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
 
-
 
245
 
236