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
ArduPilot · Feb 24, 2025 · b5ae8b6 · b5ae8b6
1 parent ae55f9a
commit b5ae8b6
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 5 deletions.
diff --git 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)-1) {
+                            if (active_config.done_mask == (1U<<active_config.count)-1 ||
+                                active_config.fetch_index >= 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
@@ -887,6 +887,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
         int8_t fetch_index;
         int8_t set_index;
     } active_config;
+    bool use_single_valget;
 
 #if GPS_MOVING_BASELINE
     // config for moving baseline base