Subversion Repositories Projects

Rev

Rev 450 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
391 Brean 1
#include <Parser.h>
513 Brean 2
#include <iostream>
391 Brean 3
 
449 Brean 4
/**
5
 * create a frame that can be send to the MK using the
6
 * connection class.
7
 * see http://www.mikrokopter.com/ucwiki/en/SerialProtocol
8
 * how the protocol is encoded and
9
 * see http://www.mikrokopter.com/ucwiki/en/SerialCommands
10
 * to look at the possible commands that are already coded
11
 * in data
12
 */
513 Brean 13
void Parser::create_frame(char * send_data, char cmd, int address, char * data, unsigned int length) {
450 Brean 14
    //if # is cmd we do not touch anything, because
15
    // the command is already encoded in data
16
    if (cmd != '#') {
17
        /*
18
        //calculate buffer length
19
        //(4 Bytes for 1-byte '#', 1-byte address, and 2-byte crc)
20
        //(the rest for the data length - see encode how this is calculated)
21
        int buff_len = 4+(length/3 + (length%3==0?0:1) )*4;
449 Brean 22
 
450 Brean 23
        //allociate memory for the data we want to send
24
        char * send_data = (char *)malloc(buf_len);
25
        */
26
        Parser::encode64(data, length);
513 Brean 27
 
450 Brean 28
        send_data[0]='#';
29
        send_data[1]=(char)address+'a';
30
        send_data[2]=cmd;
31
        for (int i = 0; i < length; i++)
32
            send_data[i+3] = data[i];
33
        add_CRC(send_data, length+3);
34
        data = send_data;
35
    }
449 Brean 36
}
37
 
38
 
39
/**
40
 * Base64 Decoder
41
 * see Parser.h for details about sRxData
42
 * data = data that will be decoded
43
 * len = length of data
44
 * ptrOut = pointer to decoded data
45
 * offset = offset in data
46
 */
47
int Parser::decode64(char * data, int len, unsigned char *ptrOut, int offset)
391 Brean 48
{
49
    unsigned char a,b,c,d;
50
    unsigned char ptr = 0;
51
    unsigned char x,y,z;
52
 
53
    int decLen = 0;
513 Brean 54
 
55
    std::cout << "decode64" << offset << " " << len << std::endl;
56
    FlightLog::log_data(data, len);
57
    if (data[offset] == 0) {
58
        //return -1;
396 Brean 59
        //TODO: catch error to show that something went wrong during the decode process
60
        //throw "Nothing received";
513 Brean 61
        FlightLog::warning("incorrect data received");
391 Brean 62
    }
449 Brean 63
    //decode data
64
    while(len) {
65
        a = data[offset++] - '=';
66
        b = data[offset++] - '=';
67
        c = data[offset++] - '=';
68
        d = data[offset++] - '=';
391 Brean 69
 
449 Brean 70
        //if(offset > max - 2) break;
391 Brean 71
 
72
        x = (a << 2) | (b >> 4);
73
        y = ((b & 0x0f) << 4) | (c >> 2);
74
        z = ((c & 0x03) << 6) | d;
75
 
76
        if(len--) ptrOut[ptr++] = x; else break;
77
        if(len--) ptrOut[ptr++] = y; else break;
78
        if(len--) ptrOut[ptr++] = z; else break;
79
    }
449 Brean 80
    //decoded data
81
    unsigned char * decData;
391 Brean 82
    for (int a=0; a<ptr; a++) {
449 Brean 83
        if (len) {
84
            decData[decLen] = ptrOut[a];
85
            decLen++;
86
        } else {
391 Brean 87
            int b1, b2, b3;
88
 
89
            b1 = ptrOut[a++];
90
            b2 = ptrOut[a];
91
 
92
            b3 = (b2 << 8) | b1;
93
 
94
            if (b3 > 32767)
95
                b3 = b3 - 65536;
96
 
449 Brean 97
            decData[decLen] = b3;
396 Brean 98
            decLen++;
391 Brean 99
        }
100
    }
449 Brean 101
    ptrOut = decData;
102
    return decLen;
391 Brean 103
}
104
 
449 Brean 105
/**
106
 * base64 encoder
107
 */
108
void Parser::encode64(char data[150],unsigned int length)
391 Brean 109
{
110
    unsigned int pt = 0;
111
    unsigned char a,b,c;
112
    unsigned char ptr = 0;
113
 
114
    char tx_buff[150];
115
 
396 Brean 116
    while(length > 0)
391 Brean 117
    {
396 Brean 118
        if(length) { a = data[ptr++]; length--;} else a = 0;
119
        if(length) { b = data[ptr++]; length--;} else b = 0;
120
        if(length) { c = data[ptr++]; length--;} else c = 0;
391 Brean 121
 
122
        tx_buff[pt++] = '=' + (a >> 2);
123
        tx_buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
124
        tx_buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
125
        tx_buff[pt++] = '=' + ( c & 0x3f);
126
    }
127
    tx_buff[pt] = 0;
128
 
449 Brean 129
    //move pointer of tx_buff to data
130
    data = tx_buff;
391 Brean 131
}
132
 
133
// Datensatz nach 8bit Integer
450 Brean 134
int Parser::dataToChar(char *data , int start, bool is_signed) {
391 Brean 135
    int out = (data[start]);
136
 
137
    if ((out > 128) && (is_signed))
138
      out = out - 256;
139
 
140
    return out;
141
}
142
 
449 Brean 143
/**
144
 * received char to 8 bit integer
145
 */
391 Brean 146
int Parser::charToData(int data) {
147
    if (data < 0)
148
        return data + 256;
149
    return data;
150
}
151
 
449 Brean 152
/**
153
 * convert data to 16bit Integer
154
 */
450 Brean 155
int Parser::dataToInt(char *Data , int Start, bool is_signed)
391 Brean 156
{
157
    int Out = (Data[Start+1]<<8) | (Data[Start+0]);
158
 
159
    if ((Out > 32767) && (is_signed))
160
      Out = Out - 65536;
161
 
162
    return Out;
163
 
164
}
165
 
449 Brean 166
/**
167
 * convert data to 32bit Long
168
 */
450 Brean 169
long Parser::dataToLong(char *Data , int Start, bool is_signed)
391 Brean 170
{
171
    long Out = (Data[Start+3]<<24) | (Data[Start+2]<<16) | (Data[Start+1]<<8) | (Data[Start+0]);
172
 
173
    if ((Out > 32767) && (is_signed))
174
      Out = Out;
175
 
176
    return Out;
177
}
178
 
450 Brean 179
std::string Parser::dataToString(char * data, int start, int end)
180
{
181
    char tmp[MAX_DATA_SIZE];
182
    for (int i = start; i < end; i++)
183
        tmp[i-start] = data[i];
184
    return std::string(tmp);
185
}
186
 
391 Brean 187
float Parser::getFloat(long value, int count)
188
{
396 Brean 189
    long num = pow(10, count);
391 Brean 190
 
191
    float temp = value;
192
 
396 Brean 193
    return temp / num;
391 Brean 194
}
195
 
449 Brean 196
/**
197
 * check CRC
198
 */
199
bool Parser::check_CRC(char * rx, int length)
391 Brean 200
{
201
    int CRC = 0;
202
 
449 Brean 203
    if (rx[1] == 127)
204
        rx[1] = 0;
391 Brean 205
 
206
    for(int i=0; i < length-2; i++)
513 Brean 207
        CRC+=rx[i];
391 Brean 208
 
209
    CRC = CRC % 4096;
210
 
449 Brean 211
    if(rx[length - 2] != ('=' + (CRC / 64)))
391 Brean 212
        return false;
213
 
449 Brean 214
    if(rx[length - 1] != ('=' + CRC % 64))
391 Brean 215
        return false;
216
 
217
    return true;
218
}
219
 
449 Brean 220
/**
221
 * create CRC and add it to tx
222
 */
223
void Parser::add_CRC(char * tx, int length)
391 Brean 224
{
225
    unsigned int tmpCRC = 0;
226
 
227
    for(int i = 0; i < length; i++)
449 Brean 228
        tmpCRC += tx[i];
391 Brean 229
 
230
    tmpCRC %= 4096;
231
 
450 Brean 232
    tx[length] = '=' + tmpCRC / 64;
233
    tx[length+1] = '=' + tmpCRC % 64;
234
    tx[length+2] = '\0';
391 Brean 235
}