From 69c540a0defcbc5547a3cd9dc2a81b989930e2e0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 18 Feb 2025 11:03:02 +1100 Subject: [PATCH] AP_GPS: fixed the check for disabling CONFIG_RATE_SOL config we should stop looking for SOL as soon as we know we have PVT, instead of waiting for all others to be cleared may help with https://discuss.ardupilot.org/t/fc-tries-to-config-gps-constantly-when-trying-to-arm-in-loiter-mode/129930 --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 28 +++++++++++++++++++++++----- libraries/AP_GPS/AP_GPS_UBLOX.h | 1 + 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index b74f163a64..3910fcdbef 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -278,7 +278,7 @@ AP_GPS_UBLOX::_request_next_config(void) return; } - if (_unconfigured_messages == CONFIG_RATE_SOL && havePvtMsg) { + if ((_unconfigured_messages & CONFIG_RATE_SOL) != 0 && havePvtMsg) { /* we don't need SOL if we have PVT and TIMEGPS. This is needed as F9P doesn't support the SOL message @@ -453,7 +453,8 @@ AP_GPS_UBLOX::_request_next_config(void) break; case STEP_F9: { - if (_hardware_generation == UBLOX_F9) { + if (_hardware_generation == UBLOX_F9 || + _hardware_generation == UBLOX_M10) { uint8_t cfg_count = populate_F9_gnss(); // special handling of F9 config if (cfg_count > 0) { @@ -470,7 +471,8 @@ AP_GPS_UBLOX::_request_next_config(void) } case STEP_F9_VALIDATE: { - if (_hardware_generation == UBLOX_F9) { + if (_hardware_generation == UBLOX_F9 || + _hardware_generation == UBLOX_M10) { // GNSS takes 0.5s to reset if (AP_HAL::millis() - _f9_config_time < 500) { _next_message--; @@ -662,6 +664,17 @@ AP_GPS_UBLOX::read(void) } } +#if 0 + // this can be modified to force a particular config state for + // state machine config debugging + static bool done_force_config_error; + if (!_unconfigured_messages && !done_force_config_error) { + done_force_config_error = true; + _unconfigured_messages = CONFIG_F9 | CONFIG_RATE_SOL; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Forcing config state 0x%04x", unsigned(_unconfigured_messages)); + } +#endif + if(!_unconfigured_messages && gps._save_config && !_cfg_saved && _num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 && !hal.util->get_soft_armed()) { @@ -1088,6 +1101,7 @@ AP_GPS_UBLOX::_parse_gps(void) if (active_config.fetch_index == -1) { CFG_Debug("NACK starting %u", unsigned(active_config.count)); active_config.fetch_index = 0; + use_single_valget = true; } else { // the device does not support the config key we asked for, // consider the bit as done @@ -1096,7 +1110,8 @@ AP_GPS_UBLOX::_parse_gps(void) int(active_config.fetch_index), unsigned(active_config.list[active_config.fetch_index].key), unsigned(active_config.done_mask)); - if (active_config.done_mask == (1U<= active_config.count) { // all done! _unconfigured_messages &= ~active_config.unconfig_bit; } @@ -1441,6 +1456,9 @@ AP_GPS_UBLOX::_parse_gps(void) // M10 does not support CONFIG_GNSS _unconfigured_messages &= ~CONFIG_GNSS; check_L1L5 = true; + + // M10 does not support multi-valued VALGET + use_single_valget = true; } if (check_L1L5) { // check if L1L5 in extension @@ -1989,7 +2007,7 @@ AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint // -1) then if we get a NACK for VALGET we switch to fetching one // value at a time. This copes with the M10S which can only fetch // one value at a time - active_config.fetch_index = -1; + active_config.fetch_index = use_single_valget? 0 :-1; uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)]; struct ubx_cfg_valget msg {}; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index caf51f6554..47d1ccbd3f 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -887,6 +887,7 @@ private: int8_t fetch_index; int8_t set_index; } active_config; + bool use_single_valget; #if GPS_MOVING_BASELINE // config for moving baseline base