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