Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
740 | Nick666 | 1 | #include "main.h" |
2 | |||
3 | // ubx protocol parser state machine |
||
4 | #define UBXSTATE_IDLE 0 |
||
5 | #define UBXSTATE_SYNC1 1 |
||
6 | #define UBXSTATE_SYNC2 2 |
||
7 | #define UBXSTATE_CLASS 3 |
||
8 | #define UBXSTATE_LEN1 4 |
||
9 | #define UBXSTATE_LEN2 5 |
||
10 | #define UBXSTATE_DATA 6 |
||
11 | #define UBXSTATE_CKA 7 |
||
12 | #define UBXSTATE_CKB 8 |
||
13 | |||
14 | // ublox protocoll identifier |
||
15 | #define UBX_CLASS_NAV 0x01 |
||
16 | #define UBX_ID_POSLLH 0x02 |
||
17 | #define UBX_ID_POSUTM 0x08 |
||
18 | #define UBX_ID_STATUS 0x03 |
||
19 | #define UBX_ID_VELNED 0x12 |
||
20 | #define UBX_SYNC1_CHAR 0xB5 |
||
21 | #define UBX_SYNC2_CHAR 0x62 |
||
22 | |||
23 | typedef struct { |
||
24 | uint32_t ITOW; // ms GPS Millisecond Time of Week |
||
25 | uint8_t GPSfix; // GPSfix Type, range 0..6 |
||
26 | uint8_t Flags; // Navigation Status Flags |
||
27 | uint8_t DiffS; // Differential Status |
||
28 | uint8_t res; // reserved |
||
29 | uint32_t TTFF; // Time to first fix (millisecond time tag) |
||
30 | uint32_t MSSS; // Milliseconds since Startup / Reset |
||
31 | uint8_t Status; |
||
32 | } GPS_STATUS_t; |
||
33 | |||
34 | typedef struct { |
||
35 | uint32_t ITOW; // ms GPS Millisecond Time of Week |
||
36 | int32_t LON; // 1e-07 deg Longitude |
||
37 | int32_t LAT; // 1e-07 deg Latitude |
||
38 | int32_t HEIGHT; // mm Height above Ellipsoid |
||
39 | int32_t HMSL; // mm Height above mean sea level |
||
40 | uint32_t Hacc; // mm Horizontal Accuracy Estimate |
||
41 | uint32_t Vacc; // mm Vertical Accuracy Estimate |
||
42 | uint8_t Status; |
||
43 | } GPS_POSLLH_t; |
||
44 | |||
45 | typedef struct { |
||
46 | uint32_t ITOW; // ms GPS Millisecond Time of Week |
||
47 | int32_t EAST; // cm UTM Easting |
||
48 | int32_t NORTH; // cm UTM Nording |
||
49 | int32_t ALT; // cm altitude |
||
50 | uint8_t ZONE; // UTM zone number |
||
51 | uint8_t HEM; // Hemisphere Indicator (0=North, 1=South) |
||
52 | uint8_t Status; |
||
53 | } GPS_POSUTM_t; |
||
54 | |||
55 | typedef struct { |
||
56 | uint32_t ITOW; // ms GPS Millisecond Time of Week |
||
57 | int32_t VEL_N; // cm/s NED north velocity |
||
58 | int32_t VEL_E; // cm/s NED east velocity |
||
59 | int32_t VEL_D; // cm/s NED down velocity |
||
60 | uint32_t Speed; // cm/s Speed (3-D) |
||
61 | uint32_t GSpeed; // cm/s Ground Speed (2-D) |
||
62 | int32_t Heading; // 1e-05 deg Heading 2-D |
||
63 | uint32_t SAcc; // cm/s Speed Accuracy Estimate |
||
64 | uint32_t CAcc; // deg Course / Heading Accuracy Estimate |
||
65 | uint8_t Status; |
||
66 | } GPS_VELNED_t; |
||
67 | |||
68 | GPS_STATUS_t GpsStatus = {0,0,0,0,0,0,0, INVALID}; |
||
69 | GPS_POSLLH_t GpsPosLlh = {0,0,0,0,0,0,0, INVALID}; |
||
70 | GPS_POSUTM_t GpsPosUtm = {0,0,0,0,0,0, INVALID}; |
||
71 | GPS_VELNED_t GpsVelNed = {0,0,0,0,0,0,0,0,0, INVALID}; |
||
72 | GPS_INFO_t GPSInfo = {0,0,0,0,0,0,0,0,0,0, INVALID}; |
||
73 | |||
74 | volatile uint8_t GPSTimeout = 0; |
||
75 | |||
76 | void UpdateGPSInfo (void) |
||
77 | { |
||
78 | if (GpsStatus.Status == VALID) // valid packet |
||
79 | { |
||
80 | GPSInfo.satfix = GpsStatus.GPSfix; |
||
81 | GpsStatus.Status = PROCESSED; // never update old data |
||
82 | } |
||
83 | if (GpsPosLlh.Status == VALID) // valid packet |
||
84 | { |
||
85 | GPSInfo.longitude = GpsPosLlh.LON; |
||
86 | GPSInfo.latitude = GpsPosLlh.LAT; |
||
87 | GPSInfo.altitude = GpsPosLlh.HEIGHT; |
||
88 | GpsPosLlh.Status = PROCESSED; // never update old data |
||
89 | } |
||
90 | if (GpsPosUtm.Status == VALID) // valid packet |
||
91 | { |
||
92 | GPSInfo.utmeast = GpsPosUtm.EAST; |
||
93 | GPSInfo.utmnorth = GpsPosUtm.NORTH; |
||
94 | GPSInfo.utmalt = GpsPosUtm.ALT; |
||
95 | GpsPosUtm.Status = PROCESSED; // never update old data |
||
96 | } |
||
97 | if (GpsVelNed.Status == VALID) // valid packet |
||
98 | { |
||
99 | GPSInfo.veleast = GpsVelNed.VEL_E; |
||
100 | GPSInfo.velnorth = GpsVelNed.VEL_N; |
||
101 | GPSInfo.veltop = -GpsVelNed.VEL_D; |
||
102 | GpsVelNed.Status = PROCESSED; // never update old data |
||
103 | } |
||
104 | if (GpsStatus.Status != INVALID) |
||
105 | { |
||
106 | GPSInfo.status = VALID; // set valid if data are updated |
||
107 | } |
||
108 | } |
||
109 | |||
110 | |||
111 | // this function should be called within the UART RX ISR |
||
112 | void ubx_parser(uint8_t c) |
||
113 | { |
||
114 | static uint8_t ubxstate = UBXSTATE_IDLE; |
||
115 | static uint8_t cka, ckb; |
||
116 | static uint16_t msglen; |
||
117 | static int8_t *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered |
||
118 | |||
119 | switch(ubxstate) |
||
120 | { |
||
121 | case UBXSTATE_IDLE: // check 1st sync byte |
||
122 | if (c == UBX_SYNC1_CHAR) ubxstate = UBXSTATE_SYNC1; |
||
123 | else ubxstate = UBXSTATE_IDLE; // out of synchronization |
||
124 | break; |
||
125 | |||
126 | case UBXSTATE_SYNC1: // check 2nd sync byte |
||
127 | if (c == UBX_SYNC2_CHAR) ubxstate = UBXSTATE_SYNC2; |
||
128 | else ubxstate = UBXSTATE_IDLE; // out of synchronization |
||
129 | break; |
||
130 | |||
131 | case UBXSTATE_SYNC2: // check msg class to be NAV |
||
132 | if (c == UBX_CLASS_NAV) ubxstate = UBXSTATE_CLASS; |
||
133 | else ubxstate = UBXSTATE_IDLE; // unsupported message class |
||
134 | break; |
||
135 | |||
136 | case UBXSTATE_CLASS: // check message identifier |
||
137 | switch(c) |
||
138 | { |
||
139 | case UBX_ID_POSUTM: // utm position |
||
140 | ubxP = (int8_t *)&GpsPosUtm; // data start pointer |
||
141 | ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer |
||
142 | ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer |
||
143 | break; |
||
144 | |||
145 | case UBX_ID_POSLLH: // geodetic position |
||
146 | ubxP = (int8_t *)&GpsPosLlh; // data start pointer |
||
147 | ubxEp = (int8_t *)(&GpsPosLlh + sizeof(GPS_POSLLH_t)); // data end pointer |
||
148 | ubxSp = (int8_t *)&GpsPosLlh.Status; // status pointer |
||
149 | break; |
||
150 | |||
151 | case UBX_ID_STATUS: // receiver status |
||
152 | ubxP = (int8_t *)&GpsStatus; // data start pointer |
||
153 | ubxEp = (int8_t *)(&GpsStatus + sizeof(GPS_STATUS_t)); // data end pointer |
||
154 | ubxSp = (int8_t *)&GpsStatus.Status; // status pointer |
||
155 | break; |
||
156 | |||
157 | case UBX_ID_VELNED: // velocity vector in tangent plane |
||
158 | ubxP = (int8_t *)&GpsVelNed; // data start pointer |
||
159 | ubxEp = (int8_t *)(&GpsVelNed + sizeof(GPS_VELNED_t)); // data end pointer |
||
160 | ubxSp = (int8_t *)&GpsVelNed.Status; // status pointer |
||
161 | break; |
||
162 | |||
163 | default: // unsupported identifier |
||
164 | ubxstate = UBXSTATE_IDLE; |
||
165 | break; |
||
166 | } |
||
167 | if (ubxstate != UBXSTATE_IDLE) |
||
168 | { |
||
169 | ubxstate = UBXSTATE_LEN1; |
||
170 | cka = UBX_CLASS_NAV + c; |
||
171 | ckb = UBX_CLASS_NAV + cka; |
||
172 | } |
||
173 | break; |
||
174 | |||
175 | case UBXSTATE_LEN1: // 1st message length byte |
||
176 | msglen = c; |
||
177 | cka += c; |
||
178 | ckb += cka; |
||
179 | ubxstate = UBXSTATE_LEN2; |
||
180 | break; |
||
181 | |||
182 | case UBXSTATE_LEN2: // 2nd message length byte |
||
183 | msglen += ((uint16_t)c)<<8; |
||
184 | cka += c; |
||
185 | ckb += cka; |
||
186 | // if the old data are not processed so far then break parsing now |
||
187 | // to avoid writing new data in ISR during reading by another function |
||
188 | if ( *ubxSp == VALID ) ubxstate = UBXSTATE_IDLE; |
||
189 | else // data invalid or allready processd |
||
190 | { |
||
191 | *ubxSp = INVALID; |
||
192 | ubxstate = UBXSTATE_DATA; |
||
193 | } |
||
194 | break; |
||
195 | |||
196 | case UBXSTATE_DATA: |
||
197 | if (ubxP < ubxEp) *ubxP++ = c; // copy curent data byte if any space is left |
||
198 | cka += c; |
||
199 | ckb += cka; |
||
200 | if (--msglen == 0) ubxstate = UBXSTATE_CKA; // switch to next state if all data was read |
||
201 | break; |
||
202 | |||
203 | case UBXSTATE_CKA: |
||
204 | if (c == cka) ubxstate = UBXSTATE_CKB; |
||
205 | else |
||
206 | { |
||
207 | *ubxSp = INVALID; |
||
208 | ubxstate = UBXSTATE_IDLE; |
||
209 | } |
||
210 | break; |
||
211 | |||
212 | case UBXSTATE_CKB: |
||
213 | if (c == ckb) |
||
214 | { |
||
215 | *ubxSp = VALID; // new data are valid |
||
216 | ROT_FLASH; |
||
217 | UpdateGPSInfo(); //update GPS info respectively |
||
218 | GPSTimeout = 255; |
||
219 | } |
||
220 | else |
||
221 | { // if checksum not fit then set data invalid |
||
222 | *ubxSp = INVALID; |
||
223 | } |
||
224 | ubxstate = UBXSTATE_IDLE; // ready to parse new data |
||
225 | break; |
||
226 | |||
227 | default: // unknown ubx state |
||
228 | ubxstate = UBXSTATE_IDLE; |
||
229 | break; |
||
230 | } |
||
231 | |||
232 | } |
||
233 | |||
234 |