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/>.
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"
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
[] = {
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
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
133 } ubx_gnss_element_t
;
139 uint8_t numConfigBlocks
;
140 ubx_gnss_element_t config
[0];
144 #define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
147 uint8_t bytes
[MAX_GNSS_SIZE_BYTES
]; // placeholder
166 } __attribute__((packed
)) ubx_message
;
169 char swVersion
[30]; // Zero-terminated Software Version String
170 char hwVersion
[10]; // Zero-terminated Hardware Version String
174 uint32_t time
; // GPS msToW
177 int32_t altitude_ellipsoid
;
178 int32_t altitude_msl
;
179 uint32_t horizontal_accuracy
;
180 uint32_t vertical_accuracy
;
184 uint32_t time
; // GPS msToW
187 uint8_t differential_status
;
189 uint32_t time_to_first_fix
;
190 uint32_t uptime
; // milliseconds
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
;
214 uint32_t time
; // GPS msToW
221 uint32_t speed_accuracy
;
222 uint32_t heading_accuracy
;
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
;
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
245 uint32_t time
; // GPS msToW
258 uint32_t time
; // GPS msToW
274 int32_t altitude_ellipsoid
;
275 int32_t altitude_msl
;
276 uint32_t horizontal_accuracy
;
277 uint32_t vertical_accuracy
;
283 uint32_t speed_accuracy
;
284 uint32_t heading_accuracy
;
285 uint16_t position_DOP
;
302 MSG_CLASS_UBX
= 0x01,
303 MSG_CLASS_NMEA
= 0xF0,
324 MSG_CFG_SET_RATE
= 0x01,
325 MSG_CFG_NAV_SETTINGS
= 0x24,
328 } ubx_protocol_bytes
;
332 FIX_DEAD_RECKONING
= 1,
335 FIX_GPS_DEAD_RECKONING
= 4,
340 NAV_STATUS_FIX_VALID
= 1
341 } ubx_nav_status_bits
;
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'
395 ubx_nav_posllh posllh
;
396 ubx_nav_status status
;
397 ubx_nav_solution solution
;
398 ubx_nav_velned velned
;
400 ubx_nav_svinfo svinfo
;
402 ubx_nav_timeutc timeutc
;
404 uint8_t bytes
[UBLOX_BUFFER_SIZE
];
407 void _update_checksum(uint8_t *data
, uint8_t len
, uint8_t *ck_a
, uint8_t *ck_b
)
416 static uint8_t gpsMapFixType(bool fixValid
, uint8_t ubloxFixType
)
418 if (fixValid
&& ubloxFixType
== FIX_2D
)
420 if (fixValid
&& ubloxFixType
== FIX_3D
)
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;
467 gnss_block
->enabled
= 1;
468 gnss_block
->resTrkCh
= 1;
474 static int configureGNSS_GALILEO(ubx_gnss_element_t
* gnss_block
)
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;
487 gnss_block
->enabled
= 0;
488 gnss_block
->resTrkCh
= 0;
494 static void configureGNSS(void)
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
]);
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();
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)
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;
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
;
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
);
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;
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;
651 gpsSol
.flags
.validTime
= false;
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;
684 gpsSol
.flags
.validTime
= false;
687 _new_position
= true;
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')) {
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)) {
706 if ((_ack_state
== UBX_ACK_WAITING
) && (_buffer
.ack
.msg
== _ack_waiting_msg
)) {
707 _ack_state
= UBX_ACK_GOT_ACK
;
711 if ((_ack_state
== UBX_ACK_WAITING
) && (_buffer
.ack
.msg
== _ack_waiting_msg
)) {
712 _ack_state
= UBX_ACK_GOT_NAK
;
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;
729 static bool gpsNewFrameUBLOX(uint8_t data
)
734 case 0: // Sync char 1 (0xB5)
735 if (PREAMBLE1
== data
) {
736 _skip_packet
= false;
740 case 1: // Sync char 2 (0x62)
741 if (PREAMBLE2
!= data
) {
750 _ck_b
= _ck_a
= data
; // reset the checksum accumulators
754 _ck_b
+= (_ck_a
+= data
); // checksum byte
757 case 4: // Payload length (part 1)
759 _ck_b
+= (_ck_a
+= data
); // checksum byte
760 _payload_length
= data
; // payload length low byte
762 case 5: // Payload length (part 2)
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.
772 // prepare to receive payload
773 _payload_counter
= 0;
774 if (_payload_length
== 0) {
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) {
792 _skip_packet
= true; // bad checksum
802 break; // bad checksum
805 gpsStats
.packetCount
++;
811 if (gpsParceFrameUBLOX()) {
819 STATIC_PROTOTHREAD(gpsConfigure
)
821 ptBegin(gpsConfigure
);
824 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT
);
827 switch (gpsState
.gpsConfig
->dynModel
) {
828 case GPS_DYNMODEL_PEDESTRIAN
:
829 configureNAV5(UBX_DYNMODEL_PEDESTRIAN
, UBX_FIXMODE_AUTO
);
831 case GPS_DYNMODEL_AIR_1G
: // Default to this
833 configureNAV5(UBX_DYNMODEL_AIR_1G
, UBX_FIXMODE_AUTO
);
835 case GPS_DYNMODEL_AIR_4G
:
836 configureNAV5(UBX_DYNMODEL_AIR_4G
, UBX_FIXMODE_AUTO
);
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
891 ptWait(_ack_state
== UBX_ACK_GOT_ACK
);
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
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
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
953 ptWait(_ack_state
== UBX_ACK_GOT_ACK
);
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
);
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
);
968 ptWaitTimeout((_ack_state
== UBX_ACK_GOT_ACK
|| _ack_state
== UBX_ACK_GOT_NAK
), GPS_CFG_CMD_TIMEOUT_MS
);
974 static ptSemaphore_t semNewDataReady
;
976 STATIC_PROTOTHREAD(gpsProtocolReceiverThread
)
978 ptBegin(gpsProtocolReceiverThread
);
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
);
997 STATIC_PROTOTHREAD(gpsProtocolStateThread
)
999 ptBegin(gpsProtocolStateThread
);
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
]]);
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;
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
);
1046 ptSpawn(gpsConfigure
);
1049 // GPS setup done, reset timeout
1050 gpsSetProtocolTimeout(gpsState
.baseTimeoutMs
);
1052 // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
1054 ptSemaphoreWait(semNewDataReady
);
1055 gpsProcessNewSolutionData();
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
);