Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
#include "parse_packet.h"
// returns the length of the data in bytes (datalen from packet) and filles data and metadata with the packet information
// use as follows:
//
// packet is the entire packet message (formatted)
// data is an unallocated (char *) (pass it to this function as &data)
// meta_data is a pointer to an instance of metadata_t
//
int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data)
{
//----------------------------------------------------------------------------------------------
// index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
//---------------------------------------------------------------------------------------------|
// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
//-------------------------------------------------------------------------------------------- |
// bytes|| 1 | 1 | 1 | 2 | 2 | var | 1 |
//----------------------------------------------------------------------------------------------
// first byte must be the begin char
if(packet[0] != 0xBE)
return -1;
// receive metadata
meta_data->begin_char = packet[0];
meta_data->msg_type = packet[1];
meta_data->msg_subtype = packet[2];
meta_data->msg_id = (packet[4] << 8) | (packet[3]);
meta_data->data_len = (packet[6] << 8) | (packet[5]);
meta_data->checksum = packet[7+meta_data->data_len];
int i;
// receive data
*data = malloc(meta_data->data_len);
for(i = 0; i < meta_data->data_len; i++)
{
(*data)[i] = packet[7+i];
}
// calculate checksum
char data_checksum = 0;
for(i = 0; i < meta_data->data_len + 7; i++)
{
data_checksum ^= packet[i];
}
// compare checksum
if(meta_data->checksum != data_checksum)
fprintf(stderr, "Checksums did not match (Quadlog): 0x%02x\t0x%02x\n", meta_data->checksum, data_checksum);
return meta_data->data_len;
}
float getFloat(unsigned char* str, int pos) {
union {
float f;
int i;
} x;
x.i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
return x.f;
}
int getInt(unsigned char* str, int pos) {
int i = ((str[pos+3] << 24) | (str[pos+2] << 16) | (str[pos+1] << 8) | (str[pos]));
return i;
}