Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : //
17 : // u-blox UBX GPS driver for ArduPilot and ArduPilotMega.
18 : // Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
19 : //
20 : // UBlox Lea6H protocol: http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
21 : #pragma once
22 :
23 : #include "AP_GPS_config.h"
24 :
25 : #if AP_GPS_UBLOX_ENABLED
26 :
27 : #include "AP_GPS.h"
28 : #include "GPS_Backend.h"
29 :
30 : #include <AP_HAL/AP_HAL.h>
31 : #if AP_GPS_UBLOX_CFGV2_ENABLED
32 : #include "AP_GPS_UBLOX_CFGv2.h"
33 : #else
34 : #include "AP_GPS_UBLOX_CFG_List.h"
35 : #endif
36 : /*
37 : * try to put a UBlox into binary mode. This is in two parts.
38 : *
39 : * First we send a ubx binary message that enables the NAV_SOL message
40 : * at rate 1. Then we send a NMEA message to set the baud rate to our
41 : * desired rate. The reason for doing the NMEA message second is if we
42 : * send it first the second message will be ignored for a baud rate
43 : * change.
44 : * The reason we need the NAV_SOL rate message at all is some uBlox
45 : * modules are configured with all ubx binary messages off, which
46 : * would mean we would never detect it.
47 :
48 : * running a uBlox at less than 38400 will lead to packet
49 : * corruption, as we can't receive the packets in the 200ms
50 : * window for 5Hz fixes. The NMEA startup message should force
51 : * the uBlox into 230400 no matter what rate it is configured
52 : * for.
53 : */
54 : #define UBLOX_SET_BINARY_115200 "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,115200,0*1C\r\n"
55 :
56 : // a variant with 230400 baudrate
57 : #define UBLOX_SET_BINARY_230400 "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,230400,0*1E\r\n"
58 :
59 : // a variant with 460800 baudrate
60 : #define UBLOX_SET_BINARY_460800 "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0023,0001,460800,0*11\r\n"
61 :
62 : #if AP_GPS_UBLOX_CFGV2_ENABLED
63 : #define UBLOX_RXM_RAW_LOGGING (HAL_LOGGING_ENABLED && (HAL_PROGRAM_SIZE_LIMIT_KB>1024))
64 : #else
65 : #define UBLOX_RXM_RAW_LOGGING 1
66 : #endif
67 : #define UBLOX_MAX_RXM_RAW_SATS 22
68 : #if AP_GPS_UBLOX_CFGV2_ENABLED
69 : #define UBLOX_MAX_RXM_RAWX_SATS 80
70 : #define UBLOX_MAX_INTERFACE_PORTS 5
71 : #else
72 : #define UBLOX_MAX_RXM_RAWX_SATS 32
73 : #endif
74 : #define UBLOX_MAX_EXTENSIONS 8
75 : #define UBLOX_GNSS_SETTINGS 1
76 : #ifndef UBLOX_TIM_TM2_LOGGING
77 : #define UBLOX_TIM_TM2_LOGGING (HAL_PROGRAM_SIZE_LIMIT_KB>1024)
78 : #endif
79 :
80 : #define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
81 :
82 : #define UBX_TIMEGPS_VALID_WEEK_MASK 0x2
83 :
84 : #define UBLOX_MAX_PORTS 6
85 : #define UBLOX_MODULE_LEN 9
86 : #define UBLOX_PROTVER_LEN 8
87 :
88 : #define CONFIG_RATE_NAV (1<<0)
89 : #define CONFIG_RATE_POSLLH (1<<1)
90 : #define CONFIG_RATE_STATUS (1<<2)
91 : #define CONFIG_RATE_SOL (1<<3)
92 : #define CONFIG_RATE_VELNED (1<<4)
93 : #define CONFIG_RATE_DOP (1<<5)
94 : #define CONFIG_RATE_MON_HW (1<<6)
95 : #define CONFIG_RATE_MON_HW2 (1<<7)
96 : #define CONFIG_RATE_RAW (1<<8)
97 : #define CONFIG_VERSION (1<<9)
98 : #define CONFIG_NAV_SETTINGS (1<<10)
99 : #define CONFIG_GNSS (1<<11)
100 : #define CONFIG_SBAS (1<<12)
101 : #define CONFIG_RATE_PVT (1<<13)
102 : #define CONFIG_TP5 (1<<14)
103 : #define CONFIG_RATE_TIMEGPS (1<<15)
104 : #define CONFIG_TMODE_MODE (1<<16)
105 : #define CONFIG_RTK_MOVBASE (1<<17)
106 : #define CONFIG_TIM_TM2 (1<<18)
107 : #define CONFIG_F9 (1<<19)
108 : #define CONFIG_M10 (1<<20)
109 : #define CONFIG_L5 (1<<21)
110 : #define CONFIG_LAST (1<<22) // this must always be the last bit
111 :
112 : #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
113 :
114 : #define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \
115 : | CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \
116 : | CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS)
117 :
118 : //Configuration Sub-Sections
119 : #define SAVE_CFG_IO (1<<0)
120 : #define SAVE_CFG_MSG (1<<1)
121 : #define SAVE_CFG_INF (1<<2)
122 : #define SAVE_CFG_NAV (1<<3)
123 : #define SAVE_CFG_RXM (1<<4)
124 : #define SAVE_CFG_RINV (1<<9)
125 : #define SAVE_CFG_ANT (1<<10)
126 : #define SAVE_CFG_ALL (SAVE_CFG_IO|SAVE_CFG_MSG|SAVE_CFG_INF|SAVE_CFG_NAV|SAVE_CFG_RXM|SAVE_CFG_RINV|SAVE_CFG_ANT)
127 :
128 : #define START_OF_WEEK_THRESHOLD_MS (60UL * AP_MSEC_PER_SEC)
129 : #define END_OF_WEEK_THRESHOLD_MS AP_MSEC_PER_WEEK - START_OF_WEEK_THRESHOLD_MS
130 :
131 : class RTCM3_Parser;
132 :
133 : class AP_GPS_UBLOX : public AP_GPS_Backend
134 : {
135 : #if AP_GPS_UBLOX_CFGV2_ENABLED
136 : using CFGv2 = AP_GPS_UBLOX_CFGv2;
137 : #endif
138 : friend class AP_GPS_UBLOX_CFGv2;
139 : public:
140 : AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role role);
141 : ~AP_GPS_UBLOX() override;
142 :
143 : // Methods
144 : bool read() override;
145 :
146 0 : AP_GPS_FixType highest_supported_status(void) override { return AP_GPS_FixType::RTK_FIXED; }
147 :
148 : static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
149 :
150 39144255 : bool is_configured(void) const override {
151 : #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
152 0 : if (!gps._auto_config) {
153 0 : return true;
154 : } else {
155 0 : return !_unconfigured_messages
156 : #if AP_GPS_UBLOX_CFGV2_ENABLED
157 0 : || (_cfg_v2.curr_state == CFGv2::CONFIGURED)
158 : #endif
159 : ;
160 : }
161 : #else
162 39144255 : return true;
163 : #endif // CONFIG_HAL_BOARD != HAL_BOARD_SITL
164 : }
165 :
166 0 : bool get_error_codes(uint32_t &error_codes) const override {
167 : #if AP_GPS_UBLOX_CFGV2_ENABLED
168 0 : if (!_legacy_cfg_supported) {
169 0 : return false;
170 : }
171 : #endif
172 0 : error_codes = _unconfigured_messages;
173 0 : return true;
174 : };
175 :
176 : void broadcast_configuration_failure_reason(void) const override;
177 : #if HAL_LOGGING_ENABLED
178 : void Write_AP_Logger_Log_Startup_messages() const override;
179 : #endif
180 :
181 : // get the velocity lag, returns true if the driver is confident in the returned value
182 : bool get_lag(float &lag_sec) const override;
183 :
184 3821 : const char *name() const override { return "u-blox"; }
185 :
186 : // support for retrieving RTCMv3 data from a moving baseline base
187 : bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len) override;
188 : void clear_RTCMV3(void) override;
189 :
190 : // ublox specific healthy checks
191 : bool is_healthy(void) const override;
192 :
193 : private:
194 : // u-blox UBX protocol essentials
195 : struct PACKED ubx_header {
196 : uint8_t preamble1;
197 : uint8_t preamble2;
198 : uint8_t msg_class;
199 : uint8_t msg_id;
200 : uint16_t length;
201 : };
202 : #if UBLOX_GNSS_SETTINGS
203 : struct PACKED ubx_cfg_gnss {
204 : uint8_t msgVer;
205 : uint8_t numTrkChHw;
206 : uint8_t numTrkChUse;
207 : uint8_t numConfigBlocks;
208 : PACKED struct configBlock {
209 : uint8_t gnssId;
210 : uint8_t resTrkCh;
211 : uint8_t maxTrkCh;
212 : uint8_t reserved1;
213 : uint32_t flags;
214 : } configBlock[UBLOX_MAX_GNSS_CONFIG_BLOCKS];
215 : };
216 : #endif
217 : struct PACKED ubx_cfg_nav_rate {
218 : uint16_t measure_rate_ms;
219 : uint16_t nav_rate;
220 : uint16_t timeref;
221 : };
222 : struct PACKED ubx_cfg_msg {
223 : uint8_t msg_class;
224 : uint8_t msg_id;
225 : };
226 : struct PACKED ubx_cfg_msg_rate {
227 : uint8_t msg_class;
228 : uint8_t msg_id;
229 : uint8_t rate;
230 : };
231 : struct PACKED ubx_cfg_msg_rate_6 {
232 : uint8_t msg_class;
233 : uint8_t msg_id;
234 : uint8_t rates[6];
235 : };
236 :
237 : #if AP_GPS_UBLOX_CFGV2_ENABLED
238 : struct PACKED ubx_cfg_rst {
239 : uint16_t navBbrMask;
240 : uint8_t resetMode;
241 : uint8_t reserved0;
242 : };
243 : #endif
244 : struct PACKED ubx_cfg_nav_settings {
245 : uint16_t mask;
246 : uint8_t dynModel;
247 : uint8_t fixMode;
248 : int32_t fixedAlt;
249 : uint32_t fixedAltVar;
250 : int8_t minElev;
251 : uint8_t drLimit;
252 : uint16_t pDop;
253 : uint16_t tDop;
254 : uint16_t pAcc;
255 : uint16_t tAcc;
256 : uint8_t staticHoldThresh;
257 : uint8_t res1;
258 : uint32_t res2;
259 : uint32_t res3;
260 : uint32_t res4;
261 : };
262 : struct PACKED ubx_cfg_tp5 {
263 : uint8_t tpIdx;
264 : uint8_t version;
265 : uint8_t reserved1[2];
266 : int16_t antCableDelay;
267 : int16_t rfGroupDelay;
268 : uint32_t freqPeriod;
269 : uint32_t freqPeriodLock;
270 : uint32_t pulseLenRatio;
271 : uint32_t pulseLenRatioLock;
272 : int32_t userConfigDelay;
273 : uint32_t flags;
274 : };
275 : struct PACKED ubx_cfg_prt {
276 : uint8_t portID;
277 : };
278 : struct PACKED ubx_cfg_sbas {
279 : uint8_t mode;
280 : uint8_t usage;
281 : uint8_t maxSBAS;
282 : uint8_t scanmode2;
283 : uint32_t scanmode1;
284 : };
285 :
286 : using ConfigKey = AP::UBXConfigKey;
287 :
288 : // layers for VALSET
289 : #define UBX_VALSET_LAYER_RAM 0x1U
290 : #define UBX_VALSET_LAYER_BBR 0x2U
291 : #define UBX_VALSET_LAYER_FLASH 0x4U
292 : #define UBX_VALSET_LAYER_ALL 0x7U
293 :
294 : struct PACKED ubx_cfg_valset {
295 : uint8_t version;
296 : uint8_t layers;
297 : uint8_t transaction;
298 : uint8_t reserved[1];
299 : uint32_t key;
300 : };
301 : struct PACKED ubx_cfg_valget {
302 : uint8_t version;
303 : uint8_t layers;
304 : #if AP_GPS_UBLOX_CFGV2_ENABLED
305 : uint16_t position;
306 : #else
307 : uint8_t reserved[2];
308 : #endif
309 : // variable length data, check buffer length
310 : };
311 :
312 : #if AP_GPS_UBLOX_CFGV2_ENABLED
313 : struct PACKED ubx_cfg_valdel {
314 : uint8_t version;
315 : uint8_t layers;
316 : uint8_t transaction;
317 : uint8_t reserved0;
318 : // variable length data (keys), check buffer length
319 : };
320 : #endif
321 : struct PACKED ubx_nav_posllh {
322 : uint32_t itow; // GPS msToW
323 : int32_t longitude;
324 : int32_t latitude;
325 : int32_t altitude_ellipsoid;
326 : int32_t altitude_msl;
327 : uint32_t horizontal_accuracy;
328 : uint32_t vertical_accuracy;
329 : };
330 : struct PACKED ubx_nav_status {
331 : uint32_t itow; // GPS msToW
332 : uint8_t fix_type;
333 : uint8_t fix_status;
334 : uint8_t differential_status;
335 : uint8_t res;
336 : uint32_t time_to_first_fix;
337 : uint32_t uptime; // milliseconds
338 : };
339 : struct PACKED ubx_nav_dop {
340 : uint32_t itow; // GPS msToW
341 : uint16_t gDOP;
342 : uint16_t pDOP;
343 : uint16_t tDOP;
344 : uint16_t vDOP;
345 : uint16_t hDOP;
346 : uint16_t nDOP;
347 : uint16_t eDOP;
348 : };
349 : struct PACKED ubx_nav_solution {
350 : uint32_t itow;
351 : int32_t time_nsec;
352 : uint16_t week;
353 : uint8_t fix_type;
354 : uint8_t fix_status;
355 : int32_t ecef_x;
356 : int32_t ecef_y;
357 : int32_t ecef_z;
358 : uint32_t position_accuracy_3d;
359 : int32_t ecef_x_velocity;
360 : int32_t ecef_y_velocity;
361 : int32_t ecef_z_velocity;
362 : uint32_t speed_accuracy;
363 : uint16_t position_DOP;
364 : uint8_t res;
365 : uint8_t satellites;
366 : uint32_t res2;
367 : };
368 : struct PACKED ubx_nav_pvt {
369 : uint32_t itow;
370 : uint16_t year;
371 : uint8_t month, day, hour, min, sec;
372 : uint8_t valid;
373 : uint32_t t_acc;
374 : int32_t nano;
375 : uint8_t fix_type;
376 : uint8_t flags;
377 : uint8_t flags2;
378 : uint8_t num_sv;
379 : int32_t lon, lat;
380 : int32_t h_ellipsoid, h_msl;
381 : uint32_t h_acc, v_acc;
382 : int32_t velN, velE, velD, gspeed;
383 : int32_t head_mot;
384 : uint32_t s_acc;
385 : uint32_t head_acc;
386 : uint16_t p_dop;
387 : uint8_t flags3;
388 : uint8_t reserved1[5];
389 : int32_t headVeh;
390 : int16_t magDec;
391 : uint16_t magAcc;
392 : };
393 : struct PACKED ubx_nav_relposned {
394 : uint8_t version;
395 : uint8_t reserved1;
396 : uint16_t refStationId;
397 : uint32_t iTOW;
398 : int32_t relPosN;
399 : int32_t relPosE;
400 : int32_t relPosD;
401 : int32_t relPosLength;
402 : int32_t relPosHeading;
403 : uint8_t reserved2[4];
404 : int8_t relPosHPN;
405 : int8_t relPosHPE;
406 : int8_t relPosHPD;
407 : int8_t relPosHPLength;
408 : uint32_t accN;
409 : uint32_t accE;
410 : uint32_t accD;
411 : uint32_t accLength;
412 : uint32_t accHeading;
413 : uint8_t reserved3[4];
414 : uint32_t flags;
415 : };
416 :
417 : struct PACKED ubx_nav_velned {
418 : uint32_t itow; // GPS msToW
419 : int32_t ned_north;
420 : int32_t ned_east;
421 : int32_t ned_down;
422 : uint32_t speed_3d;
423 : uint32_t speed_2d;
424 : int32_t heading_2d;
425 : uint32_t speed_accuracy;
426 : uint32_t heading_accuracy;
427 : };
428 :
429 : struct PACKED ubx_nav_timegps {
430 : uint32_t itow;
431 : int32_t ftow;
432 : uint16_t week;
433 : int8_t leapS;
434 : uint8_t valid; // leapsvalid | weekvalid | tow valid;
435 : uint32_t tAcc;
436 : };
437 :
438 : // Lea6 uses a 60 byte message
439 : struct PACKED ubx_mon_hw_60 {
440 : uint32_t pinSel;
441 : uint32_t pinBank;
442 : uint32_t pinDir;
443 : uint32_t pinVal;
444 : uint16_t noisePerMS;
445 : uint16_t agcCnt;
446 : uint8_t aStatus;
447 : uint8_t aPower;
448 : uint8_t flags;
449 : uint8_t reserved1;
450 : uint32_t usedMask;
451 : uint8_t VP[17];
452 : uint8_t jamInd;
453 : uint16_t reserved3;
454 : uint32_t pinIrq;
455 : uint32_t pullH;
456 : uint32_t pullL;
457 : };
458 : // Neo7 uses a 68 byte message
459 : struct PACKED ubx_mon_hw_68 {
460 : uint32_t pinSel;
461 : uint32_t pinBank;
462 : uint32_t pinDir;
463 : uint32_t pinVal;
464 : uint16_t noisePerMS;
465 : uint16_t agcCnt;
466 : uint8_t aStatus;
467 : uint8_t aPower;
468 : uint8_t flags;
469 : uint8_t reserved1;
470 : uint32_t usedMask;
471 : uint8_t VP[25];
472 : uint8_t jamInd;
473 : uint16_t reserved3;
474 : uint32_t pinIrq;
475 : uint32_t pullH;
476 : uint32_t pullL;
477 : };
478 : struct PACKED ubx_mon_hw2 {
479 : int8_t ofsI;
480 : uint8_t magI;
481 : int8_t ofsQ;
482 : uint8_t magQ;
483 : uint8_t cfgSource;
484 : uint8_t reserved0[3];
485 : uint32_t lowLevCfg;
486 : uint32_t reserved1[2];
487 : uint32_t postStatus;
488 : uint32_t reserved2;
489 : };
490 : struct PACKED ubx_mon_ver {
491 : char swVersion[30];
492 : char hwVersion[10];
493 : char extension[30*UBLOX_MAX_EXTENSIONS]; // extensions are not enabled
494 : };
495 :
496 : #if AP_GPS_UBLOX_CFGV2_ENABLED
497 : // UBX-MON-RF block (repeated nBlocks times after ubx_mon_rf header)
498 : struct PACKED ubx_mon_rf_block {
499 : uint8_t blockId; // 0
500 : uint8_t flags; // 1 (jammingState etc)
501 : uint8_t antStatus; // 2
502 : uint8_t antPower; // 3
503 : uint32_t postStatus; // 4..7
504 : uint8_t reserved1[4]; // 8..11
505 : uint16_t noisePerMS; // 12..13
506 : uint16_t agcCnt; // 14..15
507 : uint8_t cwSuppression; // 16
508 : int8_t ofsI; // 17
509 : uint8_t magI; // 18
510 : int8_t ofsQ; // 19
511 : uint8_t magQ; // 20
512 : uint8_t rfBlockGnssBand;// 21
513 : uint8_t reserved2[2]; // 22..23
514 : };
515 : struct PACKED ubx_mon_rf {
516 : uint8_t version; // 0
517 : uint8_t nBlocks; // 1
518 : uint8_t reserved0[2]; // 2..3
519 : ubx_mon_rf_block blocks[2]; // provision for up to 2 RF blocks
520 : };
521 : #endif
522 : struct PACKED ubx_nav_svinfo_header {
523 : uint32_t itow;
524 : uint8_t numCh;
525 : uint8_t globalFlags;
526 : uint16_t reserved;
527 : };
528 : #if UBLOX_RXM_RAW_LOGGING
529 : struct PACKED ubx_rxm_raw {
530 : int32_t iTOW;
531 : int16_t week;
532 : uint8_t numSV;
533 : uint8_t reserved1;
534 : struct ubx_rxm_raw_sv {
535 : double cpMes;
536 : double prMes;
537 : float doMes;
538 : uint8_t sv;
539 : int8_t mesQI;
540 : int8_t cno;
541 : uint8_t lli;
542 : } svinfo[UBLOX_MAX_RXM_RAW_SATS];
543 : };
544 : struct PACKED ubx_rxm_rawx {
545 : double rcvTow;
546 : uint16_t week;
547 : int8_t leapS;
548 : uint8_t numMeas;
549 : uint8_t recStat;
550 : uint8_t reserved1[3];
551 : PACKED struct ubx_rxm_rawx_sv {
552 : double prMes;
553 : double cpMes;
554 : float doMes;
555 : uint8_t gnssId;
556 : uint8_t svId;
557 : uint8_t reserved2;
558 : uint8_t freqId;
559 : uint16_t locktime;
560 : uint8_t cno;
561 : uint8_t prStdev;
562 : uint8_t cpStdev;
563 : uint8_t doStdev;
564 : uint8_t trkStat;
565 : uint8_t reserved3;
566 : } svinfo[UBLOX_MAX_RXM_RAWX_SATS];
567 : };
568 : #endif
569 :
570 : struct PACKED ubx_ack_ack {
571 : uint8_t clsID;
572 : uint8_t msgID;
573 : };
574 : struct PACKED ubx_ack_nack {
575 : uint8_t clsID;
576 : uint8_t msgID;
577 : };
578 :
579 :
580 : struct PACKED ubx_cfg_cfg {
581 : uint32_t clearMask;
582 : uint32_t saveMask;
583 : uint32_t loadMask;
584 : };
585 :
586 : struct PACKED ubx_tim_tm2 {
587 : uint8_t ch;
588 : uint8_t flags;
589 : uint16_t count;
590 : uint16_t wnR;
591 : uint16_t wnF;
592 : uint32_t towMsR;
593 : uint32_t towSubMsR;
594 : uint32_t towMsF;
595 : uint32_t towSubMsF;
596 : uint32_t accEst;
597 : };
598 :
599 : #if AP_GPS_UBLOX_CFGV2_ENABLED
600 : // UBX-MON-COMMS structures (per UBX spec)
601 : struct PACKED ubx_mon_comms_port_block {
602 : uint16_t portId; // 8 + n*40
603 : uint16_t txPending; // 10 + n*40
604 : uint32_t txBytes; // 12 + n*40
605 : uint8_t txUsage; // 16 + n*40
606 : uint8_t txPeakUsage; // 17 + n*40
607 : uint16_t rxPending; // 18 + n*40
608 : uint32_t rxBytes; // 20 + n*40
609 : uint8_t rxUsage; // 24 + n*40
610 : uint8_t rxPeakUsage; // 25 + n*40
611 : uint16_t overrunErrs; // 26 + n*40
612 : uint16_t msgs[4]; // 28 + n*40
613 : uint8_t reserved1[8]; // 36 + n*40
614 : uint32_t skipped; // 44 + n*40 (but within block size 40; spec describes as bytes skipped)
615 : };
616 : struct PACKED ubx_mon_comms_header {
617 : uint8_t version; // 0
618 : uint8_t nPorts; // 1
619 : uint8_t txErrors; // 2 (bits 4..2 = outputPort)
620 : uint8_t reserved0; // 3
621 : uint8_t protIds[4]; // 4..7
622 : // followed by ubx_mon_comms_port_block[nPorts]
623 : ubx_mon_comms_port_block port_block[UBLOX_MAX_INTERFACE_PORTS];
624 : };
625 : #endif
626 :
627 : // Receive buffer
628 : union PACKED {
629 257539721 : DEFINE_BYTE_ARRAY_METHODS
630 : ubx_nav_posllh posllh;
631 : ubx_nav_status status;
632 : ubx_nav_dop dop;
633 : ubx_nav_solution solution;
634 : ubx_nav_pvt pvt;
635 : ubx_nav_timegps timegps;
636 : ubx_nav_velned velned;
637 : ubx_cfg_msg_rate msg_rate;
638 : ubx_cfg_msg_rate_6 msg_rate_6;
639 : ubx_cfg_nav_settings nav_settings;
640 : ubx_cfg_nav_rate nav_rate;
641 : ubx_cfg_prt prt;
642 : ubx_mon_hw_60 mon_hw_60;
643 : ubx_mon_hw_68 mon_hw_68;
644 : ubx_mon_hw2 mon_hw2;
645 : ubx_mon_ver mon_ver;
646 : #if AP_GPS_UBLOX_CFGV2_ENABLED
647 : ubx_mon_rf mon_rf;
648 : #endif
649 : ubx_cfg_tp5 nav_tp5;
650 : #if UBLOX_GNSS_SETTINGS
651 : ubx_cfg_gnss gnss;
652 : #endif
653 : ubx_cfg_sbas sbas;
654 : ubx_cfg_valget valget;
655 : ubx_nav_svinfo_header svinfo_header;
656 : ubx_nav_relposned relposned;
657 : #if UBLOX_RXM_RAW_LOGGING
658 : ubx_rxm_raw rxm_raw;
659 : ubx_rxm_rawx rxm_rawx;
660 : #endif
661 : ubx_ack_ack ack;
662 : ubx_ack_nack nack;
663 : ubx_tim_tm2 tim_tm2;
664 : #if AP_GPS_UBLOX_CFGV2_ENABLED
665 : ubx_mon_comms_header mon_comms;
666 : #endif
667 : } _buffer;
668 :
669 : enum class RELPOSNED {
670 : gnssFixOK = 1U << 0,
671 : diffSoln = 1U << 1,
672 : relPosValid = 1U << 2,
673 : carrSolnFloat = 1U << 3,
674 :
675 : carrSolnFixed = 1U << 4,
676 : isMoving = 1U << 5,
677 : refPosMiss = 1U << 6,
678 : refObsMiss = 1U << 7,
679 :
680 : relPosHeadingValid = 1U << 8,
681 : relPosNormalized = 1U << 9
682 : };
683 :
684 : enum ubs_protocol_bytes {
685 : PREAMBLE1 = 0xb5,
686 : PREAMBLE2 = 0x62,
687 : CLASS_NAV = 0x01,
688 : CLASS_ACK = 0x05,
689 : CLASS_CFG = 0x06,
690 : CLASS_MON = 0x0A,
691 : CLASS_RXM = 0x02,
692 : CLASS_TIM = 0x0d,
693 : MSG_ACK_NACK = 0x00,
694 : MSG_ACK_ACK = 0x01,
695 : MSG_POSLLH = 0x2,
696 : MSG_STATUS = 0x3,
697 : MSG_DOP = 0x4,
698 : MSG_SOL = 0x6,
699 : MSG_PVT = 0x7,
700 : MSG_TIMEGPS = 0x20,
701 : MSG_RELPOSNED = 0x3c,
702 : MSG_VELNED = 0x12,
703 : MSG_CFG_CFG = 0x09,
704 : MSG_CFG_RATE = 0x08,
705 : MSG_CFG_MSG = 0x01,
706 : #if AP_GPS_UBLOX_CFGV2_ENABLED
707 : MSG_CFG_RST = 0x04,
708 : #endif
709 : MSG_CFG_NAV_SETTINGS = 0x24,
710 : MSG_CFG_PRT = 0x00,
711 : MSG_CFG_SBAS = 0x16,
712 : MSG_CFG_GNSS = 0x3E,
713 : MSG_CFG_TP5 = 0x31,
714 : MSG_CFG_VALSET = 0x8A,
715 : MSG_CFG_VALGET = 0x8B,
716 : #if AP_GPS_UBLOX_CFGV2_ENABLED
717 : MSG_CFG_VALDEL = 0x8C,
718 : #endif
719 : MSG_MON_HW = 0x09,
720 : MSG_MON_HW2 = 0x0B,
721 : #if AP_GPS_UBLOX_CFGV2_ENABLED
722 : MSG_MON_RF = 0x38,
723 : MSG_MON_COMMS = 0x36,
724 : #endif
725 : MSG_MON_VER = 0x04,
726 : MSG_NAV_SVINFO = 0x30,
727 : MSG_RXM_RAW = 0x10,
728 : MSG_RXM_RAWX = 0x15,
729 : MSG_TIM_TM2 = 0x03
730 : };
731 :
732 : enum ubx_gnss_identifier {
733 : GNSS_GPS = 0x00,
734 : GNSS_SBAS = 0x01,
735 : GNSS_GAL = 0x02,
736 : GNSS_BDS = 0x03,
737 : GNSS_IMES = 0x04,
738 : GNSS_QZSS = 0x05,
739 : GNSS_GLO = 0x06,
740 : GNSS_NAVIC = 0x07,
741 : #if AP_GPS_UBLOX_CFGV2_ENABLED
742 : GNSS_LAST
743 : #endif
744 : };
745 :
746 : #if AP_GPS_UBLOX_CFGV2_ENABLED
747 : #define UBLOX_GNSS_TYPE_ASSERT(NAME, INDEX) \
748 : static_assert(AP_GPS_UBLOX::GNSS_##NAME == INDEX, "GNSS_" #NAME " has incorrect index");
749 : UBLOX_GNSS(UBLOX_GNSS_TYPE_ASSERT)
750 : #undef UBLOX_GNSS_TYPE_ASSERT
751 : #endif
752 :
753 : enum ubs_nav_fix_type {
754 : FIX_NONE = 0,
755 : FIX_DEAD_RECKONING = 1,
756 : FIX_2D = 2,
757 : FIX_3D = 3,
758 : FIX_GPS_DEAD_RECKONING = 4,
759 : FIX_TIME = 5
760 : };
761 : enum ubx_nav_status_bits {
762 : NAV_STATUS_FIX_VALID = 1,
763 : NAV_STATUS_DGPS_USED = 2
764 : };
765 : #if AP_GPS_UBLOX_CFGV2_ENABLED
766 : enum ubx_reset_type {
767 : RESET_HW_WATCHDOG_IMMEDIATE = 0x00,
768 : RESET_SW_CONTROLLED = 0x01,
769 : RESET_SW_GNSS_ONLY = 0x02,
770 : RESET_HW_WATCHDOG_AFTER_SHUTDOWN = 0x04,
771 : RESET_GNSS_STOP = 0x08,
772 : RESET_GNSS_START = 0x09,
773 : RESET_HW_PWSEQ_BBR_RETAINED = 0x0a
774 : };
775 : #endif
776 : enum ubx_hardware_version {
777 : ANTARIS = 0,
778 : UBLOX_5,
779 : UBLOX_6,
780 : UBLOX_7,
781 : UBLOX_M8,
782 : UBLOX_F9 = 0x80, // comes from MON_VER hwVersion/swVersion strings
783 : UBLOX_M9 = 0x81, // comes from MON_VER hwVersion/swVersion strings
784 : UBLOX_M10 = 0x82,
785 : #if AP_GPS_UBLOX_CFGV2_ENABLED
786 : UBLOX_F10 = 0x83,
787 : UBLOX_F20 = 0x85,
788 : UBLOX_X20 = 0x84,
789 : #endif
790 : UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for
791 : // flagging state in the driver
792 : };
793 :
794 : enum ubx_hardware_variant {
795 : UBLOX_F9_ZED, // comes from MON_VER extension strings
796 : UBLOX_F9_NEO, // comes from MON_VER extension strings
797 : UBLOX_UNKNOWN_HARDWARE_VARIANT = 0xff
798 : };
799 :
800 : enum config_step {
801 : STEP_PVT = 0,
802 : STEP_NAV_RATE, // poll NAV rate
803 : STEP_SOL,
804 : STEP_PORT,
805 : STEP_STATUS,
806 : STEP_POSLLH,
807 : STEP_VELNED,
808 : STEP_TIMEGPS,
809 : STEP_POLL_SVINFO, // poll svinfo
810 : STEP_POLL_SBAS, // poll SBAS
811 : STEP_POLL_NAV, // poll NAV settings
812 : STEP_POLL_GNSS, // poll GNSS
813 : STEP_POLL_TP5, // poll TP5
814 : STEP_TMODE, // set TMODE-MODE
815 : STEP_DOP,
816 : STEP_MON_HW,
817 : STEP_MON_HW2,
818 : STEP_RAW,
819 : STEP_RAWX,
820 : STEP_VERSION,
821 : STEP_RTK_MOVBASE, // setup moving baseline
822 : STEP_TIM_TM2,
823 : STEP_F9,
824 : STEP_F9_VALIDATE,
825 : STEP_M10,
826 : STEP_L5,
827 : STEP_LAST
828 : };
829 :
830 : // Packet checksum accumulators
831 : uint8_t _ck_a;
832 : uint8_t _ck_b;
833 :
834 : // State machine state
835 : uint8_t _step;
836 : uint8_t _msg_id;
837 : uint16_t _payload_length;
838 : uint16_t _payload_counter;
839 :
840 : uint8_t _class;
841 : bool _cfg_saved;
842 :
843 : uint32_t _last_vel_time;
844 : uint32_t _last_pos_time;
845 : uint32_t _last_cfg_sent_time;
846 : uint8_t _num_cfg_save_tries;
847 : uint32_t _last_config_time;
848 : uint32_t _f9_config_time;
849 : uint16_t _delay_time;
850 : uint8_t _next_message { STEP_PVT };
851 : uint8_t _ublox_port { 255 };
852 : bool _have_version;
853 : struct ubx_mon_ver _version;
854 : char _module[UBLOX_MODULE_LEN];
855 : #if AP_GPS_UBLOX_CFGV2_ENABLED
856 : char _protver[UBLOX_PROTVER_LEN];
857 : #endif
858 : uint32_t _unconfigured_messages {CONFIG_ALL};
859 : uint8_t _hardware_generation { UBLOX_UNKNOWN_HARDWARE_GENERATION };
860 : uint8_t _hardware_variant;
861 : uint32_t _last_pvt_itow;
862 : uint32_t _last_relposned_itow;
863 : uint32_t _last_relposned_ms;
864 :
865 : // the role set from GPS_TYPE
866 : AP_GPS::GPS_Role role;
867 :
868 : // do we have new position information?
869 : bool _new_position:1;
870 : // do we have new speed information?
871 : bool _new_speed:1;
872 :
873 : uint8_t _disable_counter;
874 :
875 : // Buffer parse & GPS state update
876 : bool _parse_gps();
877 : #if AP_GPS_UBLOX_CFGV2_ENABLED
878 : bool _legacy_config_update(void);
879 : bool _legacy_cfg_supported = true;
880 : #endif
881 : // used to update fix between status and position packets
882 : AP_GPS_FixType next_fix { AP_GPS_FixType::NONE };
883 :
884 : bool _cfg_needs_save;
885 :
886 : bool noReceivedHdop { true };
887 :
888 : bool havePvtMsg;
889 :
890 : using config_list = ubx_config_list;
891 :
892 : bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
893 : bool _configure_valset(ConfigKey key, const void *value, uint8_t layers=UBX_VALSET_LAYER_ALL);
894 : bool _configure_list_valset(const config_list *list, uint8_t count, uint8_t layers=UBX_VALSET_LAYER_ALL);
895 : bool _configure_valget(ConfigKey key);
896 : void _configure_rate(void);
897 : static void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);
898 : bool _send_message(uint8_t msg_class, uint8_t msg_id, const void *msg, uint16_t size);
899 : bool _request_message_rate(uint8_t msg_class, uint8_t msg_id);
900 : void _request_next_config(void);
901 : void _request_port(void);
902 : void _request_version(void);
903 : void _save_cfg(void);
904 : void _verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
905 : void _check_new_itow(uint32_t itow);
906 :
907 : void unexpected_message(void);
908 : void log_mon_hw(void);
909 : void log_mon_hw2(void);
910 : #if HAL_LOGGING_ENABLED && AP_GPS_UBLOX_CFGV2_ENABLED
911 : void log_mon_rf(void);
912 : #endif
913 : void log_tim_tm2(void);
914 : void log_rxm_raw(const struct ubx_rxm_raw &raw);
915 : void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
916 :
917 : #if GPS_MOVING_BASELINE
918 : // see if we should use uart2 for moving baseline config
919 8 : bool mb_use_uart2(void) const {
920 8 : return option_set(AP_GPS::DriverOptions::UBX_MBUseUart2)?true:false;
921 : }
922 : #endif
923 :
924 : // return size of a config key payload
925 : uint8_t config_key_size(ConfigKey key) const;
926 :
927 : // configure a set of config key/value pairs. The unconfig_bit corresponds to
928 : // a bit in _unconfigured_messages
929 : bool _configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit, uint8_t layers=UBX_VALSET_LAYER_ALL);
930 :
931 : // find index in active_config list
932 : int8_t find_active_config_index(ConfigKey key) const;
933 :
934 : // return true if GPS is capable of F9 config
935 : bool supports_F9_config(void) const;
936 :
937 : // is the config key a GNSS key
938 : bool is_gnss_key(ConfigKey key) const;
939 :
940 : // populate config_GNSS for F9P
941 : uint8_t populate_F9_gnss(void);
942 : uint8_t last_configured_gnss;
943 :
944 : uint8_t _pps_freq = 1;
945 : #ifdef HAL_GPIO_PPS
946 : void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us);
947 : void set_pps_desired_freq(uint8_t freq) override;
948 : #endif
949 :
950 : // status of active configuration for a role
951 : struct {
952 : const config_list *list;
953 : uint8_t count;
954 : uint32_t done_mask;
955 : uint32_t unconfig_bit;
956 : uint8_t layers;
957 : int8_t fetch_index;
958 : int8_t set_index;
959 : } active_config;
960 : bool use_single_valget;
961 :
962 : #if GPS_MOVING_BASELINE
963 : // RTCM3 parser for when in moving baseline base mode
964 : RTCM3_Parser *rtcm3_parser;
965 : #endif // GPS_MOVING_BASELINE
966 :
967 : bool supports_l5;
968 : // scratch space for GNSS config
969 : config_list* config_GNSS;
970 : #if AP_GPS_UBLOX_CFGV2_ENABLED
971 : AP_GPS_UBLOX_CFGv2 _cfg_v2;
972 : #endif
973 : };
974 :
975 : #endif
|
