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 <[email protected]>

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
 };
 

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];

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<HW_Baud>(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<uint8_t>(portIndex), static_cast<uint8_t>(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<uint8_t>(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<uint8_t>(HW_Baud::BAUD230K):
+        case static_cast<uint8_t>(HW_Baud::BAUD115K):
+            return true;
+        default:
+            return false;
+    }
+}
 #endif

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
     {