AP_GPS: Move init logic from constructor to class

Signed-off-by: Ryan Friedman <[email protected]>
ArduPilot · Nov 14, 2023 · 48144dd · 48144dd
1 parent b6ba93b
commit 48144dd
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 24 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp
@@ -58,44 +58,56 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
 {
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_Overview.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257COverview%257C_____0
     static_assert(ARRAY_SIZE(gsofmsgreq) <= 10, "The maximum number of outputs allowed with GSOF is 10.");
-
-    msg.state = Msg_Parser::State::STARTTX;
-
-    constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2);
-    gps._com_port[state.instance].set_default(default_com_port);
-    const auto com_port = gps._com_port[state.instance].get();
-    if (!validate_com_port(com_port)) {
-        // The user parameter for COM port is not a valid GSOF port
-        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has invalid COM port setting of %d", state.instance, com_port);
-        return;
-    }
-    const HW_Baud baud = HW_Baud::BAUD115K;
-    // Start by disabling output during config
-    [[maybe_unused]] const auto output_disabled = disableOutput(static_cast<HW_Port>(com_port));
-    is_baud_configured = requestBaud(static_cast<HW_Port>(com_port), baud);
+    static_assert(sizeof(gsofmsgreq) != 0, "gsofmsgreq is not empty");
 }
 
 // Process all bytes available from the stream
 //
 bool
 AP_GPS_GSOF::read(void)
 {
+
     if (!is_baud_configured) {
         // Debug("Baud not configured, not ready to read()");
-        return false;
-    }
 
-    bool gsof_configured = true;
-
-    static_assert(sizeof(gsofmsgreq) != 0, "gsofmsgreq is not empty");
-    while (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
+        constexpr uint8_t default_com_port = static_cast<uint8_t>(HW_Port::COM2);
+        gps._com_port[state.instance].set_default(default_com_port);
         const auto com_port = gps._com_port[state.instance].get();
         if (!validate_com_port(com_port)) {
             // The user parameter for COM port is not a valid GSOF port
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has invalid COM port setting of %d", state.instance, com_port);
+            return false;
+        }
+        const HW_Baud baud = HW_Baud::BAUD115K;
+        // Start by disabling output during config
+        if (!disableOutput(static_cast<HW_Port>(com_port))) {
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d cannot disable output on port %d", state.instance, com_port);
             return false;
         }
 
-        gsof_configured &= requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
+        is_baud_configured = requestBaud(static_cast<HW_Port>(com_port), baud);
+        if (!is_baud_configured) {
+            // Don't try further configuration if the baud request fails.
+            return false;
+        }
+    }
+
+    while (gsofmsgreq_index < sizeof(gsofmsgreq)) {
+        const auto com_port = gps._com_port[state.instance].get();
+        if (!validate_com_port(com_port)) {
+            // The user parameter for COM port is not a valid GSOF port.
+            return false;
+        }
+
+        const auto success = requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
+        if (!success) {
+            // Try again to do the rest of the configuration next time.
+            return false;
+        } else {
+            if (gsofmsgreq_index == sizeof(gsofmsgreq) - 1) {
+                gsof_configured = true;
+            }
+        }
         gsofmsgreq_index++;
     }
     if (!gsof_configured) {

diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/libraries/AP_GPS/AP_GPS_GSOF.h
@@ -108,7 +108,7 @@ class AP_GPS_GSOF : public AP_GPS_Backend
             ENDTX
         };
 
-        State state;
+        State state = State::STARTTX;
 
         uint8_t status;
         uint8_t packettype;
@@ -142,6 +142,6 @@ class AP_GPS_GSOF : public AP_GPS_Backend
     uint8_t gsofmsgreq_index;
     const uint8_t gsofmsgreq[5] = {1,2,8,9,12};
     bool is_baud_configured {false};
-    bool is_configured {false};
+    bool gsof_configured {false};
 };
 #endif