Revert "AP_GPS: Move init logic from constructor to class"

This reverts commit 5f84cd8.
ArduPilot · Nov 28, 2023 · b1333c6 · b1333c6
1 parent 35b52a4
commit b1333c6
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 36 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp
@@ -59,56 +59,44 @@ 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.");
-    static_assert(sizeof(gsofmsgreq) != 0, "gsofmsgreq is not empty");
+
+    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);
 }
 
 // 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()");
-
-        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;
-        }
-
-        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;
-        }
+        return false;
     }
 
-    while (gsofmsgreq_index < sizeof(gsofmsgreq)) {
+    bool gsof_configured = true;
+
+    static_assert(sizeof(gsofmsgreq) != 0, "gsofmsgreq is not empty");
+    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.
+            // 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;
-            }
-        }
+        gsof_configured &= requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
         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::STARTTX;
+        State state;
 
         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 gsof_configured {false};
+    bool is_configured {false};
 };
 #endif