AP_GPS: Support configuring the SBF com port

libraries/AP_GPS/AP_GPS.cpp

@@ -299,6 +299,26 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     AP_GROUPINFO("DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
 #endif
 
+    // @Param: COM_PORT
+    // @DisplayName: GPS physical COM port
+    // @Description: The physical COM port on the connected device, currently only applies to SBF GPS
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("COM_PORT", 23, AP_GPS, _com_port[0], 1),
+
+#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
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("COM_PORT2", 24, AP_GPS, _com_port[1], 1),
+#endif
+
     AP_GROUPEND
 };
 

libraries/AP_GPS/AP_GPS.h

@@ -500,6 +500,7 @@ protected:
     AP_Int8 _auto_config;
     AP_Vector3f _antenna_offset[GPS_MAX_RECEIVERS];
     AP_Int16 _delay_ms[GPS_MAX_RECEIVERS];
+    AP_Int8  _com_port[GPS_MAX_RECEIVERS];
     AP_Int8 _blend_mask;
     AP_Float _blend_tc;
     AP_Int16 _driver_options;

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -23,6 +23,7 @@
 #include "AP_GPS.h"
 #include "AP_GPS_SBF.h"
 #include <GCS_MAVLink/GCS.h>
+#include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -40,6 +41,10 @@ do {                                            \
  # define Debug(fmt, args ...)
 #endif
 
+#ifndef GPS_SBF_STREAM_NUMBER
+  #define GPS_SBF_STREAM_NUMBER 1
+#endif
+
 #define SBF_EXCESS_COMMAND_BYTES 5 // 2 start bytes + validity byte + space byte + endline byte
 
 #define RX_ERROR_MASK (CONGESTION    | \
@@ -59,6 +64,10 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
     _config_last_ack_time = AP_HAL::millis();
 }
 
+AP_GPS_SBF::~AP_GPS_SBF (void) {
+    free(_initial_sso);
+}
+
 // Process all bytes available from the stream
 //
 bool
@@ -74,8 +83,6 @@ AP_GPS_SBF::read(void)
     if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
         if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
             uint32_t now = AP_HAL::millis();
-            const char *init_str = _initialisation_blob[_init_blob_index];
-
             if (now > _init_blob_time) {
                 if (now > _config_last_ack_time + 2500) {
                     // try to enable input on the GPS port if we have not made progress on configuring it
@@ -83,8 +90,25 @@ AP_GPS_SBF::read(void)
                     port->write((const uint8_t*)_port_enable, strlen(_port_enable));
                     _config_last_ack_time = now;
                 } else {
-                    Debug("SBF sending init string: %s", init_str);
+                    char *init_str = nullptr;
-                    port->write((const uint8_t*)init_str, strlen(init_str));
+                    if (!_validated_initial_sso) {
+                        if (_initial_sso == nullptr) {
+                            if (asprintf(&_initial_sso, "sso, Stream%d, COM%d%s",
+                                         (int)GPS_SBF_STREAM_NUMBER,
+                                         (int)gps._com_port[state.instance],
+                                         _sso_normal) == -1) {
+                                _initial_sso = nullptr;
+                            }
+                        }
+                        init_str = _initial_sso;
+                    } else {
+                        init_str = (char *)_initialisation_blob[_init_blob_index];
+                    }
+
+                    if (init_str != nullptr) {
+                        Debug("SBF sending init string: %s", init_str);
+                        port->write((const uint8_t*)init_str, strlen(init_str));
+                    }
                 }
                 _init_blob_time = now + 1000;
             }
@@ -220,11 +244,25 @@ AP_GPS_SBF::parse(uint8_t temp)
                 if (sbf_msg.data.bytes[0] == ':') {
                     // valid command, determine if it was the one we were trying
                     // to send in the configuration sequence
-                    if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
+                    const char * reference_blob = nullptr;
-                        if (!strncmp(_initialisation_blob[_init_blob_index], (char *)(sbf_msg.data.bytes + 2),
+                    if (!_validated_initial_sso) {
+                        reference_blob = _initial_sso;
+                    } else {
+                        if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
+                            reference_blob = _initialisation_blob[_init_blob_index];
+                        }
+                    }
+                    if (reference_blob != nullptr) {
+                        if (!strncmp(reference_blob, (char *)(sbf_msg.data.bytes + 2),
                                      sbf_msg.read - SBF_EXCESS_COMMAND_BYTES)) {
                             Debug("SBF Ack Command: %s\n", sbf_msg.data.bytes);
-                            _init_blob_index++;
+                            if (!_validated_initial_sso) {
+                                free(_initial_sso);
+                                _initial_sso = nullptr;
+                                _validated_initial_sso = true;
+                            } else {
+                                _init_blob_index++;
+                            }
                             _config_last_ack_time = AP_HAL::millis();
                         } else {
                             Debug("SBF Ack command (unexpected): %s\n", sbf_msg.data.bytes);

libraries/AP_GPS/AP_GPS_SBF.h

@@ -30,6 +30,7 @@ class AP_GPS_SBF : public AP_GPS_Backend
 {
 public:
     AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
+    ~AP_GPS_SBF();
 
     AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }
 
@@ -60,10 +61,12 @@ private:
     static const uint8_t SBF_PREAMBLE1 = '$';
     static const uint8_t SBF_PREAMBLE2 = '@';
 
-    uint8_t _init_blob_index = 0;
+    bool _validated_initial_sso;
-    uint32_t _init_blob_time = 0;
+    uint8_t _init_blob_index;
-    const char* _initialisation_blob[5] = {
+    uint32_t _init_blob_time;
-    "sso, Stream1, COM1, PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic, msec100\n",
+    char *_initial_sso;
+    const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic, msec100\n";
+    const char* _initialisation_blob[4] = {
     "srd, Moderate, UAV\n",
     "sem, PVT, 5\n",
     "spm, Rover, all\n",