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 GPS driver for ArduPilot
18 : // Origin code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
19 : // Substantially rewritten for new GPS driver structure by Andrew Tridgell
20 : //
21 : #include "AP_GPS_UBLOX.h"
22 :
23 : #if AP_GPS_UBLOX_ENABLED
24 :
25 : #if AP_GPS_UBLOX_CFGV2_ENABLED
26 : #include "AP_GPS_UBLOX_CFGv2.h"
27 : #endif
28 :
29 : #include "AP_GPS.h"
30 : #include <AP_HAL/Util.h>
31 : #include <AP_Logger/AP_Logger.h>
32 : #include <GCS_MAVLink/GCS.h>
33 : #include "RTCM3_Parser.h"
34 : #include <stdio.h>
35 :
36 : #ifndef UBLOX_SPEED_CHANGE
37 : #define UBLOX_SPEED_CHANGE 0
38 : #endif
39 :
40 :
41 : #define UBLOX_DEBUGGING 0
42 : #define UBLOX_FAKE_3DLOCK 0
43 : #ifndef CONFIGURE_PPS_PIN
44 : #define CONFIGURE_PPS_PIN 0
45 : #endif
46 :
47 : // this is number of epochs per output. A higher value will reduce
48 : // the uart bandwidth needed and allow for higher latency
49 : #define RTK_MB_RTCM_RATE 1
50 :
51 : // use this to enable debugging of moving baseline configs
52 : #define UBLOX_MB_DEBUGGING 0
53 :
54 : // debug VALGET/VALSET configuration
55 : #define UBLOX_CFG_DEBUGGING 0
56 :
57 : // debug CFGv2 configuration
58 : #define UBLOX_CFGV2_DEBUGGING 0
59 :
60 : extern const AP_HAL::HAL& hal;
61 :
62 : #if UBLOX_DEBUGGING
63 : #if defined(HAL_BUILD_AP_PERIPH)
64 : extern "C" {
65 : void can_printf(const char *fmt, ...);
66 : }
67 : # define Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
68 : #else
69 : # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
70 : #endif
71 : #else
72 : # define Debug(fmt, args ...)
73 : #endif
74 :
75 : #if UBLOX_MB_DEBUGGING
76 : #if defined(HAL_BUILD_AP_PERIPH)
77 : extern "C" {
78 : void can_printf(const char *fmt, ...);
79 : }
80 : # define MB_Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
81 : #else
82 : # define MB_Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
83 : #endif
84 : #else
85 : # define MB_Debug(fmt, args ...)
86 : #endif
87 :
88 : #if UBLOX_CFG_DEBUGGING
89 : #if defined(HAL_BUILD_AP_PERIPH)
90 : extern "C" {
91 : void can_printf(const char *fmt, ...);
92 : }
93 : # define CFG_Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
94 : #else
95 : # define CFG_Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
96 : #endif
97 : #else
98 : # define CFG_Debug(fmt, args ...)
99 : #endif
100 :
101 : #if UBLOX_CFGV2_DEBUGGING
102 : #if defined(HAL_BUILD_AP_PERIPH)
103 : extern "C" {
104 : void can_printf(const char *fmt, ...);
105 : }
106 : # define CFGv2_Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
107 : #else
108 : # define CFGv2_Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
109 : #endif
110 : #else
111 : # define CFGv2_Debug(fmt, args ...)
112 : #endif
113 :
114 1023 : AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps,
115 : AP_GPS::Params &_params,
116 : AP_GPS::GPS_State &_state,
117 : AP_HAL::UARTDriver *_port,
118 1023 : AP_GPS::GPS_Role _role) :
119 : AP_GPS_Backend(_gps, _params, _state, _port),
120 1023 : role(_role)
121 : #if AP_GPS_UBLOX_CFGV2_ENABLED
122 1023 : ,_cfg_v2(*this)
123 : #endif
124 : {
125 : // stop any config strings that are pending
126 1023 : gps.send_blob_start(state.instance, nullptr, 0);
127 :
128 : // start the process of updating the GPS rates
129 1023 : _request_next_config();
130 :
131 : #if CONFIGURE_PPS_PIN
132 : _unconfigured_messages |= CONFIG_TP5;
133 : #endif
134 :
135 : #if GPS_MOVING_BASELINE
136 1023 : if (role == AP_GPS::GPS_ROLE_MB_BASE && !mb_use_uart2()) {
137 8 : rtcm3_parser = NEW_NOTHROW RTCM3_Parser;
138 8 : if (rtcm3_parser == nullptr) {
139 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "u-blox %d: failed RTCMv3 parser allocation", state.instance + 1);
140 : }
141 8 : _unconfigured_messages |= CONFIG_RTK_MOVBASE;
142 : }
143 1023 : if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
144 8 : _unconfigured_messages |= CONFIG_RTK_MOVBASE;
145 8 : state.gps_yaw_configured = true;
146 : }
147 : #endif
148 1023 : }
149 :
150 6 : AP_GPS_UBLOX::~AP_GPS_UBLOX()
151 : {
152 : #if GPS_MOVING_BASELINE
153 3 : delete rtcm3_parser;
154 : #endif
155 :
156 3 : free(config_GNSS);
157 6 : }
158 :
159 : void
160 692829 : AP_GPS_UBLOX::_request_next_config(void)
161 : {
162 : // don't request config if we shouldn't configure the GPS
163 692829 : if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
164 4688 : return;
165 : }
166 :
167 : // Ensure there is enough space for the largest possible outgoing message
168 688141 : if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
169 : // not enough space - do it next time
170 0 : return;
171 : }
172 :
173 688141 : if ((_unconfigured_messages & CONFIG_RATE_SOL) != 0 && havePvtMsg) {
174 : /*
175 : we don't need SOL if we have PVT and TIMEGPS. This is needed
176 : as F9P doesn't support the SOL message
177 : */
178 1008 : _unconfigured_messages &= ~CONFIG_RATE_SOL;
179 : }
180 :
181 : Debug("Unconfigured messages: 0x%x Current message: %u\n", (unsigned)_unconfigured_messages, (unsigned)_next_message);
182 :
183 : // check AP_GPS_UBLOX.h for the enum that controls the order.
184 : // This switch statement isn't maintained against the enum in order to reduce code churn
185 688141 : switch (_next_message++) {
186 688141 : case STEP_PVT:
187 688141 : if(!_request_message_rate(CLASS_NAV, MSG_PVT)) {
188 688141 : _next_message--;
189 : }
190 688141 : break;
191 0 : case STEP_TIMEGPS:
192 0 : if(!_request_message_rate(CLASS_NAV, MSG_TIMEGPS)) {
193 0 : _next_message--;
194 : }
195 0 : break;
196 0 : case STEP_PORT:
197 0 : _request_port();
198 0 : break;
199 0 : case STEP_POLL_SVINFO:
200 : // not required once we know what generation we are on
201 0 : if(_hardware_generation == UBLOX_UNKNOWN_HARDWARE_GENERATION) {
202 0 : if (!_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) {
203 0 : _next_message--;
204 : }
205 : }
206 0 : break;
207 0 : case STEP_POLL_SBAS:
208 0 : if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
209 0 : _send_message(CLASS_CFG, MSG_CFG_SBAS, nullptr, 0);
210 : } else {
211 0 : _unconfigured_messages &= ~CONFIG_SBAS;
212 : }
213 0 : break;
214 0 : case STEP_POLL_NAV:
215 0 : if (!_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, nullptr, 0)) {
216 0 : _next_message--;
217 : }
218 0 : break;
219 0 : case STEP_POLL_GNSS:
220 0 : if (supports_F9_config()) {
221 0 : if (last_configured_gnss != params.gnss_mode) {
222 0 : _unconfigured_messages |= CONFIG_F9;
223 : }
224 0 : break;
225 : }
226 0 : if (!_send_message(CLASS_CFG, MSG_CFG_GNSS, nullptr, 0)) {
227 0 : _next_message--;
228 : }
229 0 : break;
230 0 : case STEP_POLL_TP5:
231 : #if CONFIGURE_PPS_PIN
232 : if (!_send_message(CLASS_CFG, MSG_CFG_TP5, nullptr, 0)) {
233 : _next_message--;
234 : }
235 : #endif
236 0 : break;
237 0 : case STEP_NAV_RATE:
238 0 : if (!_send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0)) {
239 0 : _next_message--;
240 : }
241 0 : break;
242 0 : case STEP_POSLLH:
243 0 : if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
244 0 : _next_message--;
245 : }
246 0 : break;
247 0 : case STEP_STATUS:
248 0 : if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) {
249 0 : _next_message--;
250 : }
251 0 : break;
252 0 : case STEP_SOL:
253 0 : if(!_request_message_rate(CLASS_NAV, MSG_SOL)) {
254 0 : _next_message--;
255 : }
256 0 : break;
257 0 : case STEP_VELNED:
258 0 : if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) {
259 0 : _next_message--;
260 : }
261 0 : break;
262 0 : case STEP_DOP:
263 0 : if(! _request_message_rate(CLASS_NAV, MSG_DOP)) {
264 0 : _next_message--;
265 : }
266 0 : break;
267 0 : case STEP_MON_HW:
268 0 : if(!_request_message_rate(CLASS_MON, MSG_MON_HW)) {
269 0 : _next_message--;
270 : }
271 0 : break;
272 0 : case STEP_MON_HW2:
273 0 : if(!_request_message_rate(CLASS_MON, MSG_MON_HW2)) {
274 0 : _next_message--;
275 : }
276 0 : break;
277 0 : case STEP_RAW:
278 : #if UBLOX_RXM_RAW_LOGGING
279 0 : if(gps._raw_data == 0) {
280 0 : _unconfigured_messages &= ~CONFIG_RATE_RAW;
281 0 : } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAW)) {
282 0 : _next_message--;
283 : }
284 : #else
285 : _unconfigured_messages &= ~CONFIG_RATE_RAW;
286 : #endif
287 0 : break;
288 0 : case STEP_RAWX:
289 : #if UBLOX_RXM_RAW_LOGGING
290 0 : if(gps._raw_data == 0) {
291 0 : _unconfigured_messages &= ~CONFIG_RATE_RAW;
292 0 : } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAWX)) {
293 0 : _next_message--;
294 : }
295 : #else
296 : _unconfigured_messages &= ~CONFIG_RATE_RAW;
297 : #endif
298 0 : break;
299 0 : case STEP_VERSION:
300 0 : if(!_have_version && !hal.util->get_soft_armed()) {
301 0 : _request_version();
302 : } else {
303 0 : _unconfigured_messages &= ~CONFIG_VERSION;
304 : }
305 0 : break;
306 0 : case STEP_TMODE:
307 0 : if (supports_F9_config()) {
308 0 : if (!_configure_valget(ConfigKey::CFG_TMODE_MODE)) {
309 0 : _next_message--;
310 : }
311 : }
312 0 : break;
313 0 : case STEP_RTK_MOVBASE:
314 : #if GPS_MOVING_BASELINE
315 0 : if (supports_F9_config()) {
316 : static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart1), "done_mask too small, base1");
317 : static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart2), "done_mask too small, base2");
318 : static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart1), "done_mask too small, rover1");
319 : static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart2), "done_mask too small, rover2");
320 0 : if (role == AP_GPS::GPS_ROLE_MB_BASE) {
321 0 : const config_list *list = mb_use_uart2()?config_MB_Base_uart2:config_MB_Base_uart1;
322 0 : uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Base_uart2):ARRAY_SIZE(config_MB_Base_uart1);
323 0 : if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) {
324 0 : _next_message--;
325 : }
326 : }
327 0 : if (role == AP_GPS::GPS_ROLE_MB_ROVER) {
328 0 : const config_list *list = mb_use_uart2()?config_MB_Rover_uart2:config_MB_Rover_uart1;
329 0 : uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Rover_uart2):ARRAY_SIZE(config_MB_Rover_uart1);
330 0 : if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) {
331 0 : _next_message--;
332 : }
333 : }
334 : }
335 : #endif
336 0 : break;
337 0 : case STEP_TIM_TM2:
338 : #if UBLOX_TIM_TM2_LOGGING
339 0 : if(!_request_message_rate(CLASS_TIM, MSG_TIM_TM2)) {
340 0 : _next_message--;
341 : }
342 : #else
343 : _unconfigured_messages &= ~CONFIG_TIM_TM2;
344 : #endif
345 0 : break;
346 :
347 0 : case STEP_F9: {
348 0 : if (_hardware_generation == UBLOX_F9 ||
349 0 : _hardware_generation == UBLOX_M10) {
350 0 : uint8_t cfg_count = populate_F9_gnss();
351 : // special handling of F9 config
352 0 : if (cfg_count > 0) {
353 : CFG_Debug("Sending F9 settings, GNSS=%u", unsigned(params.gnss_mode));
354 :
355 0 : if (!_configure_list_valset(config_GNSS, cfg_count, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
356 0 : _next_message--;
357 0 : break;
358 : }
359 0 : _f9_config_time = AP_HAL::millis();
360 : }
361 : }
362 0 : break;
363 : }
364 :
365 0 : case STEP_F9_VALIDATE: {
366 0 : if (_hardware_generation == UBLOX_F9 ||
367 0 : _hardware_generation == UBLOX_M10) {
368 : // GNSS takes 0.5s to reset
369 0 : if (AP_HAL::millis() - _f9_config_time < 500) {
370 0 : _next_message--;
371 0 : break;
372 : }
373 0 : _f9_config_time = 0;
374 0 : uint8_t cfg_count = populate_F9_gnss();
375 : // special handling of F9 config
376 0 : if (cfg_count > 0) {
377 : CFG_Debug("Validating F9 settings, GNSS=%u", unsigned(params.gnss_mode));
378 : // now validate all of the settings, this is a no-op if the first call succeeded
379 0 : if (!_configure_config_set(config_GNSS, cfg_count, CONFIG_F9, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
380 0 : _next_message--;
381 : }
382 : }
383 : }
384 0 : break;
385 : }
386 0 : case STEP_M10: {
387 0 : if (_hardware_generation == UBLOX_M10) {
388 : // special handling of M10 config
389 0 : const config_list *list = config_M10;
390 0 : const uint8_t list_length = ARRAY_SIZE(config_M10);
391 : Debug("Sending M10 settings");
392 0 : if (!_configure_config_set(list, list_length, CONFIG_M10, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
393 0 : _next_message--;
394 : }
395 : }
396 0 : break;
397 : }
398 :
399 0 : case STEP_L5: {
400 0 : if (supports_l5 && option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) {
401 0 : const config_list *list = config_L5_ovrd_ena;
402 0 : const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_ena);
403 0 : if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
404 0 : _next_message--;
405 : }
406 0 : } else if (supports_l5 && !option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) {
407 0 : const config_list *list = config_L5_ovrd_dis;
408 0 : const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_dis);
409 0 : if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) {
410 0 : _next_message--;
411 : }
412 : }
413 0 : break;
414 : }
415 :
416 0 : default:
417 : // this case should never be reached, do a full reset if it is hit
418 0 : _next_message = STEP_PVT;
419 0 : break;
420 : }
421 : }
422 :
423 : void
424 0 : AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
425 : uint8_t desired_rate;
426 : uint32_t config_msg_id;
427 0 : switch(msg_class) {
428 0 : case CLASS_NAV:
429 0 : switch(msg_id) {
430 0 : case MSG_POSLLH:
431 0 : desired_rate = havePvtMsg ? 0 : RATE_POSLLH;
432 0 : config_msg_id = CONFIG_RATE_POSLLH;
433 0 : break;
434 0 : case MSG_STATUS:
435 0 : desired_rate = havePvtMsg ? 0 : RATE_STATUS;
436 0 : config_msg_id = CONFIG_RATE_STATUS;
437 0 : break;
438 0 : case MSG_SOL:
439 0 : desired_rate = havePvtMsg ? 0 : RATE_SOL;
440 0 : config_msg_id = CONFIG_RATE_SOL;
441 0 : break;
442 0 : case MSG_PVT:
443 0 : desired_rate = RATE_PVT;
444 0 : config_msg_id = CONFIG_RATE_PVT;
445 0 : break;
446 0 : case MSG_TIMEGPS:
447 0 : desired_rate = RATE_TIMEGPS;
448 0 : config_msg_id = CONFIG_RATE_TIMEGPS;
449 0 : break;
450 0 : case MSG_VELNED:
451 0 : desired_rate = havePvtMsg ? 0 : RATE_VELNED;
452 0 : config_msg_id = CONFIG_RATE_VELNED;
453 0 : break;
454 0 : case MSG_DOP:
455 0 : desired_rate = RATE_DOP;
456 0 : config_msg_id = CONFIG_RATE_DOP;
457 0 : break;
458 0 : default:
459 0 : return;
460 : }
461 0 : break;
462 0 : case CLASS_MON:
463 0 : switch(msg_id) {
464 0 : case MSG_MON_HW:
465 0 : desired_rate = RATE_HW;
466 0 : config_msg_id = CONFIG_RATE_MON_HW;
467 0 : break;
468 0 : case MSG_MON_HW2:
469 0 : desired_rate = RATE_HW2;
470 0 : config_msg_id = CONFIG_RATE_MON_HW2;
471 0 : break;
472 0 : default:
473 0 : return;
474 : }
475 0 : break;
476 : #if UBLOX_RXM_RAW_LOGGING
477 0 : case CLASS_RXM:
478 0 : switch(msg_id) {
479 0 : case MSG_RXM_RAW:
480 0 : desired_rate = gps._raw_data;
481 0 : config_msg_id = CONFIG_RATE_RAW;
482 0 : break;
483 0 : case MSG_RXM_RAWX:
484 0 : desired_rate = gps._raw_data;
485 0 : config_msg_id = CONFIG_RATE_RAW;
486 0 : break;
487 0 : default:
488 0 : return;
489 : }
490 0 : break;
491 : #endif // UBLOX_RXM_RAW_LOGGING
492 : #if UBLOX_TIM_TM2_LOGGING
493 0 : case CLASS_TIM:
494 0 : if (msg_id == MSG_TIM_TM2) {
495 0 : desired_rate = RATE_TIM_TM2;
496 0 : config_msg_id = CONFIG_TIM_TM2;
497 0 : break;
498 : }
499 0 : return;
500 : #endif // UBLOX_TIM_TM2_LOGGING
501 0 : default:
502 0 : return;
503 : }
504 :
505 0 : if (rate == desired_rate) {
506 : // coming in at correct rate; mark as configured
507 0 : _unconfigured_messages &= ~config_msg_id;
508 0 : return;
509 : }
510 :
511 : // coming in at wrong rate; try to configure it
512 0 : _configure_message_rate(msg_class, msg_id, desired_rate);
513 0 : _unconfigured_messages |= config_msg_id;
514 0 : _cfg_needs_save = true;
515 : }
516 :
517 : // Requests the ublox driver to identify what port we are using to communicate
518 : void
519 688141 : AP_GPS_UBLOX::_request_port(void)
520 : {
521 688141 : if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+2)) {
522 : // not enough space - do it next time
523 0 : return;
524 : }
525 688141 : _send_message(CLASS_CFG, MSG_CFG_PRT, nullptr, 0);
526 : }
527 :
528 : // Ensure there is enough space for the largest possible outgoing message
529 : // Process bytes available from the stream
530 : //
531 : // The stream is assumed to contain only messages we recognise. If it
532 : // contains other messages, and those messages contain the preamble
533 : // bytes, it is possible for this code to fail to synchronise to the
534 : // stream immediately. Without buffering the entire message and
535 : // re-processing it from the top, this is unavoidable. The parser
536 : // attempts to avoid this when possible.
537 : //
538 : bool
539 6888164 : AP_GPS_UBLOX::read(void)
540 : {
541 6888164 : bool parsed = false;
542 6888164 : uint32_t millis_now = AP_HAL::millis();
543 : #if AP_GPS_UBLOX_CFGV2_ENABLED
544 6888164 : _cfg_v2.update();
545 : #endif
546 : // walk through the gps configuration at 1 message per second
547 13776328 : if (millis_now - _last_config_time >= _delay_time
548 : #if AP_GPS_UBLOX_CFGV2_ENABLED
549 6888164 : && _cfg_v2.using_legacy_config()
550 : #endif
551 : ) {
552 691806 : _request_next_config();
553 691806 : _last_config_time = millis_now;
554 691806 : if (_unconfigured_messages) {
555 : // send the updates faster until fully configured
556 691806 : _delay_time = 200;
557 : } else {
558 0 : _delay_time = 2000;
559 : }
560 : }
561 :
562 : #if 0
563 : // this can be modified to force a particular config state for
564 : // state machine config debugging
565 : static bool done_force_config_error;
566 : if (!_unconfigured_messages && !done_force_config_error) {
567 : done_force_config_error = true;
568 : _unconfigured_messages = CONFIG_F9 | CONFIG_RATE_SOL;
569 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Forcing config state 0x%04x", unsigned(_unconfigured_messages));
570 : }
571 : #endif
572 :
573 0 : if(!_unconfigured_messages && gps._save_config && !_cfg_saved &&
574 6888164 : _num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 &&
575 0 : !hal.util->get_soft_armed()) {
576 : //save the configuration sent until now
577 0 : if (gps._save_config == 1 ||
578 0 : (gps._save_config == 2 && _cfg_needs_save)) {
579 0 : _save_cfg();
580 : }
581 : }
582 :
583 6888164 : const uint16_t numc = MIN(port->available(), 8192U);
584 303399151 : for (uint16_t i = 0; i < numc; i++) { // Process bytes received
585 :
586 : // read the next byte
587 : uint8_t data;
588 296510987 : if (!port->read(data)) {
589 0 : break;
590 : }
591 : #if AP_GPS_DEBUG_LOGGING_ENABLED
592 : log_data(&data, 1);
593 : #endif
594 :
595 : #if GPS_MOVING_BASELINE
596 296510987 : if (rtcm3_parser) {
597 1180068 : if (rtcm3_parser->read(data)) {
598 : // we've found a RTCMv3 packet. We stop parsing at
599 : // this point and reset u-blox parse state. We need to
600 : // stop parsing to give the higher level driver a
601 : // chance to send the RTCMv3 packet to another (rover)
602 : // GPS
603 0 : _step = 0;
604 0 : break;
605 : }
606 : }
607 : #endif
608 :
609 295330919 : reset:
610 : #if AP_GPS_UBLOX_CFGV2_ENABLED
611 296510990 : if (_step == 0) {
612 : // reset the valget state machine
613 4875112 : _cfg_v2.process_valget_complete(false);
614 : }
615 : #endif
616 296510990 : switch(_step) {
617 :
618 : // Message preamble detection
619 : //
620 : // If we fail to match any of the expected bytes, we reset
621 : // the state machine and re-consider the failed byte as
622 : // the first byte of the preamble. This improves our
623 : // chances of recovering from a mismatch and makes it less
624 : // likely that we will be fooled by the preamble appearing
625 : // as data in some other message.
626 : //
627 4870880 : case 1:
628 4870880 : if (PREAMBLE2 == data) {
629 4870880 : _step++;
630 4870880 : break;
631 : }
632 0 : _step = 0;
633 : Debug("reset %u", __LINE__);
634 : FALLTHROUGH;
635 4875112 : case 0:
636 4875112 : if(PREAMBLE1 == data)
637 4870880 : _step++;
638 4875112 : break;
639 :
640 : // Message header processing
641 : //
642 : // We sniff the class and message ID to decide whether we
643 : // are going to gather the message bytes or just discard
644 : // them.
645 : //
646 : // We always collect the length so that we can avoid being
647 : // fooled by preamble bytes in messages.
648 : //
649 4870880 : case 2:
650 4870880 : _step++;
651 4870880 : _class = data;
652 4870880 : _ck_b = _ck_a = data; // reset the checksum accumulators
653 4870880 : break;
654 4870880 : case 3:
655 4870880 : _step++;
656 4870880 : _ck_b += (_ck_a += data); // checksum byte
657 4870880 : _msg_id = data;
658 4870880 : break;
659 4870880 : case 4:
660 4870880 : _step++;
661 4870880 : _ck_b += (_ck_a += data); // checksum byte
662 4870880 : _payload_length = data; // payload length low byte
663 4870880 : break;
664 4870880 : case 5:
665 4870880 : _step++;
666 4870880 : _ck_b += (_ck_a += data); // checksum byte
667 :
668 4870880 : _payload_length += (uint16_t)(data<<8);
669 4870880 : if ((_payload_length > sizeof(_buffer))
670 : #if AP_GPS_UBLOX_CFGV2_ENABLED
671 0 : && !(_class == CLASS_CFG || _msg_id == MSG_CFG_VALGET)
672 : #endif
673 : ) {
674 : Debug("large payload %u", (unsigned)_payload_length);
675 : // assume any payload bigger then what we know about is noise
676 0 : _payload_length = 0;
677 0 : _step = 0;
678 0 : goto reset;
679 : }
680 4870880 : _payload_counter = 0; // prepare to receive payload
681 4870880 : if (_payload_length == 0) {
682 : // bypass payload and go straight to checksum
683 0 : _step++;
684 : }
685 4870880 : break;
686 :
687 : // Receive message data
688 : //
689 257539721 : case 6:
690 257539721 : _ck_b += (_ck_a += data); // checksum byte
691 : #if AP_GPS_UBLOX_CFGV2_ENABLED
692 257539721 : if (_class == CLASS_CFG && _msg_id == MSG_CFG_VALGET) {
693 : CFGv2_Debug("V2 VALGET byte %u/%u: 0x%02x\n", (unsigned)_payload_counter, (unsigned)_payload_length, data);
694 0 : _cfg_v2.process_valget_byte(data);
695 : }
696 : #endif
697 257539721 : if (_payload_counter < sizeof(_buffer)) {
698 257539721 : _buffer[_payload_counter] = data;
699 : }
700 257539721 : if (++_payload_counter == _payload_length)
701 4870880 : _step++;
702 257539721 : break;
703 :
704 : // Checksum and message processing
705 : //
706 4870880 : case 7:
707 4870880 : _step++;
708 4870880 : if (_ck_a != data) {
709 : Debug("bad cka %x should be %x", data, _ck_a);
710 3 : _step = 0;
711 : #if AP_GPS_UBLOX_CFGV2_ENABLED
712 3 : if (_class == CLASS_CFG && _msg_id == MSG_CFG_VALGET) {
713 0 : _cfg_v2.process_valget_complete(false);
714 : }
715 : #endif
716 3 : goto reset;
717 : }
718 4870877 : break;
719 4870877 : case 8:
720 4870877 : _step = 0;
721 4870877 : if (_ck_b != data) {
722 : Debug("bad ckb %x should be %x", data, _ck_b);
723 : #if AP_GPS_UBLOX_CFGV2_ENABLED
724 0 : if (_class == CLASS_CFG && _msg_id == MSG_CFG_VALGET) {
725 0 : _cfg_v2.process_valget_complete(false);
726 : }
727 : #endif
728 0 : break; // bad checksum
729 : }
730 :
731 : #if GPS_MOVING_BASELINE
732 4870877 : if (rtcm3_parser) {
733 : // this is a uBlox packet, discard any partial RTCMv3 state
734 18962 : rtcm3_parser->reset();
735 : }
736 : #endif
737 : #if AP_GPS_UBLOX_CFGV2_ENABLED
738 4870877 : if (_class == CLASS_CFG && _msg_id == MSG_CFG_VALGET) {
739 0 : _cfg_v2.process_valget_complete(true);
740 : }
741 : #endif
742 4870877 : if (_parse_gps()) {
743 695319 : parsed = true;
744 : }
745 4870877 : break;
746 : }
747 : }
748 6888164 : return parsed;
749 : }
750 :
751 : // Private Methods /////////////////////////////////////////////////////////////
752 0 : void AP_GPS_UBLOX::log_mon_hw(void)
753 : {
754 : #if HAL_LOGGING_ENABLED
755 0 : if (!should_log()) {
756 0 : return;
757 : }
758 0 : struct log_Ubx1 pkt = {
759 : LOG_PACKET_HEADER_INIT(LOG_GPS_UBX1_MSG),
760 0 : time_us : AP_HAL::micros64(),
761 0 : instance : state.instance,
762 0 : noisePerMS : _buffer.mon_hw_60.noisePerMS,
763 0 : jamInd : _buffer.mon_hw_60.jamInd,
764 0 : aPower : _buffer.mon_hw_60.aPower,
765 0 : agcCnt : _buffer.mon_hw_60.agcCnt,
766 0 : config : _unconfigured_messages,
767 0 : };
768 0 : if (_payload_length == 68) {
769 0 : pkt.noisePerMS = _buffer.mon_hw_68.noisePerMS;
770 0 : pkt.jamInd = _buffer.mon_hw_68.jamInd;
771 0 : pkt.aPower = _buffer.mon_hw_68.aPower;
772 0 : pkt.agcCnt = _buffer.mon_hw_68.agcCnt;
773 : }
774 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
775 : #endif
776 : }
777 :
778 0 : void AP_GPS_UBLOX::log_mon_hw2(void)
779 : {
780 : #if HAL_LOGGING_ENABLED
781 0 : if (!should_log()) {
782 0 : return;
783 : }
784 :
785 0 : struct log_Ubx2 pkt = {
786 : LOG_PACKET_HEADER_INIT(LOG_GPS_UBX2_MSG),
787 0 : time_us : AP_HAL::micros64(),
788 0 : instance : state.instance,
789 0 : ofsI : _buffer.mon_hw2.ofsI,
790 0 : magI : _buffer.mon_hw2.magI,
791 0 : ofsQ : _buffer.mon_hw2.ofsQ,
792 0 : magQ : _buffer.mon_hw2.magQ,
793 0 : };
794 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
795 : #endif
796 : }
797 :
798 : #if HAL_LOGGING_ENABLED && AP_GPS_UBLOX_CFGV2_ENABLED
799 0 : void AP_GPS_UBLOX::log_mon_rf(void)
800 : {
801 0 : if (!should_log()) {
802 0 : return;
803 : }
804 : // Use structured buffer
805 0 : const ubx_mon_rf &rf = _buffer.mon_rf;
806 0 : const uint8_t version = rf.version;
807 0 : const uint8_t nBlocks = rf.nBlocks;
808 0 : if (version != 0 || nBlocks == 0) {
809 0 : return;
810 : }
811 0 : const ubx_mon_rf_block &blk0 = rf.blocks[0];
812 :
813 : // Fill UBX1 with RF noise and AGC-like info
814 0 : const struct log_Ubx1 pkt1 {
815 : LOG_PACKET_HEADER_INIT(LOG_GPS_UBX1_MSG),
816 0 : time_us : AP_HAL::micros64(),
817 0 : instance : state.instance,
818 0 : noisePerMS : blk0.noisePerMS,
819 0 : jamInd : uint8_t(blk0.flags & 0x03U), // jammingState per spec (0..3)
820 0 : aPower : blk0.antPower,
821 0 : agcCnt : blk0.agcCnt,
822 : config : 0, // not used for CFGv2
823 0 : };
824 0 : AP::logger().WriteBlock(&pkt1, sizeof(pkt1));
825 :
826 : // Fill UBX2 with I/Q imbalance and magnitudes
827 0 : const struct log_Ubx2 pkt2 {
828 : LOG_PACKET_HEADER_INIT(LOG_GPS_UBX2_MSG),
829 0 : time_us : AP_HAL::micros64(),
830 0 : instance : state.instance,
831 0 : ofsI : blk0.ofsI,
832 0 : magI : blk0.magI,
833 0 : ofsQ : blk0.ofsQ,
834 0 : magQ : blk0.magQ,
835 0 : };
836 0 : AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
837 : }
838 : #endif // HAL_LOGGING_ENABLED && AP_GPS_UBLOX_CFGV2_ENABLED
839 :
840 : #if UBLOX_TIM_TM2_LOGGING
841 0 : void AP_GPS_UBLOX::log_tim_tm2(void)
842 : {
843 : #if HAL_LOGGING_ENABLED
844 0 : if (!should_log()) {
845 0 : return;
846 : }
847 :
848 : // @LoggerMessage: UBXT
849 : // @Description: uBlox specific UBX-TIM-TM2 logging, see uBlox interface description
850 : // @Field: TimeUS: Time since system startup
851 : // @Field: I: GPS instance number
852 : // @Field: ch: Channel (i.e. EXTINT) upon which the pulse was measured
853 : // @Field: flags: Bitmask
854 : // @Field: count: Rising edge counter
855 : // @Field: wnR: Week number of last rising edge
856 : // @Field: MsR: Tow of rising edge
857 : // @Field: SubMsR: Millisecond fraction of tow of rising edge in nanoseconds
858 : // @Field: wnF: Week number of last falling edge
859 : // @Field: MsF: Tow of falling edge
860 : // @Field: SubMsF: Millisecond fraction of tow of falling edge in nanoseconds
861 : // @Field: accEst: Accuracy estimate
862 :
863 0 : AP::logger().WriteStreaming("UBXT",
864 : "TimeUS,I,ch,flags,count,wnR,MsR,SubMsR,wnF,MsF,SubMsF,accEst",
865 : "s#----ss-sss",
866 : "F-----CI-CII",
867 : "QBBBHHIIHIII",
868 : AP_HAL::micros64(),
869 0 : state.instance,
870 0 : _buffer.tim_tm2.ch,
871 0 : _buffer.tim_tm2.flags,
872 0 : _buffer.tim_tm2.count,
873 0 : _buffer.tim_tm2.wnR,
874 : _buffer.tim_tm2.towMsR,
875 : _buffer.tim_tm2.towSubMsR,
876 0 : _buffer.tim_tm2.wnF,
877 : _buffer.tim_tm2.towMsF,
878 : _buffer.tim_tm2.towSubMsF,
879 : _buffer.tim_tm2.accEst);
880 : #endif
881 : }
882 : #endif // UBLOX_TIM_TM2_LOGGING
883 :
884 : #if UBLOX_RXM_RAW_LOGGING
885 0 : void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
886 : {
887 : #if HAL_LOGGING_ENABLED
888 0 : if (!should_log()) {
889 0 : return;
890 : }
891 :
892 0 : uint64_t now = AP_HAL::micros64();
893 0 : for (uint8_t i=0; i<raw.numSV; i++) {
894 0 : struct log_GPS_RAW pkt = {
895 : LOG_PACKET_HEADER_INIT(LOG_GPS_RAW_MSG),
896 : time_us : now,
897 0 : iTOW : raw.iTOW,
898 0 : week : raw.week,
899 0 : numSV : raw.numSV,
900 0 : sv : raw.svinfo[i].sv,
901 0 : cpMes : raw.svinfo[i].cpMes,
902 0 : prMes : raw.svinfo[i].prMes,
903 0 : doMes : raw.svinfo[i].doMes,
904 0 : mesQI : raw.svinfo[i].mesQI,
905 0 : cno : raw.svinfo[i].cno,
906 0 : lli : raw.svinfo[i].lli
907 0 : };
908 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
909 : }
910 : #endif
911 : }
912 :
913 0 : void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
914 : {
915 : #if HAL_LOGGING_ENABLED
916 0 : if (!should_log()) {
917 0 : return;
918 : }
919 :
920 0 : uint64_t now = AP_HAL::micros64();
921 :
922 0 : struct log_GPS_RAWH header = {
923 : LOG_PACKET_HEADER_INIT(LOG_GPS_RAWH_MSG),
924 : time_us : now,
925 0 : rcvTow : raw.rcvTow,
926 0 : week : raw.week,
927 0 : leapS : raw.leapS,
928 0 : numMeas : raw.numMeas,
929 0 : recStat : raw.recStat
930 0 : };
931 0 : AP::logger().WriteBlock(&header, sizeof(header));
932 :
933 0 : for (uint8_t i=0; i<raw.numMeas; i++) {
934 0 : struct log_GPS_RAWS pkt = {
935 : LOG_PACKET_HEADER_INIT(LOG_GPS_RAWS_MSG),
936 : time_us : now,
937 0 : prMes : raw.svinfo[i].prMes,
938 0 : cpMes : raw.svinfo[i].cpMes,
939 0 : doMes : raw.svinfo[i].doMes,
940 0 : gnssId : raw.svinfo[i].gnssId,
941 0 : svId : raw.svinfo[i].svId,
942 0 : freqId : raw.svinfo[i].freqId,
943 0 : locktime : raw.svinfo[i].locktime,
944 0 : cno : raw.svinfo[i].cno,
945 0 : prStdev : raw.svinfo[i].prStdev,
946 0 : cpStdev : raw.svinfo[i].cpStdev,
947 0 : doStdev : raw.svinfo[i].doStdev,
948 0 : trkStat : raw.svinfo[i].trkStat
949 0 : };
950 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
951 : }
952 : #endif
953 : }
954 : #endif // UBLOX_RXM_RAW_LOGGING
955 :
956 0 : void AP_GPS_UBLOX::unexpected_message(void)
957 : {
958 : Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
959 0 : if (++_disable_counter == 0) {
960 : // disable future sends of this message id, but
961 : // only do this every 256 messages, as some
962 : // message types can't be disabled and we don't
963 : // want to get into an ack war
964 : Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
965 0 : _configure_message_rate(_class, _msg_id, 0);
966 : }
967 0 : }
968 :
969 : // return size of a config key, or 0 if unknown
970 0 : uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const
971 : {
972 : // step over the value
973 0 : const uint8_t key_size = (uint32_t(key) >> 28) & 0x07; // mask off the storage size
974 0 : switch (key_size) {
975 0 : case 0x1: // bit
976 : case 0x2: // byte
977 0 : return 1;
978 0 : case 0x3: // 2 bytes
979 0 : return 2;
980 0 : case 0x4: // 4 bytes
981 0 : return 4;
982 0 : case 0x5: // 8 bytes
983 0 : return 8;
984 0 : default:
985 0 : break;
986 : }
987 : // invalid
988 0 : return 0;
989 : }
990 :
991 : /*
992 : find index of a config key in the active_config list, or -1
993 : */
994 0 : int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const
995 : {
996 0 : if (active_config.list == nullptr) {
997 0 : return -1;
998 : }
999 0 : for (uint8_t i=0; i<active_config.count; i++) {
1000 0 : if (key == active_config.list[i].key) {
1001 0 : return (int8_t)i;
1002 : }
1003 : }
1004 :
1005 0 : return -1;
1006 : }
1007 :
1008 : bool
1009 : #if AP_GPS_UBLOX_CFGV2_ENABLED
1010 4870877 : AP_GPS_UBLOX::_legacy_config_update(void)
1011 : #else
1012 : AP_GPS_UBLOX::_parse_gps(void)
1013 : #endif
1014 : {
1015 4870877 : if (_class == CLASS_ACK) {
1016 : Debug("ACK %u", (unsigned)_msg_id);
1017 :
1018 0 : if(_msg_id == MSG_ACK_ACK) {
1019 0 : switch(_buffer.ack.clsID) {
1020 0 : case CLASS_CFG:
1021 0 : switch(_buffer.ack.msgID) {
1022 0 : case MSG_CFG_CFG:
1023 0 : _cfg_saved = true;
1024 0 : _cfg_needs_save = false;
1025 0 : break;
1026 0 : case MSG_CFG_GNSS:
1027 0 : _unconfigured_messages &= ~CONFIG_GNSS;
1028 0 : break;
1029 0 : case MSG_CFG_MSG:
1030 : // There is no way to know what MSG config was ack'ed, assume it was the last
1031 : // one requested. To verify it rerequest the last config we sent. If we miss
1032 : // the actual ack we will catch it next time through the poll loop, but that
1033 : // will be a good chunk of time later.
1034 0 : break;
1035 0 : case MSG_CFG_NAV_SETTINGS:
1036 0 : _unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
1037 0 : break;
1038 0 : case MSG_CFG_RATE:
1039 : // The GPS will ACK a update rate that is invalid. in order to detect this
1040 : // only accept the rate as configured by reading the settings back and
1041 : // validating that they all match the target values
1042 0 : break;
1043 0 : case MSG_CFG_SBAS:
1044 0 : _unconfigured_messages &= ~CONFIG_SBAS;
1045 0 : break;
1046 0 : case MSG_CFG_TP5:
1047 0 : _unconfigured_messages &= ~CONFIG_TP5;
1048 0 : break;
1049 : }
1050 :
1051 0 : break;
1052 0 : case CLASS_MON:
1053 0 : switch(_buffer.ack.msgID) {
1054 0 : case MSG_MON_HW:
1055 0 : _unconfigured_messages &= ~CONFIG_RATE_MON_HW;
1056 0 : break;
1057 0 : case MSG_MON_HW2:
1058 0 : _unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
1059 0 : break;
1060 : }
1061 : }
1062 : }
1063 0 : if(_msg_id == MSG_ACK_NACK) {
1064 0 : switch(_buffer.nack.clsID) {
1065 0 : case CLASS_CFG:
1066 0 : switch(_buffer.nack.msgID) {
1067 0 : case MSG_CFG_VALGET:
1068 : CFG_Debug("NACK VALGET 0x%x", (unsigned)_buffer.nack.msgID);
1069 0 : if (active_config.list != nullptr) {
1070 : /*
1071 : likely this device does not support fetching multiple keys at once, go one at a time
1072 : */
1073 0 : if (active_config.fetch_index == -1) {
1074 : CFG_Debug("NACK starting %u", unsigned(active_config.count));
1075 0 : active_config.fetch_index = 0;
1076 0 : use_single_valget = true;
1077 : } else {
1078 : // the device does not support the config key we asked for,
1079 : // consider the bit as done
1080 0 : active_config.done_mask |= (1U<<active_config.fetch_index);
1081 : CFG_Debug("NACK %d 0x%x done=0x%x",
1082 : int(active_config.fetch_index),
1083 : unsigned(active_config.list[active_config.fetch_index].key),
1084 : unsigned(active_config.done_mask));
1085 0 : if (active_config.done_mask == (1U<<active_config.count)-1 ||
1086 0 : active_config.fetch_index >= active_config.count) {
1087 : // all done!
1088 0 : _unconfigured_messages &= ~active_config.unconfig_bit;
1089 : }
1090 0 : active_config.fetch_index++;
1091 : }
1092 0 : if (active_config.fetch_index < active_config.count) {
1093 0 : _configure_valget(active_config.list[active_config.fetch_index].key);
1094 : }
1095 : }
1096 0 : break;
1097 0 : case MSG_CFG_VALSET:
1098 0 : if (active_config.list != nullptr) {
1099 : CFG_Debug("NACK VALSET 0x%x 0x%x", (unsigned)_buffer.nack.msgID,
1100 : unsigned(active_config.list[active_config.set_index].key));
1101 0 : if (is_gnss_key(active_config.list[active_config.set_index].key)) {
1102 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: unable to configure band 0x%02x",
1103 : unsigned(state.instance + 1), unsigned(active_config.list[active_config.set_index].key));
1104 :
1105 : }
1106 : }
1107 0 : break;
1108 : }
1109 : }
1110 : }
1111 0 : return false;
1112 : }
1113 :
1114 4870877 : if (_class == CLASS_CFG) {
1115 0 : switch(_msg_id) {
1116 0 : case MSG_CFG_NAV_SETTINGS:
1117 : Debug("Got settings %u min_elev %d drLimit %u\n",
1118 : (unsigned)_buffer.nav_settings.dynModel,
1119 : (int)_buffer.nav_settings.minElev,
1120 : (unsigned)_buffer.nav_settings.drLimit);
1121 0 : _buffer.nav_settings.mask = 0;
1122 0 : if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
1123 0 : _buffer.nav_settings.dynModel != gps._navfilter) {
1124 : // we've received the current nav settings, change the engine
1125 : // settings and send them back
1126 : Debug("Changing engine setting from %u to %u\n",
1127 : (unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
1128 0 : _buffer.nav_settings.dynModel = gps._navfilter;
1129 0 : _buffer.nav_settings.mask |= 1;
1130 : }
1131 0 : if (gps._min_elevation != -100 &&
1132 0 : _buffer.nav_settings.minElev != gps._min_elevation) {
1133 : Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
1134 0 : _buffer.nav_settings.minElev = gps._min_elevation;
1135 0 : _buffer.nav_settings.mask |= 2;
1136 : }
1137 0 : if (_buffer.nav_settings.mask != 0) {
1138 0 : _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
1139 0 : &_buffer.nav_settings,
1140 : sizeof(_buffer.nav_settings));
1141 0 : _unconfigured_messages |= CONFIG_NAV_SETTINGS;
1142 0 : _cfg_needs_save = true;
1143 : } else {
1144 0 : _unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
1145 : }
1146 0 : return false;
1147 :
1148 : #if UBLOX_GNSS_SETTINGS
1149 0 : case MSG_CFG_GNSS:
1150 0 : if (params.gnss_mode != 0 && !supports_F9_config()) {
1151 0 : struct ubx_cfg_gnss start_gnss = _buffer.gnss;
1152 0 : uint8_t gnssCount = 0;
1153 : Debug("Got GNSS Settings %u %u %u %u:\n",
1154 : (unsigned)_buffer.gnss.msgVer,
1155 : (unsigned)_buffer.gnss.numTrkChHw,
1156 : (unsigned)_buffer.gnss.numTrkChUse,
1157 : (unsigned)_buffer.gnss.numConfigBlocks);
1158 : #if UBLOX_DEBUGGING
1159 : for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
1160 : Debug(" %u %u %u 0x%08x\n",
1161 : (unsigned)_buffer.gnss.configBlock[i].gnssId,
1162 : (unsigned)_buffer.gnss.configBlock[i].resTrkCh,
1163 : (unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
1164 : (unsigned)_buffer.gnss.configBlock[i].flags);
1165 : }
1166 : #endif
1167 :
1168 0 : for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
1169 0 : if((params.gnss_mode & (1 << i)) && i != GNSS_SBAS) {
1170 0 : gnssCount++;
1171 : }
1172 : }
1173 0 : for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
1174 : // Reserve an equal portion of channels for all enabled systems that supports it
1175 0 : if(params.gnss_mode & (1 << _buffer.gnss.configBlock[i].gnssId)) {
1176 0 : if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId && (_hardware_generation > UBLOX_M8 || GNSS_GAL !=_buffer.gnss.configBlock[i].gnssId)) {
1177 0 : _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
1178 0 : _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
1179 : } else {
1180 0 : if(GNSS_SBAS ==_buffer.gnss.configBlock[i].gnssId) {
1181 0 : _buffer.gnss.configBlock[i].resTrkCh = 1;
1182 0 : _buffer.gnss.configBlock[i].maxTrkCh = 3;
1183 : }
1184 0 : if(GNSS_GAL ==_buffer.gnss.configBlock[i].gnssId) {
1185 0 : _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
1186 0 : _buffer.gnss.configBlock[i].maxTrkCh = 8; //Per the M8 receiver description UBX-13003221 - R16, 4.1.1.3 it is not recommended to set the number of galileo channels higher then eight
1187 : }
1188 : }
1189 0 : _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
1190 : } else {
1191 0 : _buffer.gnss.configBlock[i].resTrkCh = 0;
1192 0 : _buffer.gnss.configBlock[i].maxTrkCh = 0;
1193 0 : _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
1194 : }
1195 : }
1196 0 : if (memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
1197 0 : _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
1198 0 : _unconfigured_messages |= CONFIG_GNSS;
1199 0 : _cfg_needs_save = true;
1200 : } else {
1201 0 : _unconfigured_messages &= ~CONFIG_GNSS;
1202 : }
1203 : } else {
1204 0 : _unconfigured_messages &= ~CONFIG_GNSS;
1205 : }
1206 0 : return false;
1207 : #endif
1208 :
1209 0 : case MSG_CFG_SBAS:
1210 0 : if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) {
1211 : Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n",
1212 : (unsigned)_buffer.sbas.mode,
1213 : (unsigned)_buffer.sbas.usage,
1214 : (unsigned)_buffer.sbas.maxSBAS,
1215 : (unsigned)_buffer.sbas.scanmode2,
1216 : (unsigned)_buffer.sbas.scanmode1);
1217 0 : if (_buffer.sbas.mode != gps._sbas_mode) {
1218 0 : _buffer.sbas.mode = gps._sbas_mode;
1219 0 : _send_message(CLASS_CFG, MSG_CFG_SBAS,
1220 0 : &_buffer.sbas,
1221 : sizeof(_buffer.sbas));
1222 0 : _unconfigured_messages |= CONFIG_SBAS;
1223 0 : _cfg_needs_save = true;
1224 : } else {
1225 0 : _unconfigured_messages &= ~CONFIG_SBAS;
1226 : }
1227 : } else {
1228 0 : _unconfigured_messages &= ~CONFIG_SBAS;
1229 : }
1230 0 : return false;
1231 0 : case MSG_CFG_MSG:
1232 0 : if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) {
1233 : // can't verify the setting without knowing the port
1234 : // request the port again
1235 0 : if(_ublox_port >= UBLOX_MAX_PORTS) {
1236 0 : _request_port();
1237 0 : return false;
1238 : }
1239 0 : _verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id,
1240 0 : _buffer.msg_rate_6.rates[_ublox_port]);
1241 : } else {
1242 0 : _verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id,
1243 0 : _buffer.msg_rate.rate);
1244 : }
1245 0 : return false;
1246 0 : case MSG_CFG_PRT:
1247 0 : _ublox_port = _buffer.prt.portID;
1248 0 : return false;
1249 0 : case MSG_CFG_RATE:
1250 0 : if(_buffer.nav_rate.measure_rate_ms != params.rate_ms ||
1251 0 : _buffer.nav_rate.nav_rate != 1 ||
1252 0 : _buffer.nav_rate.timeref != 0) {
1253 0 : _configure_rate();
1254 0 : _unconfigured_messages |= CONFIG_RATE_NAV;
1255 0 : _cfg_needs_save = true;
1256 : } else {
1257 0 : _unconfigured_messages &= ~CONFIG_RATE_NAV;
1258 : }
1259 0 : return false;
1260 :
1261 : #if CONFIGURE_PPS_PIN
1262 : case MSG_CFG_TP5: {
1263 : // configure the PPS pin for 1Hz, zero delay
1264 : Debug("Got TP5 ver=%u 0x%04x %u\n",
1265 : (unsigned)_buffer.nav_tp5.version,
1266 : (unsigned)_buffer.nav_tp5.flags,
1267 : (unsigned)_buffer.nav_tp5.freqPeriod);
1268 : #ifdef HAL_GPIO_PPS
1269 : hal.gpio->attach_interrupt(HAL_GPIO_PPS, FUNCTOR_BIND_MEMBER(&AP_GPS_UBLOX::pps_interrupt, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_FALLING);
1270 : #endif
1271 : const uint16_t desired_flags = 0x003f;
1272 : const uint16_t desired_period_hz = _pps_freq;
1273 :
1274 : if (_buffer.nav_tp5.flags != desired_flags ||
1275 : _buffer.nav_tp5.freqPeriod != desired_period_hz) {
1276 : _buffer.nav_tp5.tpIdx = 0;
1277 : _buffer.nav_tp5.reserved1[0] = 0;
1278 : _buffer.nav_tp5.reserved1[1] = 0;
1279 : _buffer.nav_tp5.antCableDelay = 0;
1280 : _buffer.nav_tp5.rfGroupDelay = 0;
1281 : _buffer.nav_tp5.freqPeriod = desired_period_hz;
1282 : _buffer.nav_tp5.freqPeriodLock = desired_period_hz;
1283 : _buffer.nav_tp5.pulseLenRatio = 1;
1284 : _buffer.nav_tp5.pulseLenRatioLock = 2;
1285 : _buffer.nav_tp5.userConfigDelay = 0;
1286 : _buffer.nav_tp5.flags = desired_flags;
1287 : _send_message(CLASS_CFG, MSG_CFG_TP5,
1288 : &_buffer.nav_tp5,
1289 : sizeof(_buffer.nav_tp5));
1290 : _unconfigured_messages |= CONFIG_TP5;
1291 : _cfg_needs_save = true;
1292 : } else {
1293 : _unconfigured_messages &= ~CONFIG_TP5;
1294 : }
1295 : return false;
1296 : }
1297 : #endif // CONFIGURE_PPS_PIN
1298 0 : case MSG_CFG_VALGET: {
1299 0 : uint8_t cfg_len = _payload_length - sizeof(ubx_cfg_valget);
1300 0 : const uint8_t *cfg_data = (const uint8_t *)(&_buffer) + sizeof(ubx_cfg_valget);
1301 0 : while (cfg_len >= 5) {
1302 : ConfigKey id;
1303 0 : memcpy(&id, cfg_data, sizeof(uint32_t));
1304 0 : cfg_len -= 4;
1305 0 : cfg_data += 4;
1306 0 : switch (id) {
1307 0 : case ConfigKey::CFG_TMODE_MODE: {
1308 0 : uint8_t mode = cfg_data[0];
1309 0 : if (mode != 0) {
1310 : // ask for mode 0, to disable TIME mode
1311 0 : mode = 0;
1312 0 : _configure_valset(ConfigKey::CFG_TMODE_MODE, &mode);
1313 0 : _cfg_needs_save = true;
1314 0 : _unconfigured_messages |= CONFIG_TMODE_MODE;
1315 : } else {
1316 0 : _unconfigured_messages &= ~CONFIG_TMODE_MODE;
1317 : }
1318 0 : break;
1319 : }
1320 0 : default:
1321 0 : break;
1322 : }
1323 : // see if it is in active config list
1324 0 : int8_t cfg_idx = find_active_config_index(id);
1325 0 : if (cfg_idx >= 0) {
1326 : CFG_Debug("valset(0x%lx): %u", (long unsigned)id, unsigned((*cfg_data) & 0x1));
1327 0 : const uint8_t key_size = config_key_size(id);
1328 0 : if (cfg_len < key_size
1329 : // for keys of length 1 only the LSB is significant
1330 0 : || (key_size == 1 && (active_config.list[cfg_idx].value & 0x1) != (*cfg_data & 0x1))
1331 0 : || memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) {
1332 0 : _configure_valset(id, &active_config.list[cfg_idx].value, active_config.layers);
1333 0 : _unconfigured_messages |= active_config.unconfig_bit;
1334 0 : active_config.done_mask &= ~(1U << cfg_idx);
1335 0 : active_config.set_index = cfg_idx;
1336 0 : _cfg_needs_save = true;
1337 : } else {
1338 0 : active_config.done_mask |= (1U << cfg_idx);
1339 : CFG_Debug("done %u mask=0x%x all_mask=0x%x",
1340 : unsigned(cfg_idx),
1341 : unsigned(active_config.done_mask),
1342 : (1U<<active_config.count)-1);
1343 0 : if (active_config.done_mask == (1U<<active_config.count)-1) {
1344 : // all done!
1345 0 : _unconfigured_messages &= ~active_config.unconfig_bit;
1346 : }
1347 : }
1348 0 : if (active_config.fetch_index >= 0 &&
1349 0 : active_config.fetch_index < active_config.count &&
1350 0 : id == active_config.list[active_config.fetch_index].key) {
1351 0 : active_config.fetch_index++;
1352 0 : if (active_config.fetch_index < active_config.count) {
1353 0 : _configure_valget(active_config.list[active_config.fetch_index].key);
1354 : CFG_Debug("valget %d 0x%x", int(active_config.fetch_index),
1355 : unsigned(active_config.list[active_config.fetch_index].key));
1356 : }
1357 : }
1358 : } else {
1359 : CFG_Debug("valget no active config for 0x%lx", (long unsigned)id);
1360 : }
1361 :
1362 : // step over the value
1363 0 : uint8_t step_size = config_key_size(id);
1364 0 : if (step_size == 0) {
1365 0 : return false;
1366 : }
1367 0 : cfg_len -= step_size;
1368 0 : cfg_data += step_size;
1369 : }
1370 : }
1371 : }
1372 : }
1373 : #if AP_GPS_UBLOX_CFGV2_ENABLED
1374 4870877 : return true;
1375 : }
1376 :
1377 : bool
1378 4870877 : AP_GPS_UBLOX::_parse_gps(void)
1379 : {
1380 4870877 : if (_legacy_cfg_supported && !gps.option_set(AP_GPS::ForceUBXConfigV2)) {
1381 4870877 : if (!_legacy_config_update()) {
1382 0 : return false;
1383 : }
1384 : }
1385 : // call again to parse any responses from CFGV2
1386 4870877 : _cfg_v2.update();
1387 :
1388 : #endif
1389 :
1390 4870877 : if (_class == CLASS_MON) {
1391 0 : switch(_msg_id) {
1392 0 : case MSG_MON_HW:
1393 0 : if (_payload_length == 60 || _payload_length == 68) {
1394 0 : log_mon_hw();
1395 : }
1396 0 : break;
1397 0 : case MSG_MON_HW2:
1398 0 : if (_payload_length == 28) {
1399 0 : log_mon_hw2();
1400 : }
1401 0 : break;
1402 : #if HAL_LOGGING_ENABLED && AP_GPS_UBLOX_CFGV2_ENABLED
1403 0 : case MSG_MON_RF:
1404 : // forward to logger when present
1405 0 : log_mon_rf();
1406 0 : break;
1407 : #endif // HAL_LOGGING_ENABLED
1408 0 : case MSG_MON_VER: {
1409 0 : bool check_L1L5 = false;
1410 0 : _have_version = true;
1411 0 : strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion));
1412 0 : strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion));
1413 0 : char* mod = (char*)memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "MOD=", 4);
1414 : #if AP_GPS_UBLOX_CFGV2_ENABLED
1415 0 : char* protver_end = _buffer.mon_ver.extension + sizeof(_buffer.mon_ver.extension);
1416 : #endif
1417 0 : if (mod != nullptr
1418 : #if AP_GPS_UBLOX_CFGV2_ENABLED
1419 0 : && (mod+4+UBLOX_MODULE_LEN-1) < protver_end
1420 : #endif
1421 : ) {
1422 0 : strncpy(_module, mod+4, UBLOX_MODULE_LEN-1);
1423 : }
1424 : #if AP_GPS_UBLOX_CFGV2_ENABLED
1425 0 : char* prot = (char*)memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "PROTVER=", 8);
1426 0 : if (prot != nullptr && (prot+8+UBLOX_PROTVER_LEN-1) < protver_end) {
1427 : Debug("Found PROTVER string %.20s", prot+8);
1428 0 : strncpy(_protver, prot+8, UBLOX_PROTVER_LEN-1);
1429 : }
1430 : #endif
1431 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
1432 : "u-blox %s%s%d HW: %s SW: %s",
1433 : _module, mod != nullptr ? " " : "",
1434 : state.instance + 1,
1435 : _version.hwVersion,
1436 : _version.swVersion);
1437 : // check for F9 and M9. The F9 does not respond to SVINFO,
1438 : // so we need to use MON_VER for hardware generation
1439 0 : if (strncmp(_version.hwVersion, "00190000", 8) == 0) {
1440 0 : if (strncmp(_version.swVersion, "EXT CORE 1", 10) == 0) {
1441 : // a F9
1442 0 : if (_hardware_generation != UBLOX_F9) {
1443 : // need to ensure time mode is correctly setup on F9
1444 0 : _unconfigured_messages |= CONFIG_TMODE_MODE;
1445 : }
1446 0 : _hardware_generation = UBLOX_F9;
1447 0 : _unconfigured_messages |= CONFIG_F9;
1448 0 : _unconfigured_messages &= ~CONFIG_GNSS;
1449 0 : if (strncmp(_module, "ZED-F9P", UBLOX_MODULE_LEN) == 0) {
1450 0 : _hardware_variant = UBLOX_F9_ZED;
1451 0 : } else if (strncmp(_module, "NEO-F9P", UBLOX_MODULE_LEN) == 0) {
1452 0 : _hardware_variant = UBLOX_F9_NEO;
1453 : }
1454 : }
1455 0 : if (strncmp(_version.swVersion, "EXT CORE 4", 10) == 0) {
1456 : // a M9
1457 0 : _hardware_generation = UBLOX_M9;
1458 : }
1459 0 : check_L1L5 = true;
1460 : }
1461 : // check for M10
1462 0 : if (strncmp(_version.hwVersion, "000A0000", 8) == 0) {
1463 0 : _hardware_generation = UBLOX_M10;
1464 0 : _unconfigured_messages |= CONFIG_M10;
1465 : // M10 does not support CONFIG_GNSS
1466 0 : _unconfigured_messages &= ~CONFIG_GNSS;
1467 0 : check_L1L5 = true;
1468 :
1469 : // M10 does not support multi-valued VALGET
1470 0 : use_single_valget = true;
1471 : }
1472 0 : if (check_L1L5) {
1473 : // check if L1L5 in extension
1474 0 : if (memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "L1L5", 4) != nullptr) {
1475 0 : supports_l5 = true;
1476 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "u-blox supports L5 Band");
1477 0 : _unconfigured_messages |= CONFIG_L5;
1478 : }
1479 : }
1480 0 : break;
1481 : }
1482 0 : default:
1483 0 : unexpected_message();
1484 : }
1485 0 : return false;
1486 : }
1487 :
1488 : #if UBLOX_RXM_RAW_LOGGING
1489 4870877 : if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
1490 0 : log_rxm_raw(_buffer.rxm_raw);
1491 0 : return false;
1492 4870877 : } else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
1493 0 : log_rxm_rawx(_buffer.rxm_rawx);
1494 0 : return false;
1495 : }
1496 : #endif // UBLOX_RXM_RAW_LOGGING
1497 :
1498 : #if UBLOX_TIM_TM2_LOGGING
1499 4870877 : if ((_class == CLASS_TIM) && (_msg_id == MSG_TIM_TM2) && (_payload_length == 28)) {
1500 0 : log_tim_tm2();
1501 0 : return false;
1502 : }
1503 : #endif // UBLOX_TIM_TM2_LOGGING
1504 :
1505 4870877 : if (_class != CLASS_NAV) {
1506 0 : unexpected_message();
1507 0 : return false;
1508 : }
1509 :
1510 4870877 : switch (_msg_id) {
1511 694294 : case MSG_POSLLH:
1512 : Debug("MSG_POSLLH next_fix=%u", next_fix);
1513 694294 : if (havePvtMsg) {
1514 694294 : _unconfigured_messages |= CONFIG_RATE_POSLLH;
1515 694294 : break;
1516 : }
1517 0 : _check_new_itow(_buffer.posllh.itow);
1518 0 : _last_pos_time = _buffer.posllh.itow;
1519 0 : state.location.lng = _buffer.posllh.longitude;
1520 0 : state.location.lat = _buffer.posllh.latitude;
1521 0 : state.have_undulation = true;
1522 0 : state.undulation = (_buffer.posllh.altitude_msl - _buffer.posllh.altitude_ellipsoid) * 0.001;
1523 0 : set_alt_amsl_cm(state, _buffer.posllh.altitude_msl / 10);
1524 :
1525 0 : state.status = next_fix;
1526 0 : _new_position = true;
1527 0 : state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
1528 0 : state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
1529 0 : state.have_horizontal_accuracy = true;
1530 0 : state.have_vertical_accuracy = true;
1531 : #if UBLOX_FAKE_3DLOCK
1532 : state.location.lng = 1491652300L;
1533 : state.location.lat = -353632610L;
1534 : state.location.alt = 58400;
1535 : state.vertical_accuracy = 0;
1536 : state.horizontal_accuracy = 0;
1537 : #endif
1538 0 : break;
1539 695316 : case MSG_STATUS:
1540 : Debug("MSG_STATUS fix_status=%u fix_type=%u",
1541 : _buffer.status.fix_status,
1542 : _buffer.status.fix_type);
1543 695316 : _check_new_itow(_buffer.status.itow);
1544 695316 : if (havePvtMsg) {
1545 : // when we have PVT we don't need status, just change the rate for STATUS to zero
1546 694294 : _unconfigured_messages &= ~CONFIG_RATE_STATUS;
1547 694294 : _configure_message_rate(CLASS_NAV, _msg_id, 0);
1548 694294 : break;
1549 : }
1550 1022 : if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
1551 0 : if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
1552 0 : (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
1553 0 : next_fix = AP_GPS_FixType::DGPS;
1554 0 : }else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
1555 0 : next_fix = AP_GPS_FixType::FIX_3D;
1556 0 : }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
1557 0 : next_fix = AP_GPS_FixType::FIX_2D;
1558 : }else{
1559 0 : next_fix = AP_GPS_FixType::NONE;
1560 0 : state.status = AP_GPS_FixType::NONE;
1561 : }
1562 : }else{
1563 1022 : next_fix = AP_GPS_FixType::NONE;
1564 1022 : state.status = AP_GPS_FixType::NONE;
1565 : }
1566 : #if UBLOX_FAKE_3DLOCK
1567 : state.status = AP_GPS_FixType::FIX_3D;
1568 : next_fix = state.status;
1569 : #endif
1570 1022 : break;
1571 695319 : case MSG_DOP:
1572 : Debug("MSG_DOP");
1573 695319 : noReceivedHdop = false;
1574 695319 : _check_new_itow(_buffer.dop.itow);
1575 695319 : state.hdop = _buffer.dop.hDOP;
1576 695319 : state.vdop = _buffer.dop.vDOP;
1577 : #if UBLOX_FAKE_3DLOCK
1578 : state.hdop = 130;
1579 : state.hdop = 170;
1580 : #endif
1581 695319 : break;
1582 695319 : case MSG_SOL:
1583 : Debug("MSG_SOL fix_status=%u fix_type=%u",
1584 : _buffer.solution.fix_status,
1585 : _buffer.solution.fix_type);
1586 695319 : _check_new_itow(_buffer.solution.itow);
1587 695319 : if (havePvtMsg) {
1588 694297 : state.time_week = _buffer.solution.week;
1589 694297 : break;
1590 : }
1591 1022 : if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
1592 1022 : if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
1593 0 : (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
1594 0 : next_fix = AP_GPS_FixType::DGPS;
1595 1022 : }else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
1596 0 : next_fix = AP_GPS_FixType::FIX_3D;
1597 1022 : }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
1598 0 : next_fix = AP_GPS_FixType::FIX_2D;
1599 : }else{
1600 1022 : next_fix = AP_GPS_FixType::NONE;
1601 1022 : state.status = AP_GPS_FixType::NONE;
1602 : }
1603 : }else{
1604 0 : next_fix = AP_GPS_FixType::NONE;
1605 0 : state.status = AP_GPS_FixType::NONE;
1606 : }
1607 1022 : if(noReceivedHdop) {
1608 1022 : state.hdop = _buffer.solution.position_DOP;
1609 : }
1610 1022 : state.num_sats = _buffer.solution.satellites;
1611 1022 : if (next_fix >= AP_GPS_FixType::FIX_2D) {
1612 0 : state.last_gps_time_ms = AP_HAL::millis();
1613 0 : state.time_week_ms = _buffer.solution.itow;
1614 0 : state.time_week = _buffer.solution.week;
1615 : }
1616 : #if UBLOX_FAKE_3DLOCK
1617 : next_fix = state.status;
1618 : state.num_sats = 10;
1619 : state.time_week = 1721;
1620 : state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
1621 : state.last_gps_time_ms = AP_HAL::millis();
1622 : state.hdop = 130;
1623 : #endif
1624 1022 : break;
1625 :
1626 : #if GPS_MOVING_BASELINE
1627 4678 : case MSG_RELPOSNED:
1628 : {
1629 4678 : if (role != AP_GPS::GPS_ROLE_MB_ROVER) {
1630 : // ignore RELPOSNED if not configured as a rover
1631 2360 : break;
1632 : }
1633 : // note that we require the yaw to come from a fixed solution, not a float solution
1634 : // yaw from a float solution would only be acceptable with a very large separation between
1635 : // GPS modules
1636 2318 : const uint32_t valid_mask = static_cast<uint32_t>(RELPOSNED::relPosHeadingValid) |
1637 : static_cast<uint32_t>(RELPOSNED::relPosValid) |
1638 : static_cast<uint32_t>(RELPOSNED::gnssFixOK) |
1639 : static_cast<uint32_t>(RELPOSNED::isMoving) |
1640 : static_cast<uint32_t>(RELPOSNED::carrSolnFixed);
1641 2318 : const uint32_t invalid_mask = static_cast<uint32_t>(RELPOSNED::refPosMiss) |
1642 : static_cast<uint32_t>(RELPOSNED::refObsMiss) |
1643 : static_cast<uint32_t>(RELPOSNED::carrSolnFloat);
1644 :
1645 2318 : _check_new_itow(_buffer.relposned.iTOW);
1646 2318 : if (_buffer.relposned.iTOW != _last_relposned_itow+200) {
1647 : // useful for looking at packet loss on links
1648 : MB_Debug("RELPOSNED ITOW %u %u\n", unsigned(_buffer.relposned.iTOW), unsigned(_last_relposned_itow));
1649 : }
1650 2318 : _last_relposned_itow = _buffer.relposned.iTOW;
1651 : MB_Debug("RELPOSNED flags: %lx valid: %lx invalid: %lx\n", _buffer.relposned.flags, valid_mask, invalid_mask);
1652 2318 : if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
1653 2318 : ((_buffer.relposned.flags & invalid_mask) == 0)) {
1654 2318 : if (calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
1655 2318 : _buffer.relposned.relPosLength * 0.01,
1656 2318 : _buffer.relposned.relPosD*0.01)) {
1657 2260 : state.have_gps_yaw_accuracy = true;
1658 2260 : state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
1659 2260 : _last_relposned_ms = AP_HAL::millis();
1660 : }
1661 2318 : state.relPosHeading = _buffer.relposned.relPosHeading * 1e-5;
1662 2318 : state.relPosLength = _buffer.relposned.relPosLength * 0.01;
1663 2318 : state.relPosD = _buffer.relposned.relPosD * 0.01;
1664 2318 : state.accHeading = _buffer.relposned.accHeading * 1e-5;
1665 2318 : state.relposheading_ts = AP_HAL::millis();
1666 : } else {
1667 0 : state.have_gps_yaw_accuracy = false;
1668 : }
1669 : }
1670 2318 : break;
1671 : #endif // GPS_MOVING_BASELINE
1672 :
1673 695319 : case MSG_PVT:
1674 : Debug("MSG_PVT");
1675 :
1676 695319 : havePvtMsg = true;
1677 :
1678 : // if we have PVT we don't want MSG_STATUS
1679 695319 : _unconfigured_messages &= ~CONFIG_RATE_STATUS;
1680 :
1681 : // position
1682 695319 : _check_new_itow(_buffer.pvt.itow);
1683 : // Only adjust if:
1684 : // we already have a valid week,
1685 : // PVT iTOW wrapped,
1686 : // and our time_week_ms still looks like end-of-week
1687 : // (meaning we haven't already accepted the rollover via TIMEGPS/SOL)
1688 695319 : if (state.time_week != 0 &&
1689 694297 : _last_pvt_itow != 0 &&
1690 694297 : _buffer.pvt.itow < _last_pvt_itow &&
1691 0 : state.time_week_ms > END_OF_WEEK_THRESHOLD_MS &&
1692 0 : _buffer.pvt.itow < START_OF_WEEK_THRESHOLD_MS) {
1693 0 : state.time_week++;
1694 : }
1695 :
1696 695319 : _last_pvt_itow = _buffer.pvt.itow;
1697 695319 : _last_pos_time = _buffer.pvt.itow;
1698 695319 : state.location.lng = _buffer.pvt.lon;
1699 695319 : state.location.lat = _buffer.pvt.lat;
1700 695319 : state.have_undulation = true;
1701 695319 : state.undulation = (_buffer.pvt.h_msl - _buffer.pvt.h_ellipsoid) * 0.001;
1702 695319 : set_alt_amsl_cm(state, _buffer.pvt.h_msl / 10);
1703 695319 : switch (_buffer.pvt.fix_type)
1704 : {
1705 7438 : case 0:
1706 7438 : state.status = AP_GPS_FixType::NONE;
1707 7438 : break;
1708 0 : case 1:
1709 0 : state.status = AP_GPS_FixType::NONE;
1710 0 : break;
1711 3 : case 2:
1712 3 : state.status = AP_GPS_FixType::FIX_2D;
1713 3 : break;
1714 687878 : case 3:
1715 687878 : state.status = AP_GPS_FixType::FIX_3D;
1716 687878 : if (_buffer.pvt.flags & 0b00000010) // diffsoln
1717 687875 : state.status = AP_GPS_FixType::DGPS;
1718 687878 : if (_buffer.pvt.flags & 0b01000000) // carrsoln - float
1719 3 : state.status = AP_GPS_FixType::RTK_FLOAT;
1720 687878 : if (_buffer.pvt.flags & 0b10000000) // carrsoln - fixed
1721 687869 : state.status = AP_GPS_FixType::RTK_FIXED;
1722 687878 : break;
1723 0 : case 4:
1724 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
1725 : "Unexpected state %d", _buffer.pvt.flags);
1726 0 : state.status = AP_GPS_FixType::FIX_3D;
1727 0 : break;
1728 0 : case 5:
1729 0 : state.status = AP_GPS_FixType::NONE;
1730 0 : break;
1731 0 : default:
1732 0 : state.status = AP_GPS_FixType::NONE;
1733 0 : break;
1734 : }
1735 695319 : next_fix = state.status;
1736 695319 : _new_position = true;
1737 695319 : state.horizontal_accuracy = _buffer.pvt.h_acc*1.0e-3f;
1738 695319 : state.vertical_accuracy = _buffer.pvt.v_acc*1.0e-3f;
1739 695319 : state.have_horizontal_accuracy = true;
1740 695319 : state.have_vertical_accuracy = true;
1741 : // SVs
1742 695319 : state.num_sats = _buffer.pvt.num_sv;
1743 : // velocity
1744 695319 : _last_vel_time = _buffer.pvt.itow;
1745 695319 : state.ground_speed = _buffer.pvt.gspeed*0.001f; // m/s
1746 695319 : state.ground_course = wrap_360(_buffer.pvt.head_mot * 1.0e-5f); // Heading 2D deg * 100000
1747 695319 : state.have_vertical_velocity = true;
1748 695319 : state.velocity.x = _buffer.pvt.velN * 0.001f;
1749 695319 : state.velocity.y = _buffer.pvt.velE * 0.001f;
1750 695319 : state.velocity.z = _buffer.pvt.velD * 0.001f;
1751 695319 : state.have_speed_accuracy = true;
1752 695319 : state.speed_accuracy = _buffer.pvt.s_acc*0.001f;
1753 695319 : _new_speed = true;
1754 : // dop
1755 695319 : if(noReceivedHdop) {
1756 0 : state.hdop = _buffer.pvt.p_dop;
1757 0 : state.vdop = _buffer.pvt.p_dop;
1758 : }
1759 :
1760 695319 : if (_buffer.pvt.fix_type >= 2) {
1761 687881 : state.last_gps_time_ms = AP_HAL::millis();
1762 : }
1763 :
1764 : // time
1765 695319 : state.time_week_ms = _buffer.pvt.itow;
1766 : #if UBLOX_FAKE_3DLOCK
1767 : state.location.lng = 1491652300L;
1768 : state.location.lat = -353632610L;
1769 : state.location.alt = 58400;
1770 : state.vertical_accuracy = 0;
1771 : state.horizontal_accuracy = 0;
1772 : state.status = AP_GPS_FixType::FIX_3D;
1773 : state.num_sats = 10;
1774 : state.time_week = 1721;
1775 : state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
1776 : state.last_gps_time_ms = AP_HAL::millis();
1777 : state.hdop = 130;
1778 : state.speed_accuracy = 0;
1779 : next_fix = state.status;
1780 : #endif
1781 695319 : break;
1782 0 : case MSG_TIMEGPS:
1783 : Debug("MSG_TIMEGPS");
1784 0 : _check_new_itow(_buffer.timegps.itow);
1785 0 : if (_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK) {
1786 0 : state.time_week_ms = _buffer.timegps.itow;
1787 0 : state.time_week = _buffer.timegps.week;
1788 0 : state.last_gps_time_ms = AP_HAL::millis();
1789 : }
1790 0 : break;
1791 695316 : case MSG_VELNED:
1792 : Debug("MSG_VELNED");
1793 695316 : if (havePvtMsg) {
1794 694294 : _unconfigured_messages |= CONFIG_RATE_VELNED;
1795 694294 : break;
1796 : }
1797 1022 : _check_new_itow(_buffer.velned.itow);
1798 1022 : _last_vel_time = _buffer.velned.itow;
1799 1022 : state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
1800 1022 : state.ground_course = wrap_360(_buffer.velned.heading_2d * 1.0e-5f); // Heading 2D deg * 100000
1801 1022 : state.have_vertical_velocity = true;
1802 1022 : state.velocity.x = _buffer.velned.ned_north * 0.01f;
1803 1022 : state.velocity.y = _buffer.velned.ned_east * 0.01f;
1804 1022 : state.velocity.z = _buffer.velned.ned_down * 0.01f;
1805 1022 : velocity_to_speed_course(state);
1806 1022 : state.have_speed_accuracy = true;
1807 1022 : state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
1808 : #if UBLOX_FAKE_3DLOCK
1809 : state.speed_accuracy = 0;
1810 : #endif
1811 1022 : _new_speed = true;
1812 1022 : break;
1813 695316 : case MSG_NAV_SVINFO:
1814 : {
1815 : Debug("MSG_NAV_SVINFO\n");
1816 : static const uint8_t HardwareGenerationMask = 0x07;
1817 695316 : _check_new_itow(_buffer.svinfo_header.itow);
1818 695316 : _hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
1819 695316 : switch (_hardware_generation) {
1820 0 : case UBLOX_5:
1821 : case UBLOX_6:
1822 : // only 7 and newer support CONFIG_GNSS
1823 0 : _unconfigured_messages &= ~CONFIG_GNSS;
1824 0 : break;
1825 695316 : case UBLOX_7:
1826 : case UBLOX_M8:
1827 : #if UBLOX_SPEED_CHANGE
1828 : port->begin(4000000U);
1829 : Debug("Changed speed to 4Mhz for SPI-driven UBlox\n");
1830 : #endif
1831 695316 : break;
1832 0 : default:
1833 0 : hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation);
1834 0 : break;
1835 : };
1836 695316 : _unconfigured_messages &= ~CONFIG_VERSION;
1837 : /* We don't need that anymore */
1838 695316 : _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
1839 695316 : break;
1840 : }
1841 0 : default:
1842 : Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
1843 0 : if (++_disable_counter == 0) {
1844 : Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
1845 0 : _configure_message_rate(CLASS_NAV, _msg_id, 0);
1846 : }
1847 0 : return false;
1848 : }
1849 :
1850 4870877 : if (state.have_gps_yaw) {
1851 : // when we are a rover we want to ensure we have both the new
1852 : // PVT and the new RELPOSNED message so that we give a
1853 : // consistent view
1854 18038 : if (AP_HAL::millis() - _last_relposned_ms > 400) {
1855 : // we have stopped receiving valid RELPOSNED messages, disable yaw reporting
1856 0 : state.have_gps_yaw = false;
1857 18038 : } else if (_last_relposned_itow != _last_pvt_itow) {
1858 : // wait until ITOW matches
1859 2253 : return false;
1860 : }
1861 : }
1862 :
1863 : // we only return true when we get new position and speed data
1864 : // this ensures we don't use stale data
1865 4868624 : if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
1866 695319 : _new_speed = _new_position = false;
1867 695319 : return true;
1868 : }
1869 4173305 : return false;
1870 : }
1871 :
1872 : /*
1873 : * handle pps interrupt
1874 : */
1875 : #ifdef HAL_GPIO_PPS
1876 : void
1877 : AP_GPS_UBLOX::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us)
1878 : {
1879 : _last_pps_time_us = AP_HAL::micros64();
1880 : }
1881 :
1882 : void
1883 : AP_GPS_UBLOX::set_pps_desired_freq(uint8_t freq)
1884 : {
1885 : _pps_freq = freq;
1886 : _unconfigured_messages |= CONFIG_TP5;
1887 : }
1888 : #endif
1889 :
1890 :
1891 : // UBlox auto configuration
1892 :
1893 : /*
1894 : * update checksum for a set of bytes
1895 : */
1896 : void
1897 5412742 : AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b)
1898 : {
1899 20407056 : while (len--) {
1900 14994314 : ck_a += *data;
1901 14994314 : ck_b += ck_a;
1902 14994314 : data++;
1903 : }
1904 5412742 : }
1905 :
1906 :
1907 : /*
1908 : * send a ublox message
1909 : */
1910 : bool
1911 2706371 : AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, const void *msg, uint16_t size)
1912 : {
1913 2706371 : if (port->txspace() < (sizeof(struct ubx_header) + 2 + size)) {
1914 0 : return false;
1915 : }
1916 : struct ubx_header header;
1917 2706371 : uint8_t ck_a=0, ck_b=0;
1918 2706371 : header.preamble1 = PREAMBLE1;
1919 2706371 : header.preamble2 = PREAMBLE2;
1920 2706371 : header.msg_class = msg_class;
1921 2706371 : header.msg_id = msg_id;
1922 2706371 : header.length = size;
1923 :
1924 2706371 : _update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
1925 2706371 : _update_checksum((uint8_t *)msg, size, ck_a, ck_b);
1926 :
1927 2706371 : port->write((const uint8_t *)&header, sizeof(header));
1928 2706371 : port->write((const uint8_t *)msg, size);
1929 2706371 : port->write((const uint8_t *)&ck_a, 1);
1930 2706371 : port->write((const uint8_t *)&ck_b, 1);
1931 2706371 : return true;
1932 : }
1933 :
1934 : /*
1935 : * requests the given message rate for a specific message class
1936 : * and msg_id
1937 : * returns true if it sent the request, false if waiting on knowing the port
1938 : */
1939 : bool
1940 688141 : AP_GPS_UBLOX::_request_message_rate(uint8_t msg_class, uint8_t msg_id)
1941 : {
1942 : // Without knowing what communication port is being used it isn't possible to verify
1943 : // always ensure we have a port before sending the request
1944 688141 : if(_ublox_port >= UBLOX_MAX_PORTS) {
1945 688141 : _request_port();
1946 688141 : return false;
1947 : } else {
1948 : struct ubx_cfg_msg msg;
1949 0 : msg.msg_class = msg_class;
1950 0 : msg.msg_id = msg_id;
1951 0 : return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
1952 : }
1953 : }
1954 :
1955 : /*
1956 : * configure a UBlox GPS for the given message rate for a specific
1957 : * message class and msg_id
1958 : */
1959 : bool
1960 1389610 : AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
1961 : {
1962 1389610 : if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_msg_rate)+2)) {
1963 0 : return false;
1964 : }
1965 :
1966 : struct ubx_cfg_msg_rate msg;
1967 1389610 : msg.msg_class = msg_class;
1968 1389610 : msg.msg_id = msg_id;
1969 1389610 : msg.rate = rate;
1970 1389610 : return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg));
1971 : }
1972 :
1973 : /*
1974 : * configure F9/M10 based key/value pair - VALSET
1975 : */
1976 : bool
1977 0 : AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value, uint8_t layers)
1978 : {
1979 0 : if (!supports_F9_config()) {
1980 0 : return false;
1981 : }
1982 :
1983 0 : const uint8_t len = config_key_size(key);
1984 0 : struct ubx_cfg_valset msg {};
1985 0 : uint8_t buf[sizeof(msg)+len];
1986 0 : if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
1987 0 : return false;
1988 : }
1989 0 : msg.version = 1;
1990 0 : msg.layers = layers;
1991 0 : msg.transaction = 0;
1992 0 : msg.key = uint32_t(key);
1993 0 : memcpy(buf, &msg, sizeof(msg));
1994 0 : memcpy(&buf[sizeof(msg)], value, len);
1995 0 : auto ret = _send_message(CLASS_CFG, MSG_CFG_VALSET, buf, sizeof(buf));
1996 0 : return ret;
1997 0 : }
1998 :
1999 : /*
2000 : * configure F9 based key/value pair - VALGET
2001 : */
2002 : bool
2003 0 : AP_GPS_UBLOX::_configure_valget(ConfigKey key)
2004 : {
2005 0 : if (!supports_F9_config()) {
2006 0 : return false;
2007 : }
2008 : struct {
2009 : struct ubx_cfg_valget msg;
2010 : ConfigKey key;
2011 0 : } msg {};
2012 0 : if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(msg)+2)) {
2013 0 : return false;
2014 : }
2015 0 : msg.msg.version = 0;
2016 0 : msg.msg.layers = 0; // ram
2017 0 : msg.key = key;
2018 0 : return _send_message(CLASS_CFG, MSG_CFG_VALGET, &msg, sizeof(msg));
2019 : }
2020 :
2021 : /*
2022 : * configure F9 based key/value pair for a complete configuration set
2023 : *
2024 : * this method requests each configuration variable from the GPS.
2025 : * When we handle the reply in _parse_gps we may then choose to set a
2026 : * MSG_CFG_VALSET back to the GPS if we don't like its response.
2027 : */
2028 : bool
2029 0 : AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit, uint8_t layers)
2030 : {
2031 0 : active_config.list = list;
2032 0 : active_config.count = count;
2033 0 : active_config.done_mask = 0;
2034 0 : active_config.unconfig_bit = unconfig_bit;
2035 0 : active_config.layers = layers;
2036 : // we start by fetching multiple values at once (with fetch_index
2037 : // -1) then if we get a NACK for VALGET we switch to fetching one
2038 : // value at a time. This copes with the M10S which can only fetch
2039 : // one value at a time
2040 0 : active_config.fetch_index = use_single_valget? 0 :-1;
2041 :
2042 0 : uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)];
2043 0 : struct ubx_cfg_valget msg {};
2044 0 : if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
2045 0 : return false;
2046 : }
2047 0 : msg.version = 0;
2048 0 : msg.layers = 0; // ram
2049 0 : memcpy(buf, &msg, sizeof(msg));
2050 0 : for (uint8_t i=0; i<count; i++) {
2051 0 : memcpy(&buf[sizeof(msg)+i*sizeof(ConfigKey)], &list[i].key, sizeof(ConfigKey));
2052 : }
2053 0 : return _send_message(CLASS_CFG, MSG_CFG_VALGET, buf, sizeof(buf));
2054 0 : }
2055 :
2056 : /*
2057 : * configure F9 based key/value pair for a complete configuration set
2058 : *
2059 : * this method sends all the key/value pairs in a block, but makes no attempt to check
2060 : * the results. Sending in a block is necessary for updates such as GNSS where certain
2061 : * combinations are invalid and setting one at a time will not produce the correct result
2062 : * if the result needs to be validated then a subsequent _configure_config_set() can be
2063 : * issued which will get all the values and reset those that are not properly updated.
2064 : */
2065 : bool
2066 0 : AP_GPS_UBLOX::_configure_list_valset(const config_list *list, uint8_t count, uint8_t layers)
2067 : {
2068 0 : if (!supports_F9_config()) {
2069 0 : return false;
2070 : }
2071 :
2072 0 : struct ubx_cfg_valset msg {};
2073 0 : uint8_t buf[sizeof(msg)+sizeof(config_list)*count];
2074 0 : if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) {
2075 0 : return false;
2076 : }
2077 0 : msg.version = 1;
2078 0 : msg.layers = layers;
2079 0 : msg.transaction = 0;
2080 0 : uint32_t msg_len = sizeof(msg) - sizeof(msg.key);
2081 0 : memcpy(buf, &msg, msg_len);
2082 :
2083 0 : uint8_t* payload = &buf[msg_len];
2084 0 : for (uint8_t i=0; i<count; i++) {
2085 0 : const uint8_t len = config_key_size(list[i].key);
2086 0 : memcpy(payload, &list[i].key, sizeof(ConfigKey));
2087 0 : payload += sizeof(ConfigKey);
2088 0 : msg_len += sizeof(ConfigKey);
2089 0 : memcpy(payload, &list[i].value, len);
2090 0 : payload += len;
2091 0 : msg_len += len;
2092 : }
2093 :
2094 0 : auto ret = _send_message(CLASS_CFG, MSG_CFG_VALSET, buf, msg_len);
2095 0 : return ret;
2096 0 : }
2097 :
2098 : /*
2099 : * save gps configurations to non-volatile memory sent until the call of
2100 : * this message
2101 : */
2102 : void
2103 0 : AP_GPS_UBLOX::_save_cfg()
2104 : {
2105 : static const ubx_cfg_cfg save_cfg {
2106 : clearMask: 0,
2107 : saveMask: SAVE_CFG_ALL,
2108 : loadMask: 0
2109 : };
2110 0 : _send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg));
2111 0 : _last_cfg_sent_time = AP_HAL::millis();
2112 0 : _num_cfg_save_tries++;
2113 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2114 : "GPS %d: u-blox saving config",
2115 : state.instance + 1);
2116 0 : }
2117 :
2118 : /*
2119 : detect a Ublox GPS. Adds one byte, and returns true if the stream
2120 : matches a UBlox
2121 : */
2122 : bool
2123 36828 : AP_GPS_UBLOX::_detect(struct UBLOX_detect_state &state, uint8_t data)
2124 : {
2125 36828 : reset:
2126 36828 : switch (state.step) {
2127 1023 : case 1:
2128 1023 : if (PREAMBLE2 == data) {
2129 1023 : state.step++;
2130 1023 : break;
2131 : }
2132 0 : state.step = 0;
2133 : FALLTHROUGH;
2134 1023 : case 0:
2135 1023 : if (PREAMBLE1 == data)
2136 1023 : state.step++;
2137 1023 : break;
2138 1023 : case 2:
2139 1023 : state.step++;
2140 1023 : state.ck_b = state.ck_a = data;
2141 1023 : break;
2142 1023 : case 3:
2143 1023 : state.step++;
2144 1023 : state.ck_b += (state.ck_a += data);
2145 1023 : break;
2146 1023 : case 4:
2147 1023 : state.step++;
2148 1023 : state.ck_b += (state.ck_a += data);
2149 1023 : state.payload_length = data;
2150 1023 : break;
2151 1023 : case 5:
2152 1023 : state.step++;
2153 1023 : state.ck_b += (state.ck_a += data);
2154 1023 : state.payload_counter = 0;
2155 1023 : break;
2156 28644 : case 6:
2157 28644 : state.ck_b += (state.ck_a += data);
2158 28644 : if (++state.payload_counter == state.payload_length)
2159 1023 : state.step++;
2160 28644 : break;
2161 1023 : case 7:
2162 1023 : state.step++;
2163 1023 : if (state.ck_a != data) {
2164 0 : state.step = 0;
2165 0 : goto reset;
2166 : }
2167 1023 : break;
2168 1023 : case 8:
2169 1023 : state.step = 0;
2170 1023 : if (state.ck_b == data) {
2171 : // a valid UBlox packet
2172 1023 : return true;
2173 : } else {
2174 0 : goto reset;
2175 : }
2176 : }
2177 35805 : return false;
2178 : }
2179 :
2180 : void
2181 628620 : AP_GPS_UBLOX::_request_version(void)
2182 : {
2183 628620 : _send_message(CLASS_MON, MSG_MON_VER, nullptr, 0);
2184 628620 : }
2185 :
2186 : void
2187 0 : AP_GPS_UBLOX::_configure_rate(void)
2188 : {
2189 : struct ubx_cfg_nav_rate msg;
2190 : // require a minimum measurement rate of 5Hz
2191 0 : msg.measure_rate_ms = gps.get_rate_ms(state.instance);
2192 0 : msg.nav_rate = 1;
2193 0 : msg.timeref = 0; // UTC time
2194 0 : _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
2195 0 : }
2196 :
2197 : static const char *reasons[] = {"navigation rate",
2198 : "posllh rate",
2199 : "status rate",
2200 : "solution rate",
2201 : "velned rate",
2202 : "dop rate",
2203 : "hw monitor rate",
2204 : "hw2 monitor rate",
2205 : "raw rate",
2206 : "version",
2207 : "navigation settings",
2208 : "GNSS settings",
2209 : "SBAS settings",
2210 : "PVT rate",
2211 : "time pulse settings",
2212 : "TIMEGPS rate",
2213 : "Time mode settings",
2214 : "RTK MB",
2215 : "TIM TM2",
2216 : "F9",
2217 : "M10",
2218 : "L5 Enable Disable"};
2219 :
2220 : static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description");
2221 :
2222 : void
2223 0 : AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const {
2224 0 : for (uint8_t i = 0; i < ARRAY_SIZE(reasons); i++) {
2225 0 : if (_unconfigured_messages & (1 << i)) {
2226 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: u-blox %s configuration 0x%02x",
2227 : (unsigned int)(state.instance + 1), reasons[i], (unsigned int)_unconfigured_messages);
2228 0 : break;
2229 : }
2230 : }
2231 0 : }
2232 :
2233 : /*
2234 : return velocity lag in seconds
2235 : */
2236 78073201 : bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
2237 : {
2238 78073201 : switch (_hardware_generation) {
2239 6636 : case UBLOX_UNKNOWN_HARDWARE_GENERATION:
2240 6636 : lag_sec = 0.22f;
2241 : // always bail out in this case, it's used to indicate we have yet to receive a valid
2242 : // hardware generation, however the user may have inhibited us detecting the generation
2243 : // so if we aren't allowed to do configuration, we will accept this as the default delay
2244 6636 : return gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE;
2245 0 : case UBLOX_5:
2246 : case UBLOX_6:
2247 : default:
2248 0 : lag_sec = 0.22f;
2249 0 : break;
2250 78066565 : case UBLOX_7:
2251 : case UBLOX_M8:
2252 : // based on flight logs the 7 and 8 series seem to produce about 120ms lag
2253 78066565 : lag_sec = 0.12f;
2254 78066565 : break;
2255 0 : case UBLOX_F9:
2256 : case UBLOX_M9:
2257 : case UBLOX_M10:
2258 : #if AP_GPS_UBLOX_CFGV2_ENABLED
2259 : case UBLOX_F10:
2260 : case UBLOX_F20:
2261 : case UBLOX_X20:
2262 : #endif
2263 : // F9 lag not verified yet from flight log, but likely to be at least
2264 : // as good as M8
2265 0 : lag_sec = 0.12f;
2266 : #if GPS_MOVING_BASELINE
2267 0 : if (role == AP_GPS::GPS_ROLE_MB_BASE ||
2268 0 : role == AP_GPS::GPS_ROLE_MB_ROVER) {
2269 : // the moving baseline rover will lag about 40ms from the
2270 : // base. We need to provide the more conservative value to
2271 : // ensure that the EKF allocates a larger enough buffer
2272 0 : lag_sec += 0.04;
2273 : }
2274 : #endif
2275 0 : break;
2276 : };
2277 78066565 : return true;
2278 : }
2279 :
2280 : #if HAL_LOGGING_ENABLED
2281 1776 : void AP_GPS_UBLOX::Write_AP_Logger_Log_Startup_messages() const
2282 : {
2283 1776 : AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages();
2284 :
2285 1776 : if (_have_version) {
2286 0 : AP::logger().Write_MessageF("u-blox %d HW: %s SW: %s",
2287 0 : state.instance+1,
2288 0 : _version.hwVersion,
2289 0 : _version.swVersion);
2290 : }
2291 1776 : }
2292 : #endif
2293 :
2294 : // uBlox specific check_new_itow(), handling message length
2295 3479929 : void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
2296 : {
2297 3479929 : check_new_itow(itow, _payload_length + sizeof(ubx_header) + 2);
2298 3479929 : }
2299 :
2300 : // support for retrieving RTCMv3 data from a moving baseline base
2301 22030 : bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
2302 : {
2303 : #if GPS_MOVING_BASELINE
2304 22030 : if (rtcm3_parser) {
2305 21896 : len = rtcm3_parser->get_len(bytes);
2306 21896 : return len > 0;
2307 : }
2308 : #endif
2309 134 : return false;
2310 : }
2311 :
2312 : // clear previous RTCM3 packet
2313 0 : void AP_GPS_UBLOX::clear_RTCMV3(void)
2314 : {
2315 : #if GPS_MOVING_BASELINE
2316 0 : if (rtcm3_parser) {
2317 0 : rtcm3_parser->clear_packet();
2318 : }
2319 : #endif
2320 0 : }
2321 :
2322 : // ublox specific healthy checks
2323 1062646 : bool AP_GPS_UBLOX::is_healthy(void) const
2324 : {
2325 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2326 1062646 : if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
2327 : // allow for fake ublox moving baseline
2328 2341 : return true;
2329 : }
2330 : #endif
2331 : #if GPS_MOVING_BASELINE
2332 3180870 : if ((role == AP_GPS::GPS_ROLE_MB_BASE ||
2333 1060350 : role == AP_GPS::GPS_ROLE_MB_ROVER) &&
2334 45 : !supports_F9_config()) {
2335 : // need F9 or above for moving baseline
2336 45 : return false;
2337 : }
2338 1060260 : if (role == AP_GPS::GPS_ROLE_MB_BASE && rtcm3_parser == nullptr && !mb_use_uart2()) {
2339 : // we haven't initialised RTCMv3 parser
2340 0 : return false;
2341 : }
2342 : #endif
2343 1060260 : return true;
2344 : }
2345 :
2346 : // populate config_GNSS with F9 GNSS configuration
2347 0 : uint8_t AP_GPS_UBLOX::populate_F9_gnss(void)
2348 : {
2349 0 : uint8_t cfg_count = 0;
2350 :
2351 0 : if (params.gnss_mode == 0) {
2352 0 : _unconfigured_messages &= ~CONFIG_F9;
2353 0 : last_configured_gnss = params.gnss_mode;
2354 0 : return 0;
2355 : }
2356 :
2357 0 : if ((_unconfigured_messages & CONFIG_F9) != 0) {
2358 : // ZED-F9P defaults are
2359 : // GPS L1C/A+L2C(ZED)
2360 : // SBAS L1C/A
2361 : // GALILEO E1+E5B(ZED)+E5A(NEO)
2362 : // BEIDOU B1+B2(ZED)+B2A(NEO)
2363 : // QZSS L1C/A+L2C(ZED)+L5(NEO)
2364 : // GLONASS L1+L2(ZED)
2365 : // IMES not supported
2366 : // GPS and QZSS should be enabled/disabled together, but we will leave them alone
2367 : // QZSS and SBAS can only be enabled if GPS is enabled
2368 :
2369 0 : if (config_GNSS == nullptr) {
2370 0 : config_GNSS = (config_list*)calloc(UBLOX_MAX_GNSS_CONFIG_BLOCKS*3, sizeof(config_list));
2371 : }
2372 :
2373 0 : if (config_GNSS == nullptr) {
2374 0 : return 0;
2375 : }
2376 :
2377 0 : uint8_t gnss_mode = params.gnss_mode;
2378 0 : gnss_mode |= 1U<<GNSS_GPS;
2379 0 : gnss_mode |= 1U<<GNSS_QZSS;
2380 0 : gnss_mode &= ~(1U<<GNSS_IMES);
2381 0 : params.gnss_mode.set_and_save(gnss_mode);
2382 :
2383 0 : for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
2384 0 : bool ena = gnss_mode & (1U<<i);
2385 0 : switch (i) {
2386 0 : case GNSS_SBAS:
2387 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_SBAS_ENA, ena };
2388 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_SBAS_L1CA_ENA, ena };
2389 0 : break;
2390 0 : case GNSS_GAL:
2391 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_ENA, ena };
2392 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E1_ENA, ena };
2393 0 : if (_hardware_variant == UBLOX_F9_ZED) {
2394 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E5B_ENA, ena };
2395 : } else {
2396 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GAL_E5A_ENA, ena };
2397 : }
2398 0 : break;
2399 0 : case GNSS_BDS:
2400 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_ENA, ena };
2401 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B1_ENA, ena };
2402 0 : if (_hardware_variant == UBLOX_F9_ZED) {
2403 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B2_ENA, ena };
2404 : } else {
2405 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_BDS_B2A_ENA, ena };
2406 : }
2407 0 : break;
2408 0 : case GNSS_GLO:
2409 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_ENA, ena };
2410 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L1_ENA, ena };
2411 0 : if (_hardware_variant == UBLOX_F9_ZED) {
2412 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_GLO_L2_ENA, ena };
2413 : }
2414 0 : break;
2415 0 : case GNSS_NAVIC:
2416 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_NAVIC_ENA, ena };
2417 0 : config_GNSS[cfg_count++] = { ConfigKey::CFG_SIGNAL_NAVIC_L5_ENA, ena };
2418 0 : break;
2419 : // not supported or leave alone
2420 0 : case GNSS_IMES:
2421 : case GNSS_QZSS:
2422 : case GNSS_GPS:
2423 0 : break;
2424 : }
2425 : }
2426 : }
2427 :
2428 0 : last_configured_gnss = params.gnss_mode;
2429 :
2430 0 : return cfg_count;
2431 : }
2432 :
2433 : // return true if GPS is capable of F9 config
2434 45 : bool AP_GPS_UBLOX::supports_F9_config(void) const
2435 : {
2436 45 : return _hardware_generation == UBLOX_F9 || _hardware_generation == UBLOX_M10
2437 : #if AP_GPS_UBLOX_CFGV2_ENABLED
2438 90 : || _hardware_generation == UBLOX_F20 || _hardware_generation == UBLOX_X20
2439 : #endif
2440 : ;
2441 : }
2442 :
2443 : // return true if GPS is capable of F9 config
2444 0 : bool AP_GPS_UBLOX::is_gnss_key(ConfigKey key) const
2445 : {
2446 0 : return (unsigned(key) & 0xFFFF0000) == 0x10310000;
2447 : }
2448 :
2449 : #endif
|
