From d3a26935e906779d56b58831258a30fb853884db Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Thu, 2 Nov 2023 22:42:20 -0600 Subject: [PATCH] AP_GPS: Add configurable baud rate/port for AP_GPS_GSOF * Add in ability to support 115k and 230k baud rates through AP params * Default behavior is preserved at 115k * Removed support (bug?) for trying to output GSOF on the NMEA port * Modified function signature to take in port as a param * Added a baud rate validation helper to validate the AP param is a supported baud rate Signed-off-by: Ryan Friedman --- libraries/AP_GPS/AP_GPS.cpp | 18 +++++++++++++++ libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_GSOF.cpp | 39 ++++++++++++++++++++++++-------- libraries/AP_GPS/AP_GPS_GSOF.h | 29 ++++++++++++++++++++++-- 4 files changed, 75 insertions(+), 12 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index a085c021f283cc..62c893f16a7eb7 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -430,6 +430,24 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { #endif // GPS_MAX_RECEIVERS > 1 #endif // HAL_ENABLE_DRONECAN_DRIVERS +#if AP_GPS_GSOF_ENABLED + // @Param: _GSOF_BAUD1 + // @DisplayName: Baud rate for the first GSOF GPS + // @Description: What baud rate to configure the first GSOF GPS to + // @Values: 7:115k, 11:230k + // @User: Advanced + AP_GROUPINFO("_GSOF_BAUD1", 32, AP_GPS, _gsof_baud[0], 7), + +#if GPS_MAX_RECEIVERS > 1 + // @Param: _GSOF_BAUD2 + // @DisplayName: Baud rate for the second GSOF GPS + // @Description: What baud rate to configure the second GSOF GPS to + // @Values: 7:115k, 11:230k + // @User: Advanced + AP_GROUPINFO("_GSOF_BAUD2", 33, AP_GPS, _gsof_baud[1], 7), +#endif // GPS_MAX_RECEIVERS > 1 +#endif //AP_GPS_GSOF_ENABLED + AP_GROUPEND }; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index a097e25138ee29..88b9ba32ce2ff3 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -608,6 +608,7 @@ class AP_GPS AP_Float _blend_tc; AP_Int16 _driver_options; AP_Int8 _primary; + AP_Int8 _gsof_baud[GPS_MAX_RECEIVERS]; #if HAL_ENABLE_DRONECAN_DRIVERS AP_Int32 _node_id[GPS_MAX_RECEIVERS]; AP_Int32 _override_node_id[GPS_MAX_RECEIVERS]; diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index 248e38ef139201..6fdbbca282e975 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -17,6 +17,14 @@ // Trimble GPS driver for ArduPilot. // Code by Michael Oborne // +// Usage in SITL with hardware for debugging: +// sim_vehicle.py -v Plane -A "--serial3=uart:/dev/ttyUSB0" --console --map -DG +// param set GPS_TYPE 11 // GSOF +// param set SERIAL3_PROTOCOL 5 // GPS +// +// By default, 115k is used. For low-noise vehicles, switch to 230k like so +// param set GPS_GSOF_BAUD1 11 // 230k +// #define ALLOW_DOUBLE_MATH_FUNCTIONS @@ -52,10 +60,11 @@ 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); + const auto baud = gps._gsof_baud[state.instance].get(); + if (!validate_baud(baud)) { + return; + } + requestBaud(HW_Port::COM2, static_cast(baud)); const uint32_t now = AP_HAL::millis(); gsofmsg_time = now + 110; @@ -70,8 +79,7 @@ AP_GPS_GSOF::read(void) if (gsofmsgreq_index < (sizeof(gsofmsgreq))) { if (now > gsofmsg_time) { - requestGSOF(gsofmsgreq[gsofmsgreq_index], 0); - requestGSOF(gsofmsgreq[gsofmsgreq_index], 3); + requestGSOF(gsofmsgreq[gsofmsgreq_index], HW_Port::COM2); gsofmsg_time = now + 110; gsofmsgreq_index++; } @@ -146,11 +154,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, const HW_Baud baudRate) { 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), static_cast(baudRate), 0x00,0x00, // serial port baud format 0x00,0x03 }; // checksum @@ -167,11 +175,11 @@ AP_GPS_GSOF::requestBaud(const uint8_t portindex) } void -AP_GPS_GSOF::requestGSOF(const uint8_t messagetype, const uint8_t portindex) +AP_GPS_GSOF::requestGSOF(const uint8_t messagetype, const HW_Port portIndex) { uint8_t buffer[21] = {0x02,0x00,0x64,0x0f,0x00,0x00,0x00, // application file record 0x03,0x00,0x01,0x00, // file control information block - 0x07,0x06,0x0a,portindex,0x01,0x00,0x01,0x00, // output message record + 0x07,0x06,0x0a,static_cast(portIndex),0x01,0x00,0x01,0x00, // output message record 0x00,0x03 }; // checksum @@ -344,4 +352,15 @@ AP_GPS_GSOF::process_message(void) return false; } + +bool +AP_GPS_GSOF::validate_baud(const uint8_t baud) const { + switch(baud) { + case static_cast(HW_Baud::BAUD230K): + case static_cast(HW_Baud::BAUD115K): + 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 8c8d56748a049c..28bc8076fde190 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.h +++ b/libraries/AP_GPS/AP_GPS_GSOF.h @@ -40,15 +40,40 @@ class AP_GPS_GSOF : public AP_GPS_Backend private: + // A subset of the port identifiers in the GSOF protocol that are used for serial. + // 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 + COM2 = 1, // TTL serial + }; + + // A subset of the supported baud rates in the GSOF protocol that are useful. + // These values are not documented in the API. + // The matches the GPS_GSOF_BAUD parameter. + enum class HW_Baud { + BAUD115K = 0x07, + BAUD230K = 0x0B, + }; + bool parse(const uint8_t temp) WARN_IF_UNUSED; bool process_message() WARN_IF_UNUSED; - void requestBaud(const uint8_t portindex); - void requestGSOF(const uint8_t messagetype, const uint8_t portindex); + + // Send a request to the GPS to configure its baud rate on a certain (serial) 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. + bool requestBaud(const HW_Port portIndex, const HW_Baud baudRate) WARN_IF_UNUSED; + // Send a request to the GPS to enable a message type on the port. + bool requestGSOF(const uint8_t messagetype, const HW_Port portIndex) WARN_IF_UNUSED; + double SwapDouble(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED; float SwapFloat(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED; uint32_t SwapUint32(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED; 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; + struct Msg_Parser {