From b0ff614c36bafe0ffd9bdd32a8f42639f21d79c8 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Tue, 7 Nov 2023 08:06:42 -0700 Subject: [PATCH] AP_GPS: Use GPS_COM_PORT param in GSOF driver * Instead of hard coding to COM2, allow users to set it * The enum is confusing, so this needs a wiki entry * Use the same port in requestBAUD * If the user configures an invalid param, send an error * Add values for the GSOF COM ports * Fix bug in RS232 being port 3 instead of port 0 * Use set_default for the typical user value when the GSOF driver is run Signed-off-by: Ryan Friedman --- libraries/AP_GPS/AP_GPS.cpp | 5 +++-- libraries/AP_GPS/AP_GPS_GSOF.cpp | 37 ++++++++++++++++++++++++++------ libraries/AP_GPS/AP_GPS_GSOF.h | 8 ++++--- 3 files changed, 38 insertions(+), 12 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 0a5f3a7b6e1f7c..928cdfaa8492a2 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -356,17 +356,18 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { #if AP_GPS_SBF_ENABLED // @Param: _COM_PORT // @DisplayName: GPS physical COM port - // @Description: The physical COM port on the connected device, currently only applies to SBF GPS + // @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS // @Range: 0 10 // @Increment: 1 // @User: Advanced + // @values: 0:COM1(RS232) on GSOF, 1:COM2(TTL) on GSOF // @RebootRequired: True AP_GROUPINFO("_COM_PORT", 23, AP_GPS, _com_port[0], HAL_GPS_COM_PORT_DEFAULT), #if GPS_MAX_RECEIVERS > 1 // @Param: _COM_PORT2 // @DisplayName: GPS physical COM port - // @Description: The physical COM port on the connected device, currently only applies to SBF GPS + // @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS // @Range: 0 10 // @Increment: 1 // @User: Advanced diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index af79dd2339bca7..9de37cc4b71f48 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -29,6 +29,7 @@ #include "AP_GPS_GSOF.h" #include #include +#include #if AP_GPS_GSOF_ENABLED @@ -57,10 +58,15 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state, msg.state = Msg_Parser::State::STARTTX; - // baud request for port 0 - requestBaud(0); - // baud request for port 3 - requestBaud(3); + 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; + } + requestBaud(static_cast(com_port)); const uint32_t now = AP_HAL::millis(); gsofmsg_time = now + 110; @@ -74,8 +80,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++; } @@ -150,11 +161,11 @@ AP_GPS_GSOF::parse(const uint8_t temp) } void -AP_GPS_GSOF::requestBaud(const uint8_t portindex) +AP_GPS_GSOF::requestBaud(const HW_Port portindex) { uint8_t buffer[19] = {0x02,0x00,0x64,0x0d,0x00,0x00,0x00, // application file record 0x03, 0x00, 0x01, 0x00, // file control information block - 0x02, 0x04, portindex, 0x07, 0x00,0x00, // serial port baud format + 0x02, 0x04, static_cast(portindex), 0x07, 0x00,0x00, // serial port baud format 0x00,0x03 }; // checksum @@ -347,4 +358,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..a55d91c0a9198b 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.h +++ b/libraries/AP_GPS/AP_GPS_GSOF.h @@ -44,7 +44,7 @@ class AP_GPS_GSOF : public AP_GPS_Backend // Ethernet, USB, etc are not supported by the GPS driver at this time so they are omitted. // These values are not documented in the API. enum class HW_Port { - COM1 = 3, // RS232 serial + COM1 = 0, // RS232 serial COM2 = 1, // TTL serial }; @@ -58,12 +58,13 @@ class AP_GPS_GSOF : public AP_GPS_Backend bool parse(const uint8_t temp) WARN_IF_UNUSED; bool process_message() WARN_IF_UNUSED; - void requestBaud(const uint8_t portindex); - // Send a request to the GPS to enable a message type on the port at the specified rate. + // Send a request to the GPS to set the baud rate on the specified port. // Note - these request functions currently ignore the ACK from the device. // If the device is already sending serial traffic, there is no mechanism to prevent conflict. // According to the manufacturer, the best approach is to switch to ethernet. + void requestBaud(const HW_Port portIndex); + // Send a request to the GPS to enable a message type on the port at the specified rate. void requestGSOF(const uint8_t messageType, const HW_Port portIndex, const Output_Rate rateHz); double SwapDouble(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED; @@ -72,6 +73,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 {