Fix WS2812 led definition
[inav.git] / src / main / io / gps_ublox.c
blob330cb2b75fc4bc11eaf3fc7aa0633e4d40770dff
1 /*
2 * This file is part of Cleanflight.
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.
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.
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/>.
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 #include <stdarg.h>
26 #include "platform.h"
27 #include "build/build_config.h"
29 #if defined(USE_GPS) && defined(USE_GPS_PROTO_UBLOX)
31 #include "build/debug.h"
34 #include "common/axis.h"
35 #include "common/typeconversion.h"
36 #include "common/gps_conversion.h"
37 #include "common/maths.h"
38 #include "common/utils.h"
40 #include "drivers/serial.h"
41 #include "drivers/time.h"
43 #include "fc/config.h"
44 #include "fc/runtime_config.h"
46 #include "io/serial.h"
47 #include "io/gps.h"
48 #include "io/gps_private.h"
50 #include "scheduler/protothreads.h"
52 #define GPS_CFG_CMD_TIMEOUT_MS 200
53 #define GPS_VERSION_RETRY_TIMES 2
54 #define MAX_UBLOX_PAYLOAD_SIZE 256
55 #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
56 #define UBLOX_SBAS_MESSAGE_LENGTH 16
58 #define UBX_DYNMODEL_PEDESTRIAN 3
59 #define UBX_DYNMODEL_AIR_1G 6
60 #define UBX_DYNMODEL_AIR_4G 8
62 #define UBX_FIXMODE_2D_ONLY 1
63 #define UBX_FIXMODE_3D_ONLY 2
64 #define UBX_FIXMODE_AUTO 3
66 #define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0)
67 #define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
68 #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
70 #define UBX_HW_VERSION_UNKNOWN 0
71 #define UBX_HW_VERSION_UBLOX5 500
72 #define UBX_HW_VERSION_UBLOX6 600
73 #define UBX_HW_VERSION_UBLOX7 700
74 #define UBX_HW_VERSION_UBLOX8 800
75 #define UBX_HW_VERSION_UBLOX9 900
76 #define UBX_HW_VERSION_UBLOX10 1000
78 // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
79 // note PRNs last upadted 2020-12-18
81 #define SBASMASK1_BASE 120
82 #define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE))
84 static const uint32_t ubloxScanMode1[] = {
85 0x00000000, // AUTO
86 (SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136)), // SBAS
87 (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(138)), // WAAS
88 (SBASMASK1_BITS(129) | SBASMASK1_BITS(137)), // MAAS
89 (SBASMASK1_BITS(127) | SBASMASK1_BITS(128)), // GAGAN
90 0x00000000, // NONE
93 static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
94 "$PUBX,41,1,0003,0001,115200,0*1E\r\n", // GPS_BAUDRATE_115200
95 "$PUBX,41,1,0003,0001,57600,0*2D\r\n", // GPS_BAUDRATE_57600
96 "$PUBX,41,1,0003,0001,38400,0*26\r\n", // GPS_BAUDRATE_38400
97 "$PUBX,41,1,0003,0001,19200,0*23\r\n", // GPS_BAUDRATE_19200
98 "$PUBX,41,1,0003,0001,9600,0*16\r\n", // GPS_BAUDRATE_9600
99 "$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400
102 // payload types
103 typedef struct {
104 uint8_t mode;
105 uint8_t usage;
106 uint8_t maxSBAS;
107 uint8_t scanmode2;
108 uint32_t scanmode1;
109 } ubx_sbas;
111 typedef struct {
112 uint8_t class;
113 uint8_t id;
114 uint8_t rate;
115 } ubx_msg;
117 typedef struct {
118 uint16_t meas;
119 uint16_t nav;
120 uint16_t time;
121 } ubx_rate;
123 typedef struct {
124 uint8_t gnssId;
125 uint8_t resTrkCh;
126 uint8_t maxTrkCh;
127 uint8_t reserved1;
128 // flags
129 uint8_t enabled;
130 uint8_t undefined0;
131 uint8_t sigCfgMask;
132 uint8_t undefined1;
133 } ubx_gnss_element_t;
135 typedef struct {
136 uint8_t msgVer;
137 uint8_t numTrkChHw;
138 uint8_t numTrkChUse;
139 uint8_t numConfigBlocks;
140 ubx_gnss_element_t config[0];
141 } ubx_gnss_msg_t;
143 #define MAX_GNSS 7
144 #define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
146 typedef union {
147 uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder
148 ubx_sbas sbas;
149 ubx_msg msg;
150 ubx_rate rate;
151 ubx_gnss_msg_t gnss;
152 } ubx_payload;
154 // UBX support
155 typedef struct {
156 uint8_t preamble1;
157 uint8_t preamble2;
158 uint8_t msg_class;
159 uint8_t msg_id;
160 uint16_t length;
161 } ubx_header;
163 typedef struct {
164 ubx_header header;
165 ubx_payload payload;
166 } __attribute__((packed)) ubx_message;
168 typedef struct {
169 char swVersion[30]; // Zero-terminated Software Version String
170 char hwVersion[10]; // Zero-terminated Hardware Version String
171 } ubx_mon_ver;
173 typedef struct {
174 uint32_t time; // GPS msToW
175 int32_t longitude;
176 int32_t latitude;
177 int32_t altitude_ellipsoid;
178 int32_t altitude_msl;
179 uint32_t horizontal_accuracy;
180 uint32_t vertical_accuracy;
181 } ubx_nav_posllh;
183 typedef struct {
184 uint32_t time; // GPS msToW
185 uint8_t fix_type;
186 uint8_t fix_status;
187 uint8_t differential_status;
188 uint8_t res;
189 uint32_t time_to_first_fix;
190 uint32_t uptime; // milliseconds
191 } ubx_nav_status;
193 typedef struct {
194 uint32_t time;
195 int32_t time_nsec;
196 int16_t week;
197 uint8_t fix_type;
198 uint8_t fix_status;
199 int32_t ecef_x;
200 int32_t ecef_y;
201 int32_t ecef_z;
202 uint32_t position_accuracy_3d;
203 int32_t ecef_x_velocity;
204 int32_t ecef_y_velocity;
205 int32_t ecef_z_velocity;
206 uint32_t speed_accuracy;
207 uint16_t position_DOP;
208 uint8_t res;
209 uint8_t satellites;
210 uint32_t res2;
211 } ubx_nav_solution;
213 typedef struct {
214 uint32_t time; // GPS msToW
215 int32_t ned_north;
216 int32_t ned_east;
217 int32_t ned_down;
218 uint32_t speed_3d;
219 uint32_t speed_2d;
220 int32_t heading_2d;
221 uint32_t speed_accuracy;
222 uint32_t heading_accuracy;
223 } ubx_nav_velned;
225 typedef struct {
226 uint8_t chn; // Channel number, 255 for SVx not assigned to channel
227 uint8_t svid; // Satellite ID
228 uint8_t flags; // Bitmask
229 uint8_t quality; // Bitfield
230 uint8_t cno; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
231 uint8_t elev; // Elevation in integer degrees
232 int16_t azim; // Azimuth in integer degrees
233 int32_t prRes; // Pseudo range residual in centimetres
234 } ubx_nav_svinfo_channel;
236 typedef struct {
237 uint32_t time; // GPS Millisecond time of week
238 uint8_t numCh; // Number of channels
239 uint8_t globalFlags; // Bitmask, Chip hardware generation 0:Antaris, 1:u-blox 5, 2:u-blox 6
240 uint16_t reserved2; // Reserved
241 ubx_nav_svinfo_channel channel[16]; // 16 satellites * 12 byte
242 } ubx_nav_svinfo;
244 typedef struct {
245 uint32_t time; // GPS msToW
246 uint32_t tAcc;
247 int32_t nano;
248 uint16_t year;
249 uint8_t month;
250 uint8_t day;
251 uint8_t hour;
252 uint8_t min;
253 uint8_t sec;
254 uint8_t valid;
255 } ubx_nav_timeutc;
257 typedef struct {
258 uint32_t time; // GPS msToW
259 uint16_t year;
260 uint8_t month;
261 uint8_t day;
262 uint8_t hour;
263 uint8_t min;
264 uint8_t sec;
265 uint8_t valid;
266 uint32_t tAcc;
267 int32_t nano;
268 uint8_t fix_type;
269 uint8_t fix_status;
270 uint8_t reserved1;
271 uint8_t satellites;
272 int32_t longitude;
273 int32_t latitude;
274 int32_t altitude_ellipsoid;
275 int32_t altitude_msl;
276 uint32_t horizontal_accuracy;
277 uint32_t vertical_accuracy;
278 int32_t ned_north;
279 int32_t ned_east;
280 int32_t ned_down;
281 int32_t speed_2d;
282 int32_t heading_2d;
283 uint32_t speed_accuracy;
284 uint32_t heading_accuracy;
285 uint16_t position_DOP;
286 uint16_t reserved2;
287 uint16_t reserved3;
288 } ubx_nav_pvt;
290 typedef struct {
291 uint8_t class;
292 uint8_t msg;
293 } ubx_ack_ack;
295 enum {
296 PREAMBLE1 = 0xB5,
297 PREAMBLE2 = 0x62,
298 CLASS_NAV = 0x01,
299 CLASS_ACK = 0x05,
300 CLASS_CFG = 0x06,
301 CLASS_MON = 0x0A,
302 MSG_CLASS_UBX = 0x01,
303 MSG_CLASS_NMEA = 0xF0,
304 MSG_VER = 0x04,
305 MSG_ACK_NACK = 0x00,
306 MSG_ACK_ACK = 0x01,
307 MSG_NMEA_GGA = 0x0,
308 MSG_NMEA_GLL = 0x1,
309 MSG_NMEA_GSA = 0x2,
310 MSG_NMEA_GSV = 0x3,
311 MSG_NMEA_RMC = 0x4,
312 MSG_NMEA_VGS = 0x5,
313 MSG_POSLLH = 0x2,
314 MSG_STATUS = 0x3,
315 MSG_SOL = 0x6,
316 MSG_PVT = 0x7,
317 MSG_VELNED = 0x12,
318 MSG_TIMEUTC = 0x21,
319 MSG_SVINFO = 0x30,
320 MSG_NAV_SAT = 0x35,
321 MSG_NAV_SIG = 0x35,
322 MSG_CFG_PRT = 0x00,
323 MSG_CFG_RATE = 0x08,
324 MSG_CFG_SET_RATE = 0x01,
325 MSG_CFG_NAV_SETTINGS = 0x24,
326 MSG_CFG_SBAS = 0x16,
327 MSG_CFG_GNSS = 0x3e
328 } ubx_protocol_bytes;
330 enum {
331 FIX_NONE = 0,
332 FIX_DEAD_RECKONING = 1,
333 FIX_2D = 2,
334 FIX_3D = 3,
335 FIX_GPS_DEAD_RECKONING = 4,
336 FIX_TIME = 5
337 } ubs_nav_fix_type;
339 enum {
340 NAV_STATUS_FIX_VALID = 1
341 } ubx_nav_status_bits;
343 enum {
344 UBX_ACK_WAITING = 0,
345 UBX_ACK_GOT_ACK = 1,
346 UBX_ACK_GOT_NAK = 2
347 } ubx_ack_state;
349 // Packet checksum accumulators
350 static uint8_t _ck_a;
351 static uint8_t _ck_b;
353 // State machine state
354 static bool _skip_packet;
355 static uint8_t _step;
356 static uint8_t _msg_id;
357 static uint16_t _payload_length;
358 static uint16_t _payload_counter;
360 static uint8_t next_fix_type;
361 static uint8_t _class;
362 static uint8_t _ack_state;
363 static uint8_t _ack_waiting_msg;
365 // do we have new position information?
366 static bool _new_position;
368 // do we have new speed information?
369 static bool _new_speed;
371 // Need this to determine if Galileo capable only
372 static bool capGalileo;
374 // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
375 //15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
376 //15:17:55 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position'
377 //15:17:55 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84'
378 //15:17:55 R -> UBX NAV-CLOCK, Size 28, 'Clock Status'
379 //15:17:55 R -> UBX NAV-AOPSTATUS, Size 24, 'AOP Status'
380 //15:17:55 R -> UBX 03-09, Size 208, 'Unknown'
381 //15:17:55 R -> UBX 03-10, Size 336, 'Unknown'
382 //15:17:55 R -> UBX NAV-SOL, Size 60, 'Navigation Solution'
383 //15:17:55 R -> UBX NAV, Size 100, 'Navigation'
384 //15:17:55 R -> UBX NAV-SVINFO, Size 328, 'Satellite Status and Information'
387 // Send buffer
388 static union {
389 ubx_message message;
390 uint8_t bytes[58];
391 } send_buffer;
393 // Receive buffer
394 static union {
395 ubx_nav_posllh posllh;
396 ubx_nav_status status;
397 ubx_nav_solution solution;
398 ubx_nav_velned velned;
399 ubx_nav_pvt pvt;
400 ubx_nav_svinfo svinfo;
401 ubx_mon_ver ver;
402 ubx_nav_timeutc timeutc;
403 ubx_ack_ack ack;
404 uint8_t bytes[UBLOX_BUFFER_SIZE];
405 } _buffer;
407 void _update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
409 while (len--) {
410 *ck_a += *data;
411 *ck_b += *ck_a;
412 data++;
416 static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType)
418 if (fixValid && ubloxFixType == FIX_2D)
419 return GPS_FIX_2D;
420 if (fixValid && ubloxFixType == FIX_3D)
421 return GPS_FIX_3D;
422 return GPS_NO_FIX;
425 static void sendConfigMessageUBLOX(void)
427 uint8_t ck_a=0, ck_b=0;
428 send_buffer.message.header.preamble1=PREAMBLE1;
429 send_buffer.message.header.preamble2=PREAMBLE2;
430 _update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b);
431 send_buffer.bytes[send_buffer.message.header.length+6] = ck_a;
432 send_buffer.bytes[send_buffer.message.header.length+7] = ck_b;
433 serialWriteBuf(gpsState.gpsPort, send_buffer.bytes, send_buffer.message.header.length+8);
435 // Save state for ACK waiting
436 _ack_waiting_msg = send_buffer.message.header.msg_id;
437 _ack_state = UBX_ACK_WAITING;
440 static void pollVersion(void)
442 send_buffer.message.header.msg_class = CLASS_MON;
443 send_buffer.message.header.msg_id = MSG_VER;
444 send_buffer.message.header.length = 0;
445 sendConfigMessageUBLOX();
448 static const uint8_t default_payload[] = {
449 0xFF, 0xFF, 0x03, 0x03, 0x00, // CFG-NAV5 - Set engine settings (original MWII code)
450 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and
451 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // capturing the data from the U-Center binary console.
452 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
455 #define GNSSID_SBAS 1
456 #define GNSSID_GALILEO 2
458 static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
460 gnss_block->gnssId = GNSSID_SBAS;
461 gnss_block->maxTrkCh = 3;
462 gnss_block->sigCfgMask = 1;
463 if (gpsState.gpsConfig->sbasMode == SBAS_NONE) {
464 gnss_block->enabled = 0;
465 gnss_block->resTrkCh = 0;
466 } else {
467 gnss_block->enabled = 1;
468 gnss_block->resTrkCh = 1;
471 return 1;
474 static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
476 if (!capGalileo) {
477 return 0;
480 gnss_block->gnssId = GNSSID_GALILEO;
481 gnss_block->maxTrkCh = 8;
482 gnss_block->sigCfgMask = 1;
483 if (gpsState.gpsConfig->ubloxUseGalileo) {
484 gnss_block->enabled = 1;
485 gnss_block->resTrkCh = 4;
486 } else {
487 gnss_block->enabled = 0;
488 gnss_block->resTrkCh = 0;
491 return 1;
494 static void configureGNSS(void)
496 int blocksUsed = 0;
498 send_buffer.message.header.msg_class = CLASS_CFG;
499 send_buffer.message.header.msg_id = MSG_CFG_GNSS;
500 send_buffer.message.payload.gnss.msgVer = 0;
501 send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
502 send_buffer.message.payload.gnss.numTrkChUse = 32;
504 /* SBAS, always generated */
505 blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
507 /* Galileo */
508 blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
510 send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
511 send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed);
512 sendConfigMessageUBLOX();
515 static void configureNAV5(uint8_t dynModel, uint8_t fixMode)
517 send_buffer.message.header.msg_class = CLASS_CFG;
518 send_buffer.message.header.msg_id = MSG_CFG_NAV_SETTINGS;
519 send_buffer.message.header.length = 0x24;
520 memcpy(send_buffer.message.payload.bytes, default_payload, sizeof(default_payload));
521 send_buffer.message.payload.bytes[2] = dynModel;
522 send_buffer.message.payload.bytes[3] = fixMode;
523 sendConfigMessageUBLOX();
526 static void configureMSG(uint8_t class, uint8_t id, uint8_t rate)
528 send_buffer.message.header.msg_class = CLASS_CFG;
529 send_buffer.message.header.msg_id = MSG_CFG_SET_RATE;
530 send_buffer.message.header.length = 3;
531 send_buffer.message.payload.msg.class = class;
532 send_buffer.message.payload.msg.id = id;
533 send_buffer.message.payload.msg.rate = rate;
534 sendConfigMessageUBLOX();
538 * measRate in ms
539 * navRate cycles
540 * timeRef 0 UTC, 1 GPS
542 static void configureRATE(uint16_t measRate)
544 send_buffer.message.header.msg_class = CLASS_CFG;
545 send_buffer.message.header.msg_id = MSG_CFG_RATE;
546 send_buffer.message.header.length = 6;
547 send_buffer.message.payload.rate.meas=measRate;
548 send_buffer.message.payload.rate.nav=1;
549 send_buffer.message.payload.rate.time=1;
550 sendConfigMessageUBLOX();
555 static void configureSBAS(void)
557 send_buffer.message.header.msg_class = CLASS_CFG;
558 send_buffer.message.header.msg_id = MSG_CFG_SBAS;
559 send_buffer.message.header.length = 8;
560 send_buffer.message.payload.sbas.mode=(gpsState.gpsConfig->sbasMode == SBAS_NONE?2:3);
561 send_buffer.message.payload.sbas.usage=3;
562 send_buffer.message.payload.sbas.maxSBAS=3;
563 send_buffer.message.payload.sbas.scanmode2=0;
564 send_buffer.message.payload.sbas.scanmode1=ubloxScanMode1[gpsState.gpsConfig->sbasMode];
565 sendConfigMessageUBLOX();
568 static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
570 // ublox_5 hwVersion 00040005
571 if (strncmp(szBuf, "00040005", nBufSize) == 0) {
572 return UBX_HW_VERSION_UBLOX5;
575 // ublox_6 hwVersion 00040007
576 if (strncmp(szBuf, "00040007", nBufSize) == 0) {
577 return UBX_HW_VERSION_UBLOX6;
580 // ublox_7 hwVersion 00070000
581 if (strncmp(szBuf, "00070000", nBufSize) == 0) {
582 return UBX_HW_VERSION_UBLOX7;
585 // ublox_M8 hwVersion 00080000
586 if (strncmp(szBuf, "00080000", nBufSize) == 0) {
587 return UBX_HW_VERSION_UBLOX8;
590 // ublox_M9 hwVersion 00190000
591 if (strncmp(szBuf, "00190000", nBufSize) == 0) {
592 return UBX_HW_VERSION_UBLOX9;
595 // ublox_M10 hwVersion 000A0000
596 if (strncmp(szBuf, "000A0000", nBufSize) == 0) {
597 return UBX_HW_VERSION_UBLOX10;
600 return UBX_HW_VERSION_UNKNOWN;
603 static bool gpsParceFrameUBLOX(void)
605 switch (_msg_id) {
606 case MSG_POSLLH:
607 gpsSol.llh.lon = _buffer.posllh.longitude;
608 gpsSol.llh.lat = _buffer.posllh.latitude;
609 gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm
610 gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
611 gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
612 gpsSol.flags.validEPE = true;
613 if (next_fix_type != GPS_NO_FIX)
614 gpsSol.fixType = next_fix_type;
615 _new_position = true;
616 break;
617 case MSG_STATUS:
618 next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type);
619 if (next_fix_type == GPS_NO_FIX)
620 gpsSol.fixType = GPS_NO_FIX;
621 break;
622 case MSG_SOL:
623 next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type);
624 if (next_fix_type == GPS_NO_FIX)
625 gpsSol.fixType = GPS_NO_FIX;
626 gpsSol.numSat = _buffer.solution.satellites;
627 gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
628 break;
629 case MSG_VELNED:
630 gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
631 gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
632 gpsSol.velNED[X] = _buffer.velned.ned_north;
633 gpsSol.velNED[Y] = _buffer.velned.ned_east;
634 gpsSol.velNED[Z] = _buffer.velned.ned_down;
635 gpsSol.flags.validVelNE = true;
636 gpsSol.flags.validVelD = true;
637 _new_speed = true;
638 break;
639 case MSG_TIMEUTC:
640 if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) {
641 gpsSol.time.year = _buffer.timeutc.year;
642 gpsSol.time.month = _buffer.timeutc.month;
643 gpsSol.time.day = _buffer.timeutc.day;
644 gpsSol.time.hours = _buffer.timeutc.hour;
645 gpsSol.time.minutes = _buffer.timeutc.min;
646 gpsSol.time.seconds = _buffer.timeutc.sec;
647 gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000);
649 gpsSol.flags.validTime = true;
650 } else {
651 gpsSol.flags.validTime = false;
653 break;
654 case MSG_PVT:
655 next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type);
656 gpsSol.fixType = next_fix_type;
657 gpsSol.llh.lon = _buffer.pvt.longitude;
658 gpsSol.llh.lat = _buffer.pvt.latitude;
659 gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm
660 gpsSol.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s
661 gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s
662 gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s
663 gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s
664 gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
665 gpsSol.numSat = _buffer.pvt.satellites;
666 gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
667 gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
668 gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
669 gpsSol.flags.validVelNE = true;
670 gpsSol.flags.validVelD = true;
671 gpsSol.flags.validEPE = true;
673 if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) {
674 gpsSol.time.year = _buffer.pvt.year;
675 gpsSol.time.month = _buffer.pvt.month;
676 gpsSol.time.day = _buffer.pvt.day;
677 gpsSol.time.hours = _buffer.pvt.hour;
678 gpsSol.time.minutes = _buffer.pvt.min;
679 gpsSol.time.seconds = _buffer.pvt.sec;
680 gpsSol.time.millis = _buffer.pvt.nano / (1000*1000);
682 gpsSol.flags.validTime = true;
683 } else {
684 gpsSol.flags.validTime = false;
687 _new_position = true;
688 _new_speed = true;
689 break;
690 case MSG_VER:
691 if (_class == CLASS_MON) {
692 gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion));
693 if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) && (_buffer.ver.swVersion[9] > '2')) {
694 // check extensions;
695 // after hw + sw vers; each is 30 bytes
696 for(int j = 40; j < _payload_length; j += 30) {
697 if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) {
698 capGalileo = true;
699 break;
704 break;
705 case MSG_ACK_ACK:
706 if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) {
707 _ack_state = UBX_ACK_GOT_ACK;
709 break;
710 case MSG_ACK_NACK:
711 if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) {
712 _ack_state = UBX_ACK_GOT_NAK;
714 break;
715 default:
716 return false;
719 // we only return true when we get new position and speed data
720 // this ensures we don't use stale data
721 if (_new_position && _new_speed) {
722 _new_speed = _new_position = false;
723 return true;
726 return false;
729 static bool gpsNewFrameUBLOX(uint8_t data)
731 bool parsed = false;
733 switch (_step) {
734 case 0: // Sync char 1 (0xB5)
735 if (PREAMBLE1 == data) {
736 _skip_packet = false;
737 _step++;
739 break;
740 case 1: // Sync char 2 (0x62)
741 if (PREAMBLE2 != data) {
742 _step = 0;
743 break;
745 _step++;
746 break;
747 case 2: // Class
748 _step++;
749 _class = data;
750 _ck_b = _ck_a = data; // reset the checksum accumulators
751 break;
752 case 3: // Id
753 _step++;
754 _ck_b += (_ck_a += data); // checksum byte
755 _msg_id = data;
756 break;
757 case 4: // Payload length (part 1)
758 _step++;
759 _ck_b += (_ck_a += data); // checksum byte
760 _payload_length = data; // payload length low byte
761 break;
762 case 5: // Payload length (part 2)
763 _step++;
764 _ck_b += (_ck_a += data); // checksum byte
765 _payload_length |= (uint16_t)(data << 8);
766 if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE ) {
767 // we can't receive the whole packet, just log the error and start searching for the next packet.
768 gpsStats.errors++;
769 _step = 0;
770 break;
772 // prepare to receive payload
773 _payload_counter = 0;
774 if (_payload_length == 0) {
775 _step = 7;
777 break;
778 case 6:
779 _ck_b += (_ck_a += data); // checksum byte
780 if (_payload_counter < MAX_UBLOX_PAYLOAD_SIZE) {
781 _buffer.bytes[_payload_counter] = data;
783 // NOTE: check counter BEFORE increasing so that a payload_size of 65535 is correctly handled. This can happen if garbage data is received.
784 if (_payload_counter == _payload_length - 1) {
785 _step++;
787 _payload_counter++;
788 break;
789 case 7:
790 _step++;
791 if (_ck_a != data) {
792 _skip_packet = true; // bad checksum
793 gpsStats.errors++;
794 _step = 0;
796 break;
797 case 8:
798 _step = 0;
800 if (_ck_b != data) {
801 gpsStats.errors++;
802 break; // bad checksum
805 gpsStats.packetCount++;
807 if (_skip_packet) {
808 break;
811 if (gpsParceFrameUBLOX()) {
812 parsed = true;
816 return parsed;
819 STATIC_PROTOTHREAD(gpsConfigure)
821 ptBegin(gpsConfigure);
823 // Reset timeout
824 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
826 // Set dynamic model
827 switch (gpsState.gpsConfig->dynModel) {
828 case GPS_DYNMODEL_PEDESTRIAN:
829 configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
830 break;
831 case GPS_DYNMODEL_AIR_1G: // Default to this
832 default:
833 configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO);
834 break;
835 case GPS_DYNMODEL_AIR_4G:
836 configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO);
837 break;
839 ptWait(_ack_state == UBX_ACK_GOT_ACK);
841 // Disable NMEA messages
842 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
844 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GGA, 0);
845 ptWait(_ack_state == UBX_ACK_GOT_ACK);
847 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GLL, 0);
848 ptWait(_ack_state == UBX_ACK_GOT_ACK);
850 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSA, 0);
851 ptWait(_ack_state == UBX_ACK_GOT_ACK);
853 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSV, 0);
854 ptWait(_ack_state == UBX_ACK_GOT_ACK);
856 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_RMC, 0);
857 ptWait(_ack_state == UBX_ACK_GOT_ACK);
859 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_VGS, 0);
860 ptWait(_ack_state == UBX_ACK_GOT_ACK);
862 // Configure UBX binary messages
863 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
865 // M9N & M10 does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence
866 if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9) {
867 configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
868 ptWait(_ack_state == UBX_ACK_GOT_ACK);
870 configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
871 ptWait(_ack_state == UBX_ACK_GOT_ACK);
873 configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
874 ptWait(_ack_state == UBX_ACK_GOT_ACK);
876 configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0);
877 ptWait(_ack_state == UBX_ACK_GOT_ACK);
879 configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
880 ptWait(_ack_state == UBX_ACK_GOT_ACK);
882 configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 0);
883 ptWait(_ack_state == UBX_ACK_GOT_ACK);
885 configureMSG(MSG_CLASS_UBX, MSG_NAV_SIG, 0);
886 ptWait(_ack_state == UBX_ACK_GOT_ACK);
888 // u-Blox 9 receivers such as M9N can do 10Hz as well, but the number of used satellites will be restricted to 16.
889 // Not mentioned in the datasheet
890 configureRATE(200);
891 ptWait(_ack_state == UBX_ACK_GOT_ACK);
893 else {
894 // u-Blox 5/6/7/8 or unknown
895 // u-Blox 7-8 support PVT
896 if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7) {
897 configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
898 ptWait(_ack_state == UBX_ACK_GOT_ACK);
900 configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
901 ptWait(_ack_state == UBX_ACK_GOT_ACK);
903 configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
904 ptWait(_ack_state == UBX_ACK_GOT_ACK);
906 configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
907 ptWait(_ack_state == UBX_ACK_GOT_ACK);
909 configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0);
910 ptWait(_ack_state == UBX_ACK_GOT_ACK);
912 configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
913 ptWait(_ack_state == UBX_ACK_GOT_ACK);
915 configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
916 ptWait(_ack_state == UBX_ACK_GOT_ACK);
918 if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) {
919 configureRATE(100); // 10Hz
921 else {
922 configureRATE(200); // 5Hz
924 ptWait(_ack_state == UBX_ACK_GOT_ACK);
926 // u-Blox 5/6 doesn't support PVT, use legacy config
927 // UNKNOWN also falls here, use as a last resort
928 else {
929 configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1);
930 ptWait(_ack_state == UBX_ACK_GOT_ACK);
932 configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1);
933 ptWait(_ack_state == UBX_ACK_GOT_ACK);
935 configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
936 ptWait(_ack_state == UBX_ACK_GOT_ACK);
938 configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1);
939 ptWait(_ack_state == UBX_ACK_GOT_ACK);
941 configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10);
942 ptWait(_ack_state == UBX_ACK_GOT_ACK);
944 // This may fail on old UBLOX units, advance forward on both ACK and NAK
945 configureMSG(MSG_CLASS_UBX, MSG_PVT, 0);
946 ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
948 configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
949 ptWait(_ack_state == UBX_ACK_GOT_ACK);
951 // Configure data rate to 5HZ
952 configureRATE(200);
953 ptWait(_ack_state == UBX_ACK_GOT_ACK);
957 // Configure SBAS
958 // If particular SBAS setting is not supported by the hardware we'll get a NAK,
959 // however GPS would be functional. We are waiting for any response - ACK/NACK
960 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
961 configureSBAS();
962 ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
964 // Configure GNSS for M8N and later
965 if (gpsState.hwVersion >= 80000) {
966 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
967 configureGNSS();
968 ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
971 ptEnd(0);
974 static ptSemaphore_t semNewDataReady;
976 STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
978 ptBegin(gpsProtocolReceiverThread);
980 while (1) {
981 // Wait until there are bytes to consume
982 ptWait(serialRxBytesWaiting(gpsState.gpsPort));
984 // Consume bytes until buffer empty of until we have full message received
985 while (serialRxBytesWaiting(gpsState.gpsPort)) {
986 uint8_t newChar = serialRead(gpsState.gpsPort);
987 if (gpsNewFrameUBLOX(newChar)) {
988 ptSemaphoreSignal(semNewDataReady);
989 break;
994 ptEnd(0);
997 STATIC_PROTOTHREAD(gpsProtocolStateThread)
999 ptBegin(gpsProtocolStateThread);
1001 // Change baud rate
1002 if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) {
1003 // 0. Wait for TX buffer to be empty
1004 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
1006 // Try sending baud rate switch command at all common baud rates
1007 gpsSetProtocolTimeout((GPS_BAUD_CHANGE_DELAY + 50) * (GPS_BAUDRATE_COUNT));
1008 for (gpsState.autoBaudrateIndex = 0; gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT; gpsState.autoBaudrateIndex++) {
1009 // 2. Set serial port to baud rate and send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex]
1010 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);
1011 serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]);
1013 // 3. Wait for serial port to finish transmitting
1014 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
1016 // 4. Extra wait to make sure GPS processed the command
1017 ptDelayMs(GPS_BAUD_CHANGE_DELAY);
1019 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
1021 else {
1022 // No auto baud - set port baud rate to [baudrateIndex]
1023 // Wait for TX buffer to be empty
1024 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
1026 // Set baud rate and reset GPS timeout
1027 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
1030 // Configure GPS module if enabled
1031 if (gpsState.gpsConfig->autoConfig) {
1032 // Reset protocol timeout
1033 gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS)));
1035 // Attempt to detect GPS hw version
1036 gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN;
1037 gpsState.autoConfigStep = 0;
1039 do {
1040 pollVersion();
1041 gpsState.autoConfigStep++;
1042 ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
1043 } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
1045 // Configure GPS
1046 ptSpawn(gpsConfigure);
1049 // GPS setup done, reset timeout
1050 gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
1052 // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
1053 while (1) {
1054 ptSemaphoreWait(semNewDataReady);
1055 gpsProcessNewSolutionData();
1058 ptEnd(0);
1061 void gpsRestartUBLOX(void)
1063 ptSemaphoreInit(semNewDataReady);
1064 ptRestart(ptGetHandle(gpsProtocolReceiverThread));
1065 ptRestart(ptGetHandle(gpsProtocolStateThread));
1068 void gpsHandleUBLOX(void)
1070 // Run the protocol threads
1071 gpsProtocolReceiverThread();
1072 gpsProtocolStateThread();
1074 // If thread stopped - signal communication loss and restart
1075 if (ptIsStopped(ptGetHandle(gpsProtocolReceiverThread)) || ptIsStopped(ptGetHandle(gpsProtocolStateThread))) {
1076 gpsSetState(GPS_LOST_COMMUNICATION);
1080 #endif