diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index af79dd2339bca7..2df59bb828922e 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -74,8 +74,13 @@ AP_GPS_GSOF::read(void) const uint32_t now = AP_HAL::millis(); if (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; + } if (now > gsofmsg_time) { - requestGSOF(gsofmsgreq[gsofmsgreq_index], HW_Port::COM2, Output_Rate::FREQ_10_HZ); + requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast(com_port), Output_Rate::FREQ_10_HZ); gsofmsg_time = now + 110; gsofmsgreq_index++; } @@ -347,4 +352,16 @@ AP_GPS_GSOF::process_message(void) return false; } + +bool +AP_GPS_GSOF::validate_com_port(const uint8_t com_port) const { + switch(com_port) { + case static_cast(HW_Port::COM1): + case static_cast(HW_Port::COM2): + return true; + default: + return false; + } +} + #endif diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/libraries/AP_GPS/AP_GPS_GSOF.h index fe3b0cf8f6e64e..998a644de189cc 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.h +++ b/libraries/AP_GPS/AP_GPS_GSOF.h @@ -72,6 +72,7 @@ class AP_GPS_GSOF : public AP_GPS_Backend uint16_t SwapUint16(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED; bool validate_baud(const uint8_t baud) const WARN_IF_UNUSED; + bool validate_com_port(const uint8_t com_port) const WARN_IF_UNUSED; struct Msg_Parser {