?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
[FLYWOOF411] add board documentation
[inav/snaewe.git] / src / main / io / gps_ublox.c
blobbae5e0be39541ee5bf0951d49f15e3de2e8e876a
1 /*
2 * This file is part of Cleanflight.
3 *
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
16 */
17
18 #include <stdbool.h>
19 #include <stdlib.h>
20 #include <stdint.h>
21 #include <ctype.h>
22 #include <string.h>
23 #include <math.h>
24
25 #include "platform.h"
26 #include "build/build_config.h"
27
28 #if defined(USE_GPS) && defined(USE_GPS_PROTO_UBLOX)
29
30 #include "build/debug.h"
31
32
33 #include "common/axis.h"
34 #include "common/typeconversion.h"
35 #include "common/gps_conversion.h"
36 #include "common/maths.h"
37 #include "common/utils.h"
38
39 #include "drivers/serial.h"
40 #include "drivers/time.h"
41
42 #include "fc/config.h"
43 #include "fc/runtime_config.h"
44
45 #include "io/serial.h"
46 #include "io/gps.h"
47 #include "io/gps_private.h"
48
49 #include "scheduler/protothreads.h"
50
51 #define GPS_CFG_CMD_TIMEOUT_MS 200
52 #define GPS_VERSION_RETRY_TIMES 2
53 #define MAX_UBLOX_PAYLOAD_SIZE 256
54 #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
55 #define UBLOX_SBAS_MESSAGE_LENGTH 16
56
57 #define UBX_DYNMODEL_PEDESTRIAN 3
58 #define UBX_DYNMODEL_AIR_1G 6
59 #define UBX_DYNMODEL_AIR_4G 8
60
61 #define UBX_FIXMODE_2D_ONLY 1
62 #define UBX_FIXMODE_3D_ONLY 2
63 #define UBX_FIXMODE_AUTO 3
64
65 #define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0)
66 #define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
67 #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
68
69 // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
70 static const uint32_t ubloxScanMode1[] = {
71 0x00000000, 0x00000851, 0x0004E004, 0x00020200, 0x00000180, 0x00000000,
72 };
73
74 static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
75 "$PUBX,41,1,0003,0001,115200,0*1E\r\n", // GPS_BAUDRATE_115200
76 "$PUBX,41,1,0003,0001,57600,0*2D\r\n", // GPS_BAUDRATE_57600
77 "$PUBX,41,1,0003,0001,38400,0*26\r\n", // GPS_BAUDRATE_38400
78 "$PUBX,41,1,0003,0001,19200,0*23\r\n", // GPS_BAUDRATE_19200
79 "$PUBX,41,1,0003,0001,9600,0*16\r\n" // GPS_BAUDRATE_9600
80 };
81
82 // payload types
83 typedef struct {
84 uint8_t mode;
85 uint8_t usage;
86 uint8_t maxSBAS;
87 uint8_t scanmode2;
88 uint32_t scanmode1;
89 } ubx_sbas;
90
91 typedef struct {
92 uint8_t class;
93 uint8_t id;
94 uint8_t rate;
95 } ubx_msg;
96
97 typedef struct {
98 uint16_t meas;
99 uint16_t nav;
100 uint16_t time;
101 } ubx_rate;
102
103 typedef union {
104 uint8_t bytes[60]; // sizeof Galileo config
105 ubx_sbas sbas;
106 ubx_msg msg;
107 ubx_rate rate;
108 } ubx_payload;
109
110 // UBX support
111 typedef struct {
112 uint8_t preamble1;
113 uint8_t preamble2;
114 uint8_t msg_class;
115 uint8_t msg_id;
116 uint16_t length;
117 } ubx_header;
118
119 typedef struct {
120 ubx_header header;
121 ubx_payload payload;
122 } __attribute__((packed)) ubx_message;
123
124 typedef struct {
125 char swVersion[30]; // Zero-terminated Software Version String
126 char hwVersion[10]; // Zero-terminated Hardware Version String
127 } ubx_mon_ver;
128
129 typedef struct {
130 uint32_t time; // GPS msToW
131 int32_t longitude;
132 int32_t latitude;
133 int32_t altitude_ellipsoid;
134 int32_t altitude_msl;
135 uint32_t horizontal_accuracy;
136 uint32_t vertical_accuracy;
137 } ubx_nav_posllh;
138
139 typedef struct {
140 uint32_t time; // GPS msToW
141 uint8_t fix_type;
142 uint8_t fix_status;
143 uint8_t differential_status;
144 uint8_t res;
145 uint32_t time_to_first_fix;
146 uint32_t uptime; // milliseconds
147 } ubx_nav_status;
148
149 typedef struct {
150 uint32_t time;
151 int32_t time_nsec;
152 int16_t week;
153 uint8_t fix_type;
154 uint8_t fix_status;
155 int32_t ecef_x;
156 int32_t ecef_y;
157 int32_t ecef_z;
158 uint32_t position_accuracy_3d;
159 int32_t ecef_x_velocity;
160 int32_t ecef_y_velocity;
161 int32_t ecef_z_velocity;
162 uint32_t speed_accuracy;
163 uint16_t position_DOP;
164 uint8_t res;
165 uint8_t satellites;
166 uint32_t res2;
167 } ubx_nav_solution;
168
169 typedef struct {
170 uint32_t time; // GPS msToW
171 int32_t ned_north;
172 int32_t ned_east;
173 int32_t ned_down;
174 uint32_t speed_3d;
175 uint32_t speed_2d;
176 int32_t heading_2d;
177 uint32_t speed_accuracy;
178 uint32_t heading_accuracy;
179 } ubx_nav_velned;
180
181 typedef struct {
182 uint8_t chn; // Channel number, 255 for SVx not assigned to channel
183 uint8_t svid; // Satellite ID
184 uint8_t flags; // Bitmask
185 uint8_t quality; // Bitfield
186 uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
187 uint8_t elev; // Elevation in integer degrees
188 int16_t azim; // Azimuth in integer degrees
189 int32_t prRes; // Pseudo range residual in centimetres
190 } ubx_nav_svinfo_channel;
191
192 typedef struct {
193 uint32_t time; // GPS Millisecond time of week
194 uint8_t numCh; // Number of channels
195 uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
196 uint16_t reserved2; // Reserved
197 ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
198 } ubx_nav_svinfo;
199
200 typedef struct {
201 uint32_t time; // GPS msToW
202 uint32_t tAcc;
203 int32_t nano;
204 uint16_t year;
205 uint8_t month;
206 uint8_t day;
207 uint8_t hour;
208 uint8_t min;
209 uint8_t sec;
210 uint8_t valid;
211 } ubx_nav_timeutc;
212
213 typedef struct {
214 uint32_t time; // GPS msToW
215 uint16_t year;
216 uint8_t month;
217 uint8_t day;
218 uint8_t hour;
219 uint8_t min;
220 uint8_t sec;
221 uint8_t valid;
222 uint32_t tAcc;
223 int32_t nano;
224 uint8_t fix_type;
225 uint8_t fix_status;
226 uint8_t reserved1;
227 uint8_t satellites;
228 int32_t longitude;
229 int32_t latitude;
230 int32_t altitude_ellipsoid;
231 int32_t altitude_msl;
232 uint32_t horizontal_accuracy;
233 uint32_t vertical_accuracy;
234 int32_t ned_north;
235 int32_t ned_east;
236 int32_t ned_down;
237 int32_t speed_2d;
238 int32_t heading_2d;
239 uint32_t speed_accuracy;
240 uint32_t heading_accuracy;
241 uint16_t position_DOP;
242 uint16_t reserved2;
243 uint16_t reserved3;
244 } ubx_nav_pvt;
245
246 typedef struct {
247 uint8_t class;
248 uint8_t msg;
249 } ubx_ack_ack;
250
251 enum {
252 PREAMBLE1 = 0xB5,
253 PREAMBLE2 = 0x62,
254 CLASS_NAV = 0x01,
255 CLASS_ACK = 0x05,
256 CLASS_CFG = 0x06,
257 CLASS_MON = 0x0A,
258 MSG_CLASS_UBX = 0x01,
259 MSG_CLASS_NMEA = 0xF0,
260 MSG_VER = 0x04,
261 MSG_ACK_NACK = 0x00,
262 MSG_ACK_ACK = 0x01,
263 MSG_NMEA_GGA = 0x0,
264 MSG_NMEA_GLL = 0x1,
265 MSG_NMEA_GSA = 0x2,
266 MSG_NMEA_GSV = 0x3,
267 MSG_NMEA_RMC = 0x4,
268 MSG_NMEA_VGS = 0x5,
269 MSG_POSLLH = 0x2,
270 MSG_STATUS = 0x3,
271 MSG_SOL = 0x6,
272 MSG_PVT = 0x7,
273 MSG_VELNED = 0x12,
274 MSG_TIMEUTC = 0x21,
275 MSG_SVINFO = 0x30,
276 MSG_CFG_PRT = 0x00,
277 MSG_CFG_RATE = 0x08,
278 MSG_CFG_SET_RATE = 0x01,
279 MSG_CFG_NAV_SETTINGS = 0x24,
280 MSG_CFG_SBAS = 0x16,
281 MSG_CFG_GNSS = 0x3e
282 } ubx_protocol_bytes;
283
284 enum {
285 FIX_NONE = 0,
286 FIX_DEAD_RECKONING = 1,
287 FIX_2D = 2,
288 FIX_3D = 3,
289 FIX_GPS_DEAD_RECKONING = 4,
290 FIX_TIME = 5
291 } ubs_nav_fix_type;
292
293 enum {
294 NAV_STATUS_FIX_VALID = 1
295 } ubx_nav_status_bits;
296
297 enum {
298 UBX_ACK_WAITING = 0,
299 UBX_ACK_GOT_ACK = 1,
300 UBX_ACK_GOT_NAK = 2
301 } ubx_ack_state;
302
303 // Packet checksum accumulators
304 static uint8_t _ck_a;
305 static uint8_t _ck_b;
306
307 // State machine state
308 static bool _skip_packet;
309 static uint8_t _step;
310 static uint8_t _msg_id;
311 static uint16_t _payload_length;
312 static uint16_t _payload_counter;
313
314 static uint8_t next_fix_type;
315 static uint8_t _class;
316 static uint8_t _ack_state;
317 static uint8_t _ack_waiting_msg;
318
319 // do we have new position information?
320 static bool _new_position;
321
322 // do we have new speed information?
323 static bool _new_speed;
324
325 // Need this to determine if Galileo capable only
326 static bool capGalileo;
327
328 // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
329 //15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
330 //15:17:55 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position'
331 //15:17:55 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84'
332 //15:17:55 R -> UBX NAV-CLOCK, Size 28, 'Clock Status'
333 //15:17:55 R -> UBX NAV-AOPSTATUS, Size 24, 'AOP Status'
334 //15:17:55 R -> UBX 03-09, Size 208, 'Unknown'
335 //15:17:55 R -> UBX 03-10, Size 336, 'Unknown'
336 //15:17:55 R -> UBX NAV-SOL, Size 60, 'Navigation Solution'
337 //15:17:55 R -> UBX NAV, Size 100, 'Navigation'
338 //15:17:55 R -> UBX NAV-SVINFO, Size 328, 'Satellite Status and Information'
339
340
341 // Send buffer
342 static union {
343 ubx_message message;
344 uint8_t bytes[58];
345 } send_buffer;
346
347 // Receive buffer
348 static union {
349 ubx_nav_posllh posllh;
350 ubx_nav_status status;
351 ubx_nav_solution solution;
352 ubx_nav_velned velned;
353 ubx_nav_pvt pvt;
354 ubx_nav_svinfo svinfo;
355 ubx_mon_ver ver;
356 ubx_nav_timeutc timeutc;
357 ubx_ack_ack ack;
358 uint8_t bytes[UBLOX_BUFFER_SIZE];
359 } _buffer;
360
361 void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
362 {
363 while (len--) {
364 *ck_a += *data;
365 *ck_b += *ck_a;
366 data++;
367 }
368 }
369
370 static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType)
371 {
372 if (fixValid && ubloxFixType == FIX_2D)
373 return GPS_FIX_2D;
374 if (fixValid && ubloxFixType == FIX_3D)
375 return GPS_FIX_3D;
376 return GPS_NO_FIX;
377 }
378
379 static void sendConfigMessageUBLOX(void)
380 {
381 uint8_t ck_a=0, ck_b=0;
382 send_buffer.message.header.preamble1=PREAMBLE1;
383 send_buffer.message.header.preamble2=PREAMBLE2;
384 _update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b);
385 send_buffer.bytes[send_buffer.message.header.length+6] = ck_a;
386 send_buffer.bytes[send_buffer.message.header.length+7] = ck_b;
387 serialWriteBuf(gpsState.gpsPort, send_buffer.bytes, send_buffer.message.header.length+8);
388
389 // Save state for ACK waiting
390 _ack_waiting_msg = send_buffer.message.header.msg_id;
391 _ack_state = UBX_ACK_WAITING;
392 }
393
394 static void pollVersion(void)
395 {
396 send_buffer.message.header.msg_class = CLASS_MON;
397 send_buffer.message.header.msg_id = MSG_VER;
398 send_buffer.message.header.length = 0;
399 sendConfigMessageUBLOX();
400 }
401
402 static const uint8_t default_payload[] = {
403 0xFF, 0xFF, 0x03, 0x03, 0x00, // CFG-NAV5 - Set engine settings (original MWII code)
404 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and
405 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // capturing the data from the U-Center binary console.
406 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
407 };
408
409 // Note the organisation of the bytes reflects the structure of the payload
410 // 4 bytes then 8*number of elements (7)
411 static const uint8_t galileo_payload[] = {
412 0x00, 0x00, 0x20, 0x07, // GNSS / min / max / enable
413 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS / 8 / 16 / Y
414 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS / 1 / 3 / Y
415 0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo / 4 / 8 / Y
416 0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou / 8 / 16 / N
417 0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01, // IMES / 0 / 8 / N
418 0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01, // QZSS / 0 / 3 / N
419 0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01 // GLONASS / 8 / 14 / Y
420 };
421
422 static void configureGalileo(void)
423 {
424 send_buffer.message.header.msg_class = CLASS_CFG;
425 send_buffer.message.header.msg_id = MSG_CFG_GNSS;
426 send_buffer.message.header.length = sizeof(galileo_payload);
427 memcpy(send_buffer.message.payload.bytes, galileo_payload, sizeof(galileo_payload));
428 sendConfigMessageUBLOX();
429 }
430
431 static void configureNAV5(uint8_t dynModel, uint8_t fixMode)
432 {
433 send_buffer.message.header.msg_class = CLASS_CFG;
434 send_buffer.message.header.msg_id = MSG_CFG_NAV_SETTINGS;
435 send_buffer.message.header.length = 0x24;
436 memcpy(send_buffer.message.payload.bytes, default_payload, sizeof(default_payload));
437 send_buffer.message.payload.bytes[2] = dynModel;
438 send_buffer.message.payload.bytes[3] = fixMode;
439 sendConfigMessageUBLOX();
440 }
441
442 static void configureMSG(uint8_t class, uint8_t id, uint8_t rate)
443 {
444 send_buffer.message.header.msg_class = CLASS_CFG;
445 send_buffer.message.header.msg_id = MSG_CFG_SET_RATE;
446 send_buffer.message.header.length = 3;
447 send_buffer.message.payload.msg.class = class;
448 send_buffer.message.payload.msg.id = id;
449 send_buffer.message.payload.msg.rate = rate;
450 sendConfigMessageUBLOX();
451 }
452
453 /*
454 * measRate in ms
455 * navRate cycles
456 * timeRef 0 UTC, 1 GPS
457 */
458 static void configureRATE(uint16_t measRate)
459 {
460 send_buffer.message.header.msg_class = CLASS_CFG;
461 send_buffer.message.header.msg_id = MSG_CFG_RATE;
462 send_buffer.message.header.length = 6;
463 send_buffer.message.payload.rate.meas=measRate;
464 send_buffer.message.payload.rate.nav=1;
465 send_buffer.message.payload.rate.time=1;
466 sendConfigMessageUBLOX();
467 }
468
469 /*
470 */
471 static void configureSBAS(void)
472 {
473 send_buffer.message.header.msg_class = CLASS_CFG;
474 send_buffer.message.header.msg_id = MSG_CFG_SBAS;
475 send_buffer.message.header.length = 8;
476 send_buffer.message.payload.sbas.mode=(gpsState.gpsConfig->sbasMode == SBAS_NONE?2:3);
477 send_buffer.message.payload.sbas.usage=3;
478 send_buffer.message.payload.sbas.maxSBAS=3;
479 send_buffer.message.payload.sbas.scanmode2=0;
480 send_buffer.message.payload.sbas.scanmode1=ubloxScanMode1[gpsState.gpsConfig->sbasMode];
481 sendConfigMessageUBLOX();
482 }
483
484 static bool gpsParceFrameUBLOX(void)
485 {
486 switch (_msg_id) {
487 case MSG_POSLLH:
488 //i2c_dataset.time = _buffer.posllh.time;
489 gpsSol.llh.lon = _buffer.posllh.longitude;
490 gpsSol.llh.lat = _buffer.posllh.latitude;
491 gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm
492 gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
493 gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
494 gpsSol.flags.validEPE = 1;
495 if (next_fix_type != GPS_NO_FIX)
496 gpsSol.fixType = next_fix_type;
497 _new_position = true;
498 break;
499 case MSG_STATUS:
500 next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type);
501 if (next_fix_type == GPS_NO_FIX)
502 gpsSol.fixType = GPS_NO_FIX;
503 break;
504 case MSG_SOL:
505 next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type);
506 if (next_fix_type == GPS_NO_FIX)
507 gpsSol.fixType = GPS_NO_FIX;
508 gpsSol.numSat = _buffer.solution.satellites;
509 gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
510 break;
511 case MSG_VELNED:
512 gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
513 gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
514 gpsSol.velNED[0] = _buffer.velned.ned_north;
515 gpsSol.velNED[1] = _buffer.velned.ned_east;
516 gpsSol.velNED[2] = _buffer.velned.ned_down;
517 gpsSol.flags.validVelNE = 1;
518 gpsSol.flags.validVelD = 1;
519 _new_speed = true;
520 break;
521 case MSG_TIMEUTC:
522 if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) {
523 gpsSol.time.year = _buffer.timeutc.year;
524 gpsSol.time.month = _buffer.timeutc.month;
525 gpsSol.time.day = _buffer.timeutc.day;
526 gpsSol.time.hours = _buffer.timeutc.hour;
527 gpsSol.time.minutes = _buffer.timeutc.min;
528 gpsSol.time.seconds = _buffer.timeutc.sec;
529 gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000);
530
531 gpsSol.flags.validTime = 1;
532 } else {
533 gpsSol.flags.validTime = 0;
534 }
535 break;
536 case MSG_PVT:
537 next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type);
538 gpsSol.fixType = next_fix_type;
539 gpsSol.llh.lon = _buffer.pvt.longitude;
540 gpsSol.llh.lat = _buffer.pvt.latitude;
541 gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm
542 gpsSol.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s
543 gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s
544 gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s
545 gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s
546 gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
547 gpsSol.numSat = _buffer.pvt.satellites;
548 gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
549 gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
550 gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
551 gpsSol.flags.validVelNE = 1;
552 gpsSol.flags.validVelD = 1;
553 gpsSol.flags.validEPE = 1;
554
555 if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) {
556 gpsSol.time.year = _buffer.pvt.year;
557 gpsSol.time.month = _buffer.pvt.month;
558 gpsSol.time.day = _buffer.pvt.day;
559 gpsSol.time.hours = _buffer.pvt.hour;
560 gpsSol.time.minutes = _buffer.pvt.min;
561 gpsSol.time.seconds = _buffer.pvt.sec;
562 gpsSol.time.millis = _buffer.pvt.nano / (1000*1000);
563
564 gpsSol.flags.validTime = 1;
565 } else {
566 gpsSol.flags.validTime = 0;
567 }
568
569 _new_position = true;
570 _new_speed = true;
571 break;
572 case MSG_VER:
573 if (_class == CLASS_MON) {
574 //uint32_t swver = _buffer.ver.swVersion;
575 // EXT CORE 3.01 (107900)
576 // 01234567890123456789012
577 gpsState.hwVersion = fastA2I(_buffer.ver.hwVersion);
578 capGalileo = ((gpsState.hwVersion >= 80000) && (_buffer.ver.swVersion[9] > '2')); // M8N and SW major 3 or later
579 }
580 break;
581 case MSG_ACK_ACK:
582 if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) {
583 _ack_state = UBX_ACK_GOT_ACK;
584 }
585 break;
586 case MSG_ACK_NACK:
587 if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) {
588 _ack_state = UBX_ACK_GOT_NAK;
589 }
590 break;
591 default:
592 return false;
593 }
594
595 // we only return true when we get new position and speed data
596 // this ensures we don't use stale data
597 if (_new_position && _new_speed) {
598 _new_speed = _new_position = false;
599 return true;
600 }
601
602 return false;
603 }
604
605 static bool gpsNewFrameUBLOX(uint8_t data)
606 {
607 bool parsed = false;
608
609 switch (_step) {
610 case 0: // Sync char 1 (0xB5)
611 if (PREAMBLE1 == data) {
612 _skip_packet = false;
613 _step++;
614 }
615 break;
616 case 1: // Sync char 2 (0x62)
617 if (PREAMBLE2 != data) {
618 _step = 0;
619 break;
620 }
621 _step++;
622 break;
623 case 2: // Class
624 _step++;
625 _class = data;
626 _ck_b = _ck_a = data; // reset the checksum accumulators
627 break;
628 case 3: // Id
629 _step++;
630 _ck_b += (_ck_a += data); // checksum byte
631 _msg_id = data;
632 break;
633 case 4: // Payload length (part 1)
634 _step++;
635 _ck_b += (_ck_a += data); // checksum byte
636 _payload_length = data; // payload length low byte
637 break;
638 case 5: // Payload length (part 2)
639 _step++;
640 _ck_b += (_ck_a += data); // checksum byte
641 _payload_length |= (uint16_t)(data << 8);
642 if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE ) {
643 // we can't receive the whole packet, just log the error and start searching for the next packet.
644 gpsStats.errors++;
645 _step = 0;
646 break;
647 }
648 // prepare to receive payload
649 _payload_counter = 0;
650 if (_payload_length == 0) {
651 _step = 7;
652 }
653 break;
654 case 6:
655 _ck_b += (_ck_a += data); // checksum byte
656 if (_payload_counter < MAX_UBLOX_PAYLOAD_SIZE) {
657 _buffer.bytes[_payload_counter] = data;
658 }
659 // NOTE: check counter BEFORE increasing so that a payload_size of 65535 is correctly handled. This can happen if garbage data is received.
660 if (_payload_counter == _payload_length - 1) {
661 _step++;
662 }
663 _payload_counter++;
664 break;
665 case 7:
666 _step++;
667 if (_ck_a != data) {
668 _skip_packet = true; // bad checksum
669 gpsStats.errors++;
670 _step = 0;
671 }
672 break;
673 case 8:
674 _step = 0;
675
676 if (_ck_b != data) {
677 gpsStats.errors++;
678 break; // bad checksum
679 }
680
681 gpsStats.packetCount++;
682
683 if (_skip_packet) {
684 break;
685 }
686
687 if (gpsParceFrameUBLOX()) {
688 parsed = true;
689 }
690 }
691
692 return parsed;
693 }
694
695 STATIC_PROTOTHREAD(gpsConfigure)
696 {
697 ptBegin(gpsConfigure);
698
699 // Reset timeout
700 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
701
702 // Set dynamic model
703 switch (gpsState.gpsConfig->dynModel) {
704 case GPS_DYNMODEL_PEDESTRIAN:
705 configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
706 break;
707 case GPS_DYNMODEL_AIR_1G: // Default to this
708 default:
709 configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO);
710 break;
711 case GPS_DYNMODEL_AIR_4G:
712 configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO);
713 break;
714 }
715 ptWait(_ack_state == UBX_ACK_GOT_ACK);
716
717 // Disable NMEA messages
718 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
719
720 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GGA, 0);
721 ptWait(_ack_state == UBX_ACK_GOT_ACK);
722
723 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GLL, 0);
724 ptWait(_ack_state == UBX_ACK_GOT_ACK);
725
726 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSA, 0);
727 ptWait(_ack_state == UBX_ACK_GOT_ACK);
728
729 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSV, 0);
730 ptWait(_ack_state == UBX_ACK_GOT_ACK);
731
732 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_RMC, 0);
733 ptWait(_ack_state == UBX_ACK_GOT_ACK);
734
735 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_VGS, 0);
736 ptWait(_ack_state == UBX_ACK_GOT_ACK);
737
738 // Configure UBX binary messages
739 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
740
741 if ((gpsState.gpsConfig->provider == GPS_UBLOX) || (gpsState.hwVersion < 70000)) {
742 configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1);
743 ptWait(_ack_state == UBX_ACK_GOT_ACK);
744
745 configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1);
746 ptWait(_ack_state == UBX_ACK_GOT_ACK);
747
748 configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
749 ptWait(_ack_state == UBX_ACK_GOT_ACK);
750
751 configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1);
752 ptWait(_ack_state == UBX_ACK_GOT_ACK);
753
754 configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10);
755 ptWait(_ack_state == UBX_ACK_GOT_ACK);
756
757 // This may fail on old UBLOX units, advance forward on both ACK and NAK
758 configureMSG(MSG_CLASS_UBX, MSG_PVT, 0);
759 ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
760 }
761 else {
762 configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
763 ptWait(_ack_state == UBX_ACK_GOT_ACK);
764
765 configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
766 ptWait(_ack_state == UBX_ACK_GOT_ACK);
767
768 configureMSG(MSG_CLASS_UBX, MSG_SOL, 0);
769 ptWait(_ack_state == UBX_ACK_GOT_ACK);
770
771 configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
772 ptWait(_ack_state == UBX_ACK_GOT_ACK);
773
774 configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0);
775 ptWait(_ack_state == UBX_ACK_GOT_ACK);
776
777 configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
778 ptWait(_ack_state == UBX_ACK_GOT_ACK);
779 }
780
781 configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
782 ptWait(_ack_state == UBX_ACK_GOT_ACK);
783
784 // Configure data rate
785 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
786 if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= 70000)) {
787 configureRATE(100); // 10Hz
788 }
789 else {
790 configureRATE(200); // 5Hz
791 }
792 ptWait(_ack_state == UBX_ACK_GOT_ACK);
793
794 // Configure SBAS
795 // If particular SBAS setting is not supported by the hardware we'll get a NAK,
796 // however GPS would be functional. We are waiting for any response - ACK/NACK
797 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
798 configureSBAS();
799 ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
800
801 // Enable GALILEO
802 if (gpsState.gpsConfig->ubloxUseGalileo && capGalileo) {
803 // If GALILEO is not supported by the hardware we'll get a NAK,
804 // however GPS would otherwise be perfectly initialized, so we are just waiting for any response
805 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
806 configureGalileo();
807 ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
808 }
809
810 ptEnd(0);
811 }
812
813 static ptSemaphore_t semNewDataReady;
814
815 STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
816 {
817 ptBegin(gpsProtocolReceiverThread);
818
819 while (1) {
820 // Wait until there are bytes to consume
821 ptWait(serialRxBytesWaiting(gpsState.gpsPort));
822
823 // Consume bytes until buffer empty of until we have full message received
824 while (serialRxBytesWaiting(gpsState.gpsPort)) {
825 uint8_t newChar = serialRead(gpsState.gpsPort);
826 if (gpsNewFrameUBLOX(newChar)) {
827 ptSemaphoreSignal(semNewDataReady);
828 break;
829 }
830 }
831 }
832
833 ptEnd(0);
834 }
835
836 STATIC_PROTOTHREAD(gpsProtocolStateThread)
837 {
838 ptBegin(gpsProtocolStateThread);
839
840 // Change baud rate
841 if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) {
842 #if 0
843 // Autobaud logic:
844 // 0. Wait for TX buffer to be empty
845 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
846
847 // 1. Set serial port to baud rate specified by [autoBaudrateIndex]
848 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);
849 gpsState.autoBaudrateIndex = (gpsState.autoBaudrateIndex + 1) % GPS_BAUDRATE_COUNT;
850
851 // 2. Send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex]
852 serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]);
853
854 // 3. Wait for command to be received and processed by GPS
855 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
856
857 // 4. Switch to [baudrateIndex]
858 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
859
860 // 5. Attempt to configure the GPS
861 ptDelayMs(GPS_BAUD_CHANGE_DELAY);
862 #else
863 // 0. Wait for TX buffer to be empty
864 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
865
866 // Try sending baud rate switch command at all common baud rates
867 gpsSetProtocolTimeout((GPS_BAUD_CHANGE_DELAY + 50) * (GPS_BAUDRATE_COUNT));
868 for (gpsState.autoBaudrateIndex = 0; gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT; gpsState.autoBaudrateIndex++) {
869 // 2. Set serial port to baud rate and send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex]
870 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);
871 serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]);
872
873 // 3. Wait for serial port to finish transmitting
874 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
875
876 // 4. Extra wait to make sure GPS processed the command
877 ptDelayMs(GPS_BAUD_CHANGE_DELAY);
878 }
879
880 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
881 #endif
882 }
883 else {
884 // No auto baud - set port baud rate to [baudrateIndex]
885 // Wait for TX buffer to be empty
886 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
887
888 // Set baud rate and reset GPS timeout
889 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
890 }
891
892 // Configure GPS module if enabled
893 if (gpsState.gpsConfig->autoConfig) {
894 // Reset protocol timeout
895 gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS)));
896
897 // Attempt to detect GPS hw version
898 gpsState.hwVersion = 0;
899 gpsState.autoConfigStep = 0;
900
901 do {
902 pollVersion();
903 gpsState.autoConfigStep++;
904 ptWaitTimeout((gpsState.hwVersion != 0), GPS_CFG_CMD_TIMEOUT_MS);
905 } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == 0);
906
907 // Configure GPS
908 ptSpawn(gpsConfigure);
909 }
910
911 // GPS setup done, reset timeout
912 gpsSetProtocolTimeout(GPS_TIMEOUT);
913
914 // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
915 while (1) {
916 ptSemaphoreWait(semNewDataReady);
917 gpsProcessNewSolutionData();
918 }
919
920 ptEnd(0);
921 }
922
923 void gpsRestartUBLOX(void)
924 {
925 ptSemaphoreInit(semNewDataReady);
926 ptRestart(ptGetHandle(gpsProtocolReceiverThread));
927 ptRestart(ptGetHandle(gpsProtocolStateThread));
928 }
929
930 void gpsHandleUBLOX(void)
931 {
932 // Run the protocol threads
933 gpsProtocolReceiverThread();
934 gpsProtocolStateThread();
935
936 // If thread stopped - signal communication loss and restart
937 if (ptIsStopped(ptGetHandle(gpsProtocolReceiverThread)) || ptIsStopped(ptGetHandle(gpsProtocolStateThread))) {
938 gpsSetState(GPS_LOST_COMMUNICATION);
939 }
940 }
941
942 #endif