diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index 11dcca752359f..1effd36754ef2 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -59,7 +59,21 @@ 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(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(com_port)); + is_baud_configured = requestBaud(static_cast(com_port), baud); } // Process all bytes available from the stream @@ -67,48 +81,22 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state, 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(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(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(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(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(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 index 82fcfa6f2a057..e44b2137a55c8 100644 --- 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