?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
[GPS] fix GPS discovery for M6 and earlier
[inav.git] / src / main / io / gps_ublox.c
blob8fa8366a23c318608327861add8717403cab4017
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 #include <stdarg.h>
25
26 #include "platform.h"
27 #include "build/build_config.h"
28
29 #if defined(USE_GPS) && defined(USE_GPS_PROTO_UBLOX)
30
31 #include "build/debug.h"
32
33
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"
39
40 #include "drivers/serial.h"
41 #include "drivers/time.h"
42
43 #include "fc/config.h"
44 #include "fc/runtime_config.h"
45 #include "fc/settings.h"
46
47 #include "io/serial.h"
48 #include "io/gps.h"
49 #include "io/gps_private.h"
50
51 #include "scheduler/protothreads.h"
52
53 #include "gps_ublox.h"
54 #include "gps_ublox_utils.h"
55
56
57 // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
58 // note PRNs last upadted 2023-08-10
59 // https://www.gps.gov/technical/prn-codes/L1-CA-PRN-code-assignments-2023-Apr.pdf
60
61 #define SBASMASK1_BASE 120
62 #define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE))
63
64 static const uint32_t ubloxScanMode1[] = {
65 0x00000000, // AUTO
66 (SBASMASK1_BITS(121) | SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136) | SBASMASK1_BITS(150)), // SBAS_EGNOS
67 (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(135) | SBASMASK1_BITS(138)), // WAAS
68
69 (SBASMASK1_BITS(129) | SBASMASK1_BITS(137) | SBASMASK1_BITS(139)), // MSAS
70
71 (SBASMASK1_BITS(127) | SBASMASK1_BITS(128) | SBASMASK1_BITS(132)), // GAGAN
72 (SBASMASK1_BITS(122)), // SPAN
73 0x00000000, // NONE
74 };
75
76 static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
77 "$PUBX,41,1,0003,0001,115200,0*1E\r\n", // GPS_BAUDRATE_115200
78 "$PUBX,41,1,0003,0001,57600,0*2D\r\n", // GPS_BAUDRATE_57600
79 "$PUBX,41,1,0003,0001,38400,0*26\r\n", // GPS_BAUDRATE_38400
80 "$PUBX,41,1,0003,0001,19200,0*23\r\n", // GPS_BAUDRATE_19200
81 "$PUBX,41,1,0003,0001,9600,0*16\r\n", // GPS_BAUDRATE_9600
82 "$PUBX,41,1,0003,0001,230400,0*1C\r\n", // GPS_BAUDRATE_230400
83 "$PUBX,41,1,0003,0001,460800,0*13\r\n", // GPS_BAUDRATE_460800
84 "$PUBX,41,1,0003,0001,921600,0*15\r\n" // GPS_BAUDRATE_921600
85 };
86
87 // Packet checksum accumulators
88 static uint8_t _ck_a;
89 static uint8_t _ck_b;
90
91 // State machine state
92 static bool _skip_packet;
93 static uint8_t _step;
94 static uint8_t _msg_id;
95 static uint16_t _payload_length;
96 static uint16_t _payload_counter;
97
98 static uint8_t next_fix_type;
99 static uint8_t _class;
100 static uint8_t _ack_state;
101 static uint8_t _ack_waiting_msg;
102
103 // do we have new position information?
104 static bool _new_position;
105
106 // do we have new speed information?
107 static bool _new_speed;
108
109 // Need this to determine if Galileo capable only
110 static struct {
111 uint8_t supported;
112 int capMaxGnss;
113 uint8_t defaultGnss;
114 uint8_t enabledGnss;
115 } ubx_capabilities = { };
116
117 // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver.
118 //15:17:55 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
119 //15:17:55 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position'
120 //15:17:55 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84'
121 //15:17:55 R -> UBX NAV-CLOCK, Size 28, 'Clock Status'
122 //15:17:55 R -> UBX NAV-AOPSTATUS, Size 24, 'AOP Status'
123 //15:17:55 R -> UBX 03-09, Size 208, 'Unknown'
124 //15:17:55 R -> UBX 03-10, Size 336, 'Unknown'
125 //15:17:55 R -> UBX NAV-SOL, Size 60, 'Navigation Solution'
126 //15:17:55 R -> UBX NAV, Size 100, 'Navigation'
127 //15:17:55 R -> UBX NAV-SVINFO, Size 328, 'Satellite Status and Information'
128
129
130 // Send buffer
131 static union {
132 ubx_message message;
133 uint8_t bytes[58];
134 } send_buffer;
135
136 // Receive buffer
137 static union {
138 ubx_nav_posllh posllh;
139 ubx_nav_status status;
140 ubx_nav_solution solution;
141 ubx_nav_velned velned;
142 ubx_nav_pvt pvt;
143 ubx_nav_svinfo svinfo;
144 ubx_mon_ver ver;
145 ubx_nav_timeutc timeutc;
146 ubx_ack_ack ack;
147 ubx_mon_gnss gnss;
148 uint8_t bytes[UBLOX_BUFFER_SIZE];
149 } _buffer;
150
151 bool gpsUbloxHasGalileo(void)
152 {
153 return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK);
154 }
155
156 bool gpsUbloxHasBeidou(void)
157 {
158 return ubx_capabilities.supported & UBX_MON_GNSS_BEIDOU_MASK;
159 }
160
161 bool gpsUbloxHasGlonass(void)
162 {
163 return ubx_capabilities.supported & UBX_MON_GNSS_GLONASS_MASK;
164 }
165
166 bool gpsUbloxGalileoDefault(void)
167 {
168 return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GALILEO_MASK;
169 }
170
171 bool gpsUbloxBeidouDefault(void)
172 {
173 return ubx_capabilities.defaultGnss & UBX_MON_GNSS_BEIDOU_MASK;
174 }
175
176 bool gpsUbloxGlonassDefault(void)
177 {
178 return ubx_capabilities.defaultGnss & UBX_MON_GNSS_GLONASS_MASK;
179 }
180
181 bool gpsUbloxGalileoEnabled(void)
182 {
183 return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GALILEO_MASK;
184 }
185
186 bool gpsUbloxBeidouEnabled(void)
187 {
188 return ubx_capabilities.enabledGnss & UBX_MON_GNSS_BEIDOU_MASK;
189 }
190
191 bool gpsUbloxGlonassEnabled(void)
192 {
193 return ubx_capabilities.enabledGnss & UBX_MON_GNSS_GLONASS_MASK;
194 }
195
196 uint8_t gpsUbloxMaxGnss(void)
197 {
198 return ubx_capabilities.capMaxGnss;
199 }
200
201 timeMs_t gpsUbloxCapLastUpdate(void)
202 {
203 return gpsState.lastCapaUpdMs;
204 }
205
206 static uint8_t gpsMapFixType(bool fixValid, uint8_t ubloxFixType)
207 {
208 if (fixValid && ubloxFixType == FIX_2D)
209 return GPS_FIX_2D;
210 if (fixValid && ubloxFixType == FIX_3D)
211 return GPS_FIX_3D;
212 return GPS_NO_FIX;
213 }
214
215 static void sendConfigMessageUBLOX(void)
216 {
217 uint8_t ck_a=0, ck_b=0;
218 send_buffer.message.header.preamble1=PREAMBLE1;
219 send_buffer.message.header.preamble2=PREAMBLE2;
220 ublox_update_checksum(&send_buffer.bytes[2], send_buffer.message.header.length+4, &ck_a, &ck_b);
221 send_buffer.bytes[send_buffer.message.header.length+6] = ck_a;
222 send_buffer.bytes[send_buffer.message.header.length+7] = ck_b;
223 serialWriteBuf(gpsState.gpsPort, send_buffer.bytes, send_buffer.message.header.length+8);
224
225 // Save state for ACK waiting
226 _ack_waiting_msg = send_buffer.message.header.msg_id;
227 _ack_state = UBX_ACK_WAITING;
228 }
229
230 static void pollVersion(void)
231 {
232 send_buffer.message.header.msg_class = CLASS_MON;
233 send_buffer.message.header.msg_id = MSG_VER;
234 send_buffer.message.header.length = 0;
235 sendConfigMessageUBLOX();
236 }
237
238 static void pollGnssCapabilities(void)
239 {
240 send_buffer.message.header.msg_class = CLASS_MON;
241 send_buffer.message.header.msg_id = MSG_MON_GNSS;
242 send_buffer.message.header.length = 0;
243 sendConfigMessageUBLOX();
244 }
245
246
247 static const uint8_t default_payload[] = {
248 0xFF, 0xFF, 0x03, 0x03, 0x00, // CFG-NAV5 - Set engine settings (original MWII code)
249 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, // Collected by resetting a GPS unit to defaults. Changing mode to Pedistrian and
250 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, // capturing the data from the U-Center binary console.
251 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
252 };
253
254 #define GNSSID_SBAS 1
255 #define GNSSID_GALILEO 2
256 #define GNSSID_BEIDOU 3
257 #define GNSSID_GZSS 5
258 #define GNSSID_GLONASS 6
259
260 // M10 ublox protocol info:
261 // https://content.u-blox.com/sites/default/files/u-blox-M10-SPG-5.10_InterfaceDescription_UBX-21035062.pdf
262 static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t count)
263 {
264 ubx_config_data8_t cfg = {};
265
266 ubloxCfgFillBytes(&cfg, kvPairs, count);
267
268 serialWriteBuf(gpsState.gpsPort, (uint8_t *)&cfg, cfg.header.length+8);
269 _ack_waiting_msg = cfg.header.msg_id;
270 _ack_state = UBX_ACK_WAITING;
271 }
272
273 // Info on protocol used by M8-M9, check UBX-CFG-GNSS for gnss configuration
274 // https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf
275 // https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf
276 static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
277 {
278 gnss_block->gnssId = GNSSID_SBAS;
279 gnss_block->maxTrkCh = 3;
280 gnss_block->sigCfgMask = 1;
281 if (gpsState.gpsConfig->sbasMode == SBAS_NONE) {
282 gnss_block->enabled = 0;
283 gnss_block->resTrkCh = 0;
284 } else {
285 gnss_block->enabled = 1;
286 gnss_block->resTrkCh = 1;
287 }
288
289 return 1;
290 }
291
292 static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
293 {
294 if (!gpsUbloxHasGalileo()) {
295 return 0;
296 }
297
298 gnss_block->gnssId = GNSSID_GALILEO;
299 gnss_block->maxTrkCh = 8;
300 // sigCfgMask
301 // 0x01 = Galileo E1 (not supported for protocol versions less than 18.00)
302 // 0x10 = Galileo E5a // off by default
303 // 0x20 = Galileo E5b // off by default
304 gnss_block->sigCfgMask = 0x01;
305 if (gpsState.gpsConfig->ubloxUseGalileo) {
306 gnss_block->enabled = 1;
307 gnss_block->resTrkCh = 4;
308 } else {
309 gnss_block->enabled = 0;
310 gnss_block->resTrkCh = 0;
311 }
312
313 return 1;
314 }
315
316 static int configureGNSS_BEIDOU(ubx_gnss_element_t * gnss_block)
317 {
318 if (!gpsUbloxHasBeidou()) {
319 return 0;
320 }
321
322 gnss_block->gnssId = GNSSID_BEIDOU;
323 gnss_block->maxTrkCh = 8;
324 // sigCfgMask
325 // 0x01 = BeiDou B1I
326 // 0x10 = BeiDou B2I // off by default
327 // 0x80 = BeiDou B2A // off by default
328 gnss_block->sigCfgMask = 0x01;
329 if (gpsState.gpsConfig->ubloxUseBeidou) {
330 gnss_block->enabled = 1;
331 gnss_block->resTrkCh = 4;
332 } else {
333 gnss_block->enabled = 0;
334 gnss_block->resTrkCh = 0;
335 }
336
337 return 1;
338 }
339
340 /*
341 static int configureGNSS_GZSS(ubx_gnss_element_t * gnss_block)
342 {
343 if (!ubx_capabilities.capGzss) {
344 return 0;
345 }
346
347 gnss_block->gnssId = GNSSID_GZSS;
348 gnss_block->maxTrkCh = 8;
349 // L1C = 0x01
350 // L1S = 0x04
351 // L2C = 0x10
352 gnss_block->sigCfgMask = 0x01 | 0x04;
353 gnss_block->enabled = 1;
354 gnss_block->resTrkCh = 4;
355
356 return 1;
357 }
358 */
359
360 static int configureGNSS_GLONASS(ubx_gnss_element_t * gnss_block)
361 {
362 if(!gpsUbloxHasGlonass()) {
363 return 0;
364 }
365
366 gnss_block->gnssId = GNSSID_GLONASS;
367 gnss_block->maxTrkCh = 8;
368 // 0x01 = GLONASS L1
369 // 0x10 = GLONASS L2 // off by default
370 gnss_block->sigCfgMask = 0x01;
371 if (gpsState.gpsConfig->ubloxUseGlonass) {
372 gnss_block->enabled = 1;
373 gnss_block->resTrkCh = 4;
374 } else {
375 gnss_block->enabled = 0;
376 gnss_block->resTrkCh = 0;
377 }
378
379 return 1;
380 }
381
382 static void configureGNSS10(void)
383 {
384 ubx_config_data8_payload_t gnssConfigValues[] = {
385 // SBAS
386 {UBLOX_CFG_SIGNAL_SBAS_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1},
387 {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1},
388
389 // Galileo
390 {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo},
391 {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo},
392
393 // Beidou
394 {UBLOX_CFG_SIGNAL_BDS_ENA, gpsState.gpsConfig->ubloxUseBeidou},
395 {UBLOX_CFG_SIGNAL_BDS_B1_ENA, gpsState.gpsConfig->ubloxUseBeidou},
396 {UBLOX_CFG_SIGNAL_BDS_B1C_ENA, 0},
397
398 // Should be enabled with GPS
399 {UBLOX_CFG_QZSS_ENA, 1},
400 {UBLOX_CFG_QZSS_L1CA_ENA, 1},
401 {UBLOX_CFG_QZSS_L1S_ENA, 1},
402
403 // Glonass
404 {UBLOX_CFG_GLO_ENA, gpsState.gpsConfig->ubloxUseGlonass},
405 {UBLOX_CFG_GLO_L1_ENA, gpsState.gpsConfig->ubloxUseGlonass}
406 };
407
408 ubloxSendSetCfgBytes(gnssConfigValues, 12);
409 }
410
411 static void configureGNSS(void)
412 {
413 int blocksUsed = 0;
414 send_buffer.message.header.msg_class = CLASS_CFG;
415 send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET
416 send_buffer.message.payload.gnss.msgVer = 0;
417 send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
418 send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max
419
420 /* SBAS, always generated */
421 blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
422
423 /* Galileo */
424 blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
425
426 /* BeiDou */
427 blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]);
428
429 /* GLONASS */
430 blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]);
431
432 send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
433 send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t) * blocksUsed);
434 sendConfigMessageUBLOX();
435 }
436
437 static void configureNAV5(uint8_t dynModel, uint8_t fixMode)
438 {
439 send_buffer.message.header.msg_class = CLASS_CFG;
440 send_buffer.message.header.msg_id = MSG_CFG_NAV_SETTINGS;
441 send_buffer.message.header.length = 0x24;
442 memcpy(send_buffer.message.payload.bytes, default_payload, sizeof(default_payload));
443 send_buffer.message.payload.bytes[2] = dynModel;
444 send_buffer.message.payload.bytes[3] = fixMode;
445 sendConfigMessageUBLOX();
446 }
447
448 static void configureMSG(uint8_t msg_class, uint8_t id, uint8_t rate)
449 {
450 send_buffer.message.header.msg_class = CLASS_CFG;
451 send_buffer.message.header.msg_id = MSG_CFG_SET_RATE;
452 send_buffer.message.header.length = 3;
453 send_buffer.message.payload.msg.msg_class = msg_class;
454 send_buffer.message.payload.msg.id = id;
455 send_buffer.message.payload.msg.rate = rate;
456 sendConfigMessageUBLOX();
457 }
458
459 /*
460 * measRate in ms
461 * navRate cycles
462 * timeRef 0 UTC, 1 GPS
463 */
464 static void configureRATE(uint16_t measRate)
465 {
466 send_buffer.message.header.msg_class = CLASS_CFG;
467 send_buffer.message.header.msg_id = MSG_CFG_RATE;
468 send_buffer.message.header.length = 6;
469 send_buffer.message.payload.rate.meas=measRate;
470 send_buffer.message.payload.rate.nav=1;
471 send_buffer.message.payload.rate.time=1;
472 sendConfigMessageUBLOX();
473 }
474
475 /*
476 */
477 static void configureSBAS(void)
478 {
479 send_buffer.message.header.msg_class = CLASS_CFG;
480 send_buffer.message.header.msg_id = MSG_CFG_SBAS;
481 send_buffer.message.header.length = 8;
482 send_buffer.message.payload.sbas.mode=(gpsState.gpsConfig->sbasMode == SBAS_NONE?2:3);
483 send_buffer.message.payload.sbas.usage=3;
484 send_buffer.message.payload.sbas.maxSBAS=3;
485 send_buffer.message.payload.sbas.scanmode2=0;
486 send_buffer.message.payload.sbas.scanmode1=ubloxScanMode1[gpsState.gpsConfig->sbasMode];
487 sendConfigMessageUBLOX();
488 }
489
490 static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength)
491 {
492 if (bufferLength > 13 && (!strncmp(proto, "PROTVER=", 8) || !strncmp(proto, "PROTVER ", 8))) {
493 proto+=8;
494
495 float ver = atof(proto);
496
497 gpsState.swVersionMajor = (uint8_t)ver;
498 gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f);
499 }
500 }
501
502 static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
503 {
504 // ublox_5 hwVersion 00040005
505 if (strncmp(szBuf, "00040005", nBufSize) == 0) {
506 return UBX_HW_VERSION_UBLOX5;
507 }
508
509 // ublox_6 hwVersion 00040007
510 if (strncmp(szBuf, "00040007", nBufSize) == 0) {
511 return UBX_HW_VERSION_UBLOX6;
512 }
513
514 // ublox_7 hwVersion 00070000
515 if (strncmp(szBuf, "00070000", nBufSize) == 0) {
516 return UBX_HW_VERSION_UBLOX7;
517 }
518
519 // ublox_M8 hwVersion 00080000
520 if (strncmp(szBuf, "00080000", nBufSize) == 0) {
521 return UBX_HW_VERSION_UBLOX8;
522 }
523
524 // ublox_M9 hwVersion 00190000
525 if (strncmp(szBuf, "00190000", nBufSize) == 0) {
526 return UBX_HW_VERSION_UBLOX9;
527 }
528
529 // ublox_M10 hwVersion 000A0000
530 if (strncmp(szBuf, "000A0000", nBufSize) == 0) {
531 return UBX_HW_VERSION_UBLOX10;
532 }
533
534 return UBX_HW_VERSION_UNKNOWN;
535 }
536
537 static bool gpsParseFrameUBLOX(void)
538 {
539 switch (_msg_id) {
540 case MSG_POSLLH:
541 gpsSol.llh.lon = _buffer.posllh.longitude;
542 gpsSol.llh.lat = _buffer.posllh.latitude;
543 gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm
544 gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
545 gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
546 gpsSol.flags.validEPE = true;
547 if (next_fix_type != GPS_NO_FIX)
548 gpsSol.fixType = next_fix_type;
549 _new_position = true;
550 break;
551 case MSG_STATUS:
552 next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type);
553 if (next_fix_type == GPS_NO_FIX)
554 gpsSol.fixType = GPS_NO_FIX;
555 break;
556 case MSG_SOL:
557 next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type);
558 if (next_fix_type == GPS_NO_FIX)
559 gpsSol.fixType = GPS_NO_FIX;
560 gpsSol.numSat = _buffer.solution.satellites;
561 gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
562 break;
563 case MSG_VELNED:
564 gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
565 gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
566 gpsSol.velNED[X] = _buffer.velned.ned_north;
567 gpsSol.velNED[Y] = _buffer.velned.ned_east;
568 gpsSol.velNED[Z] = _buffer.velned.ned_down;
569 gpsSol.flags.validVelNE = true;
570 gpsSol.flags.validVelD = true;
571 _new_speed = true;
572 break;
573 case MSG_TIMEUTC:
574 if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) {
575 gpsSol.time.year = _buffer.timeutc.year;
576 gpsSol.time.month = _buffer.timeutc.month;
577 gpsSol.time.day = _buffer.timeutc.day;
578 gpsSol.time.hours = _buffer.timeutc.hour;
579 gpsSol.time.minutes = _buffer.timeutc.min;
580 gpsSol.time.seconds = _buffer.timeutc.sec;
581 gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000);
582
583 gpsSol.flags.validTime = true;
584 } else {
585 gpsSol.flags.validTime = false;
586 }
587 break;
588 case MSG_PVT:
589 next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type);
590 gpsSol.fixType = next_fix_type;
591 gpsSol.llh.lon = _buffer.pvt.longitude;
592 gpsSol.llh.lat = _buffer.pvt.latitude;
593 gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm
594 gpsSol.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s
595 gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s
596 gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s
597 gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s
598 gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
599 gpsSol.numSat = _buffer.pvt.satellites;
600 gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
601 gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
602 gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
603 gpsSol.flags.validVelNE = true;
604 gpsSol.flags.validVelD = true;
605 gpsSol.flags.validEPE = true;
606
607 if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) {
608 gpsSol.time.year = _buffer.pvt.year;
609 gpsSol.time.month = _buffer.pvt.month;
610 gpsSol.time.day = _buffer.pvt.day;
611 gpsSol.time.hours = _buffer.pvt.hour;
612 gpsSol.time.minutes = _buffer.pvt.min;
613 gpsSol.time.seconds = _buffer.pvt.sec;
614 gpsSol.time.millis = _buffer.pvt.nano / (1000*1000);
615
616 gpsSol.flags.validTime = true;
617 } else {
618 gpsSol.flags.validTime = false;
619 }
620
621 _new_position = true;
622 _new_speed = true;
623 break;
624 case MSG_VER:
625 if (_class == CLASS_MON) {
626 gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion));
627 if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
628 if (_buffer.ver.swVersion[9] > '2' || true) {
629 // check extensions;
630 // after hw + sw vers; each is 30 bytes
631 bool found = false;
632 for (int j = 40; j < _payload_length && !found; j += 30)
633 {
634 // Example content: GPS;GAL;BDS;GLO
635 if (strnstr((const char *)(_buffer.bytes + j), "GAL", 30))
636 {
637 ubx_capabilities.supported |= UBX_MON_GNSS_GALILEO_MASK;
638 found = true;
639 }
640 if (strnstr((const char *)(_buffer.bytes + j), "BDS", 30))
641 {
642 ubx_capabilities.supported |= UBX_MON_GNSS_BEIDOU_MASK;
643 found = true;
644 }
645 if (strnstr((const char *)(_buffer.bytes + j), "GLO", 30))
646 {
647 ubx_capabilities.supported |= UBX_MON_GNSS_GLONASS_MASK;
648 found = true;
649 }
650 }
651 }
652 for (int j = 40; j < _payload_length; j += 30) {
653 if (strnstr((const char *)(_buffer.bytes + j), "PROTVER", 30)) {
654 gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30);
655 break;
656 }
657 }
658 }
659 }
660 break;
661 case MSG_MON_GNSS:
662 if(_class == CLASS_MON) {
663 if (_buffer.gnss.version == 0) {
664 ubx_capabilities.supported = _buffer.gnss.supported;
665 ubx_capabilities.defaultGnss = _buffer.gnss.defaultGnss;
666 ubx_capabilities.enabledGnss = _buffer.gnss.enabled;
667 ubx_capabilities.capMaxGnss = _buffer.gnss.maxConcurrent;
668 gpsState.lastCapaUpdMs = millis();
669 }
670 }
671 break;
672 case MSG_ACK_ACK:
673 if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) {
674 _ack_state = UBX_ACK_GOT_ACK;
675 }
676 break;
677 case MSG_ACK_NACK:
678 if ((_ack_state == UBX_ACK_WAITING) && (_buffer.ack.msg == _ack_waiting_msg)) {
679 _ack_state = UBX_ACK_GOT_NAK;
680 }
681 break;
682 default:
683 return false;
684 }
685
686 // we only return true when we get new position and speed data
687 // this ensures we don't use stale data
688 if (_new_position && _new_speed) {
689 _new_speed = _new_position = false;
690 return true;
691 }
692
693 return false;
694 }
695
696 static bool gpsNewFrameUBLOX(uint8_t data)
697 {
698 bool parsed = false;
699
700 switch (_step) {
701 case 0: // Sync char 1 (0xB5)
702 if (PREAMBLE1 == data) {
703 _skip_packet = false;
704 _step++;
705 }
706 break;
707 case 1: // Sync char 2 (0x62)
708 if (PREAMBLE2 != data) {
709 _step = 0;
710 break;
711 }
712 _step++;
713 break;
714 case 2: // Class
715 _step++;
716 _class = data;
717 _ck_b = _ck_a = data; // reset the checksum accumulators
718 break;
719 case 3: // Id
720 _step++;
721 _ck_b += (_ck_a += data); // checksum byte
722 _msg_id = data;
723 break;
724 case 4: // Payload length (part 1)
725 _step++;
726 _ck_b += (_ck_a += data); // checksum byte
727 _payload_length = data; // payload length low byte
728 break;
729 case 5: // Payload length (part 2)
730 _step++;
731 _ck_b += (_ck_a += data); // checksum byte
732 _payload_length |= (uint16_t)(data << 8);
733 if (_payload_length > MAX_UBLOX_PAYLOAD_SIZE ) {
734 // we can't receive the whole packet, just log the error and start searching for the next packet.
735 gpsStats.errors++;
736 _step = 0;
737 break;
738 }
739 // prepare to receive payload
740 _payload_counter = 0;
741 if (_payload_length == 0) {
742 _step = 7;
743 }
744 break;
745 case 6:
746 _ck_b += (_ck_a += data); // checksum byte
747 if (_payload_counter < MAX_UBLOX_PAYLOAD_SIZE) {
748 _buffer.bytes[_payload_counter] = data;
749 }
750 // NOTE: check counter BEFORE increasing so that a payload_size of 65535 is correctly handled. This can happen if garbage data is received.
751 if (_payload_counter == _payload_length - 1) {
752 _step++;
753 }
754 _payload_counter++;
755 break;
756 case 7:
757 _step++;
758 if (_ck_a != data) {
759 _skip_packet = true; // bad checksum
760 gpsStats.errors++;
761 _step = 0;
762 }
763 break;
764 case 8:
765 _step = 0;
766
767 if (_ck_b != data) {
768 gpsStats.errors++;
769 break; // bad checksum
770 }
771
772 gpsStats.packetCount++;
773
774 if (_skip_packet) {
775 break;
776 }
777
778 if (gpsParseFrameUBLOX()) {
779 parsed = true;
780 }
781 }
782
783 return parsed;
784 }
785
786 static uint16_t hz2rate(uint8_t hz)
787 {
788 return 1000 / hz;
789 }
790
791 STATIC_PROTOTHREAD(gpsConfigure)
792 {
793 ptBegin(gpsConfigure);
794
795 // Reset timeout
796 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
797
798 // Set dynamic model
799 switch (gpsState.gpsConfig->dynModel) {
800 case GPS_DYNMODEL_PEDESTRIAN:
801 configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
802 break;
803 case GPS_DYNMODEL_AIR_1G: // Default to this
804 default:
805 configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO);
806 break;
807 case GPS_DYNMODEL_AIR_4G:
808 configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO);
809 break;
810 }
811 ptWait(_ack_state == UBX_ACK_GOT_ACK);
812
813 // Disable NMEA messages
814 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
815
816 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GGA, 0);
817 ptWait(_ack_state == UBX_ACK_GOT_ACK);
818
819 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GLL, 0);
820 ptWait(_ack_state == UBX_ACK_GOT_ACK);
821
822 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSA, 0);
823 ptWait(_ack_state == UBX_ACK_GOT_ACK);
824
825 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_GSV, 0);
826 ptWait(_ack_state == UBX_ACK_GOT_ACK);
827
828 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_RMC, 0);
829 ptWait(_ack_state == UBX_ACK_GOT_ACK);
830
831 configureMSG(MSG_CLASS_NMEA, MSG_NMEA_VGS, 0);
832 ptWait(_ack_state == UBX_ACK_GOT_ACK);
833
834 // Configure UBX binary messages
835 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
836
837 // M9N & M10 does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence
838 if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX9) {
839 configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
840 ptWait(_ack_state == UBX_ACK_GOT_ACK);
841
842 configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
843 ptWait(_ack_state == UBX_ACK_GOT_ACK);
844
845 configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
846 ptWait(_ack_state == UBX_ACK_GOT_ACK);
847
848 configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0);
849 ptWait(_ack_state == UBX_ACK_GOT_ACK);
850
851 configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
852 ptWait(_ack_state == UBX_ACK_GOT_ACK);
853
854 configureMSG(MSG_CLASS_UBX, MSG_NAV_SAT, 0);
855 ptWait(_ack_state == UBX_ACK_GOT_ACK);
856
857 configureMSG(MSG_CLASS_UBX, MSG_NAV_SIG, 0);
858 ptWait(_ack_state == UBX_ACK_GOT_ACK);
859
860 if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) {
861 configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz
862 } else {
863 configureRATE(hz2rate(5)); // 5Hz
864 gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT;
865 }
866 ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
867
868 if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
869 configureRATE(hz2rate(5)); // 5Hz
870 ptWait(_ack_state == UBX_ACK_GOT_ACK);
871 }
872 }
873 else {
874 // u-Blox 5/6/7/8 or unknown
875 // u-Blox 7-8 support PVT
876 if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7) {
877 configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0);
878 ptWait(_ack_state == UBX_ACK_GOT_ACK);
879
880 configureMSG(MSG_CLASS_UBX, MSG_STATUS, 0);
881 ptWait(_ack_state == UBX_ACK_GOT_ACK);
882
883 configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
884 ptWait(_ack_state == UBX_ACK_GOT_ACK);
885
886 configureMSG(MSG_CLASS_UBX, MSG_VELNED, 0);
887 ptWait(_ack_state == UBX_ACK_GOT_ACK);
888
889 configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 0);
890 ptWait(_ack_state == UBX_ACK_GOT_ACK);
891
892 configureMSG(MSG_CLASS_UBX, MSG_PVT, 1);
893 ptWait(_ack_state == UBX_ACK_GOT_ACK);
894
895 configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
896 ptWait(_ack_state == UBX_ACK_GOT_ACK);
897
898 if ((gpsState.gpsConfig->provider == GPS_UBLOX7PLUS) && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) {
899 configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz
900 }
901 else {
902 configureRATE(hz2rate(5)); // 5Hz
903 }
904 ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
905
906 if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error
907 configureRATE(hz2rate(5)); // 5Hz
908 ptWait(_ack_state == UBX_ACK_GOT_ACK);
909 }
910 }
911 // u-Blox 5/6 doesn't support PVT, use legacy config
912 // UNKNOWN also falls here, use as a last resort
913 else {
914 configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 1);
915 ptWait(_ack_state == UBX_ACK_GOT_ACK);
916
917 configureMSG(MSG_CLASS_UBX, MSG_STATUS, 1);
918 ptWait(_ack_state == UBX_ACK_GOT_ACK);
919
920 configureMSG(MSG_CLASS_UBX, MSG_SOL, 1);
921 ptWait(_ack_state == UBX_ACK_GOT_ACK);
922
923 configureMSG(MSG_CLASS_UBX, MSG_VELNED, 1);
924 ptWait(_ack_state == UBX_ACK_GOT_ACK);
925
926 configureMSG(MSG_CLASS_UBX, MSG_TIMEUTC, 10);
927 ptWait(_ack_state == UBX_ACK_GOT_ACK);
928
929 // This may fail on old UBLOX units, advance forward on both ACK and NAK
930 configureMSG(MSG_CLASS_UBX, MSG_PVT, 0);
931 ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK);
932
933 configureMSG(MSG_CLASS_UBX, MSG_SVINFO, 0);
934 ptWait(_ack_state == UBX_ACK_GOT_ACK);
935
936 // Configure data rate to 5HZ
937 configureRATE(200);
938 ptWait(_ack_state == UBX_ACK_GOT_ACK);
939 }
940 }
941
942 // Configure SBAS
943 // If particular SBAS setting is not supported by the hardware we'll get a NAK,
944 // however GPS would be functional. We are waiting for any response - ACK/NACK
945 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
946 configureSBAS();
947 ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
948
949 // Configure GNSS for M8N and later
950 if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) {
951 gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
952 if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) {
953 configureGNSS10();
954 } else {
955 configureGNSS();
956 }
957 ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
958
959 if(_ack_state == UBX_ACK_GOT_NAK) {
960 gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT;
961 gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT;
962 gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT;
963 }
964 }
965
966 ptEnd(0);
967 }
968
969 static ptSemaphore_t semNewDataReady;
970
971 STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
972 {
973 ptBegin(gpsProtocolReceiverThread);
974
975 while (1) {
976 // Wait until there are bytes to consume
977 ptWait(serialRxBytesWaiting(gpsState.gpsPort));
978
979 // Consume bytes until buffer empty of until we have full message received
980 while (serialRxBytesWaiting(gpsState.gpsPort)) {
981 uint8_t newChar = serialRead(gpsState.gpsPort);
982 if (gpsNewFrameUBLOX(newChar)) {
983 ptSemaphoreSignal(semNewDataReady);
984 break;
985 }
986 }
987 }
988
989 ptEnd(0);
990 }
991
992 STATIC_PROTOTHREAD(gpsProtocolStateThread)
993 {
994 ptBegin(gpsProtocolStateThread);
995
996 // Change baud rate
997 if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) {
998 // 0. Wait for TX buffer to be empty
999 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
1000
1001 // Try sending baud rate switch command at all common baud rates
1002 gpsSetProtocolTimeout((GPS_BAUD_CHANGE_DELAY + 50) * (GPS_BAUDRATE_COUNT));
1003 for (gpsState.autoBaudrateIndex = 0; gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT; gpsState.autoBaudrateIndex++) {
1004 if (gpsBaudRateToInt(gpsState.autoBaudrateIndex) > gpsBaudRateToInt(gpsState.gpsConfig->autoBaudMax)) {
1005 // trying higher baud rates fails on m8 gps
1006 // autoBaudRateIndex is not sorted by baud rate
1007 continue;
1008 }
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]);
1012
1013 // 3. Wait for serial port to finish transmitting
1014 ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
1015
1016 // 4. Extra wait to make sure GPS processed the command
1017 ptDelayMs(GPS_BAUD_CHANGE_DELAY);
1018 }
1019 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
1020 }
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));
1025
1026 // Set baud rate and reset GPS timeout
1027 serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
1028 }
1029
1030 // Reset protocol timeout
1031 gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS)));
1032
1033 // Attempt to detect GPS hw version
1034 gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN;
1035 gpsState.autoConfigStep = 0;
1036
1037 do {
1038 pollVersion();
1039 gpsState.autoConfigStep++;
1040 ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
1041 } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
1042
1043 gpsState.autoConfigStep = 0;
1044 ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0;
1045 // M6 and earlier will never get pass this step, so skip it (#9440)
1046 if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX6) {
1047 do {
1048 pollGnssCapabilities();
1049 gpsState.autoConfigStep++;
1050 ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
1051 } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
1052 }
1053 // Configure GPS module if enabled
1054 if (gpsState.gpsConfig->autoConfig) {
1055 // Configure GPS
1056 ptSpawn(gpsConfigure);
1057 }
1058
1059 // GPS setup done, reset timeout
1060 gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
1061
1062 // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
1063 while (1) {
1064 ptSemaphoreWait(semNewDataReady);
1065 gpsProcessNewSolutionData();
1066
1067 if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
1068 if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) {
1069 gpsState.lastCapaPoolMs = millis();
1070
1071 if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN)
1072 {
1073 pollVersion();
1074 ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
1075 }
1076
1077 pollGnssCapabilities();
1078 ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
1079 }
1080 }
1081 }
1082
1083 ptEnd(0);
1084 }
1085
1086 void gpsRestartUBLOX(void)
1087 {
1088 ptSemaphoreInit(semNewDataReady);
1089 ptRestart(ptGetHandle(gpsProtocolReceiverThread));
1090 ptRestart(ptGetHandle(gpsProtocolStateThread));
1091 }
1092
1093 void gpsHandleUBLOX(void)
1094 {
1095 // Run the protocol threads
1096 gpsProtocolReceiverThread();
1097 gpsProtocolStateThread();
1098
1099 // If thread stopped - signal communication loss and restart
1100 if (ptIsStopped(ptGetHandle(gpsProtocolReceiverThread)) || ptIsStopped(ptGetHandle(gpsProtocolStateThread))) {
1101 gpsSetState(GPS_LOST_COMMUNICATION);
1102 }
1103 }
1104
1105 #endif
INAV: Navigation-enabled flight control software