AP_GPS: add support for putting ublox into recovery mode

CubePilot · Feb 9, 2024 · 94d8420 · 94d8420
1 parent fcfbbb6
commit 94d8420
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 8 deletions.
diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
@@ -972,6 +972,11 @@ void AP_GPS::update_instance(uint8_t instance)
 
     // we have an active driver for this instance
     bool result = drivers[instance]->read();
+    if (drivers[instance]->in_recovery_mode()) {
+        // We are probably in the middle of updating the unit
+        // nothing to do
+        return;
+    }
     uint32_t tnow = AP_HAL::millis();
 
     // if we did not get a message, and the idle timer of 2 seconds

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -285,6 +285,19 @@ AP_GPS_UBLOX::_request_next_config(void)
             _next_message--;
         }
         break;
+    case STEP_VERSION:
+        if(!_have_version && !hal.util->get_soft_armed()) {
+            _request_version();
+        } else {
+            _unconfigured_messages &= ~CONFIG_VERSION;
+        }
+        if (in_safeboot_mode) {
+            // keep asking for version until we get a valid
+            // version message
+            _have_version = false;
+            _next_message--;
+        }
+        break;
     case STEP_TIMEGPS:
         if(!_request_message_rate(CLASS_NAV, MSG_TIMEGPS)) {
             _next_message--;
@@ -387,13 +400,6 @@ AP_GPS_UBLOX::_request_next_config(void)
         _unconfigured_messages & = ~CONFIG_RATE_RAW;
 #endif
         break;
-    case STEP_VERSION:
-        if(!_have_version && !hal.util->get_soft_armed()) {
-            _request_version();
-        } else {
-            _unconfigured_messages &= ~CONFIG_VERSION;
-        }
-        break;
     case STEP_TMODE:
         if (supports_F9_config()) {
             if (!_configure_valget(ConfigKey::TMODE_MODE)) {
@@ -1365,6 +1371,12 @@ AP_GPS_UBLOX::_parse_gps(void)
                     }
                     _hardware_generation = UBLOX_F9;
                 }
+                if (memmem(_version.swVersion, sizeof(_version.swVersion), "ROM BOOT", 8) != nullptr) {
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "u-blox in safeboot mode");
+                    in_safeboot_mode = true;
+                } else {
+                    in_safeboot_mode = false;
+                }
                 // check if L1L5 in extension
                 if (memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "L1L5", 4) != nullptr) {
                     supports_l5 = true;

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -165,6 +165,9 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
 
     // ublox specific healthy checks
     bool is_healthy(void) const override;
+
+    // ublox in safeboot mode
+    bool in_recovery_mode(void) const override { return in_safeboot_mode; }
 
 private:
     // u-blox UBX protocol essentials
@@ -737,6 +740,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
 
     enum config_step {
         STEP_PVT = 0,
+        STEP_VERSION,
         STEP_NAV_RATE, // poll NAV rate
         STEP_SOL,
         STEP_PORT,
@@ -755,7 +759,6 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
         STEP_MON_HW2,
         STEP_RAW,
         STEP_RAWX,
-        STEP_VERSION,
         STEP_RTK_MOVBASE, // setup moving baseline
         STEP_TIM_TM2,
         STEP_M10,
@@ -898,6 +901,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
     static const config_list config_M10[];
     static const config_list config_L5_ovrd_ena[];
     static const config_list config_L5_ovrd_dis[];
+    bool in_safeboot_mode;
 };
 
 #endif
diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h
@@ -105,6 +105,11 @@ class AP_GPS_Backend
         return gps.option_set(option);
     }
 
+    // GPS in safeboot mode
+    virtual bool in_recovery_mode(void) const {
+        return false;
+    }
+
 protected:
     AP_HAL::UARTDriver *port;           ///< UART we are attached to
     AP_GPS &gps;                        ///< access to frontend (for parameters)