WebSVN - Projects - Diff - Rev 449 and 450 - /QMK-Groundstation/branches/libMK/libMK/Parser.cpp


#include <Parser.h>
 
/**
 * create a frame that can be send to the MK using the
 * connection class.
 * see http://www.mikrokopter.com/ucwiki/en/SerialProtocol
 * how the protocol is encoded and
 * see http://www.mikrokopter.com/ucwiki/en/SerialCommands
 * to look at the possible commands that are already coded
 * in data
 */
void Parser::create_frame(char cmd, int address, char * data, unsigned int length) {
    //if # is cmd we do not touch anything, because
    // the command is already encoded in data
    if (cmd != '#') {
        /*
        //calculate buffer length
        //(4 Bytes for 1-byte '#', 1-byte address, and 2-byte crc)
        //(the rest for the data length - see encode how this is calculated)
        int buff_len = 4+(length/3 + (length%3==0?0:1) )*4;
 
        //allociate memory for the data we want to send
        char * send_data = (char *)malloc(buf_len);
        */
        Parser::encode64(data, length);
        char send_data[150];
        send_data[0]='#';
        send_data[1]=(char)address+'a';
        send_data[2]=cmd;
        for (int i = 0; i < length; i++)
            send_data[i+3] = data[i];
 
        add_CRC(send_data, length+3);
        data = send_data;
 
    }
}
 
 
/**
 * Base64 Decoder
 * see Parser.h for details about sRxData
 * data = data that will be decoded
 * len = length of data
 * ptrOut = pointer to decoded data
 * offset = offset in data
 */
int Parser::decode64(char * data, int len, unsigned char *ptrOut, int offset)
{
    unsigned char a,b,c,d;
    unsigned char ptr = 0;
    unsigned char x,y,z;
 
    int decLen = 0;
/*
    //FIXME: dies wieder einklammern!
    if (data[ptrIn] == 0) {
        return -1;
        //TODO: catch error to show that something went wrong during the decode process
        //throw "Nothing received";
    }
*/
    //decode data
    while(len) {
        a = data[offset++] - '=';
        b = data[offset++] - '=';
        c = data[offset++] - '=';
        d = data[offset++] - '=';
 
        //if(offset > max - 2) break;
 
        x = (a << 2) | (b >> 4);
        y = ((b & 0x0f) << 4) | (c >> 2);
        z = ((c & 0x03) << 6) | d;
 
        if(len--) ptrOut[ptr++] = x; else break;
        if(len--) ptrOut[ptr++] = y; else break;
        if(len--) ptrOut[ptr++] = z; else break;
    }
    //decoded data
    unsigned char * decData;
    for (int a=0; a<ptr; a++) {
        if (len) {
            decData[decLen] = ptrOut[a];
            decLen++;
        } else {
            int b1, b2, b3;
 
            b1 = ptrOut[a++];
            b2 = ptrOut[a];
 
            b3 = (b2 << 8) | b1;
 
            if (b3 > 32767)
                b3 = b3 - 65536;
 
            decData[decLen] = b3;
            decLen++;
        }
    }
    ptrOut = decData;
    return decLen;
}
 
/**
 * base64 encoder
 */
void Parser::encode64(char data[150],unsigned int length)
{
    unsigned int pt = 0;
    unsigned char a,b,c;
    unsigned char ptr = 0;
 
    char tx_buff[150];
 
    while(length > 0)
    {
        if(length) { a = data[ptr++]; length--;} else a = 0;
        if(length) { b = data[ptr++]; length--;} else b = 0;
        if(length) { c = data[ptr++]; length--;} else c = 0;
 
        tx_buff[pt++] = '=' + (a >> 2);
        tx_buff[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
        tx_buff[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
        tx_buff[pt++] = '=' + ( c & 0x3f);
    }
    tx_buff[pt] = 0;
 
    //move pointer of tx_buff to data
    data = tx_buff;
}
 
// Datensatz nach 8bit Integer
int Parser::dataToChar(char *data , int start, bool is_signed) {
    int out = (data[start]);
 
    if ((out > 128) && (is_signed))
      out = out - 256;
 
    return out;
}
 
/**
 * received char to 8 bit integer
 */
int Parser::charToData(int data) {
    if (data < 0)
        return data + 256;
    return data;
}
 
/**
 * convert data to 16bit Integer
 */
int Parser::dataToInt(char *Data , int Start, bool is_signed)
{
    int Out = (Data[Start+1]<<8) | (Data[Start+0]);
 
    if ((Out > 32767) && (is_signed))
      Out = Out - 65536;
 
    return Out;
 
}
 
/**
 * convert data to 32bit Long
 */
long Parser::dataToLong(char *Data , int Start, bool is_signed)
{
    long Out = (Data[Start+3]<<24) | (Data[Start+2]<<16) | (Data[Start+1]<<8) | (Data[Start+0]);
 
    if ((Out > 32767) && (is_signed))
      Out = Out;
 
    return Out;
}
 
std::string Parser::dataToString(char * data, int start, int end)
{
    char tmp[MAX_DATA_SIZE];
    for (int i = start; i < end; i++)
        tmp[i-start] = data[i];
    return std::string(tmp);
}
 
float Parser::getFloat(long value, int count)
{
    long num = pow(10, count);
 
    float temp = value;
 
    return temp / num;
}
 
/**
 * check CRC
 */
bool Parser::check_CRC(char * rx, int length)
{
    int CRC = 0;
 
    if (rx[1] == 127)
    {
        rx[1] = 0;
    }
 
    for(int i=0; i < length-2; i++)
    {
            CRC+=rx[i];
    }
 
    CRC = CRC % 4096;
 
    if(rx[length - 2] != ('=' + (CRC / 64)))
    {
        return false;
    }
 
    if(rx[length - 1] != ('=' + CRC % 64))
    {
        return false;
    }
 
    return true;
}
 
/**
 * create CRC and add it to tx
 */
void Parser::add_CRC(char * tx, int length)
{
    unsigned int tmpCRC = 0;
 
    for(int i = 0; i < length; i++)
    {
        tmpCRC += tx[i];
    }
 
    tmpCRC %= 4096;
 
    tx[length] = '=' + tmpCRC / 64;
    tx[length+1] = '=' + tmpCRC % 64;
    tx[length+2] = '\0';
}
 
 

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

Subversion Repositories Projects

(root)/QMK-Groundstation/branches/libMK/libMK/Parser.cpp - Rev 449 → 450