-
Notifications
You must be signed in to change notification settings - Fork 4
/
UbloxParser.cpp
72 lines (60 loc) · 1.14 KB
/
UbloxParser.cpp
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
68
69
70
//#include "mbed.h"
#include "UbloxParser.hpp"
UbloxParser::UbloxParser() {
_rxIdx = 0;
_rxFirstChar = false;
_rxSecondChar = false;
// First two sync chars are always the same:
_rxBuf[0] = 0xb5;
_rxBuf[1] = 0x62;
}
int UbloxParser::rx_char(int c) {
if (!_rxFirstChar) {
if (c == 0xb5) {
_rxFirstChar = true;
}
} else if (!_rxSecondChar) {
if (c == 0x62) {
_rxSecondChar = true;
_rxIdx = 2;
} else {
_rxFirstChar = false;
}
} else {
_rxBuf[_rxIdx] = c;
switch(_rxIdx) {
case 2:
//_msgClass = c;
_rxIdx++;
break;
case 3:
//_msgId = c;
_rxIdx++;
break;
case 4:
_length_lower = c;
_rxIdx++;
break;
case 5:
//_lenght_upper = c;
_payload_length = _length_lower | (c << 8);
if (_payload_length > 300) { // TODO: what is the max payload length?
_payload_length = 300;
}
_total_length = _payload_length + 8;
_maxIdx = _payload_length + 8 - 1;
_rxIdx++;
break;
default:
if (_rxIdx < _maxIdx) {
_rxIdx++;
} else {
_rxFirstChar = false;
_rxSecondChar = false;
return _total_length;
}
break;
}
}
return 0; // no new data yet...
}