From 5f3b1b92a4f2a2306c86561922c61ae90eaefb87 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Fri, 22 Jan 2021 12:35:19 -0700
Subject: [PATCH] AP_GPS: SBF don't send configuration until we've recieved a
 prompt

This fixes a configuration problem with SBF unit's where sometimes we
fail to detect the GPS unit continously, until it's been manually
configured. This was tested by doing a hard reset to the GPS unit.

This also now accepts a set of defines from the hwdef, or build
environment, which allows us to specify extra config options.
---
 libraries/AP_GPS/AP_GPS_SBF.cpp | 37 +++++++++++++++++++++++++++++----
 libraries/AP_GPS/AP_GPS_SBF.h   | 13 ++++++++++--
 2 files changed, 44 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp
index 9b64c4deea..bc3754c617 100644
--- a/libraries/AP_GPS/AP_GPS_SBF.cpp
+++ b/libraries/AP_GPS/AP_GPS_SBF.cpp
@@ -22,6 +22,7 @@
 #include "AP_GPS_SBF.h"
 #include <GCS_MAVLink/GCS.h>
 #include <stdio.h>
+#include <ctype.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -51,6 +52,8 @@ do {                                            \
                        INVALIDCONFIG | \
                        OUTOFGEOFENCE)
 
+constexpr const char *AP_GPS_SBF::portIdentifiers[];
+constexpr const char* AP_GPS_SBF::_initialisation_blob[];
 
 AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
                        AP_HAL::UARTDriver *_port) :
@@ -58,7 +61,6 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
 {
     sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
 
-    port->write((const uint8_t*)_port_enable, strlen(_port_enable));
     _config_last_ack_time = AP_HAL::millis();
 
     // if we ever parse RTK observations it will always be of type NED, so set it once
@@ -88,12 +90,12 @@ AP_GPS_SBF::read(void)
         if (_init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
             uint32_t now = AP_HAL::millis();
             if (now > _init_blob_time) {
-                if (now > _config_last_ack_time + 2500) {
+                if (now > _config_last_ack_time + 2000) {
                     // try to enable input on the GPS port if we have not made progress on configuring it
                     Debug("SBF Sending port enable");
                     port->write((const uint8_t*)_port_enable, strlen(_port_enable));
                     _config_last_ack_time = now;
-                } else {
+                } else if (readyForCommand) {
                     char *init_str = nullptr;
                     if (!_validated_initial_sso) {
                         if (_initial_sso == nullptr) {
@@ -112,9 +114,9 @@ AP_GPS_SBF::read(void)
                     if (init_str != nullptr) {
                         Debug("SBF sending init string: %s", init_str);
                         port->write((const uint8_t*)init_str, strlen(init_str));
+                        readyForCommand = false;
                     }
                 }
-                _init_blob_time = now + 1000;
             }
         } else if (gps._raw_data == 2) { // only manage disarm/rearms when the user opts into it
             if (hal.util->get_soft_armed()) {
@@ -155,6 +157,33 @@ AP_GPS_SBF::parse(uint8_t temp)
             if (temp == SBF_PREAMBLE1) {
                 sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE2;
                 sbf_msg.read = 0;
+            } else {
+                // attempt to detect command prompt
+                portIdentifier[portLength++] = (char)temp;
+                bool foundPossiblePort = false;
+                for (const char *portId : portIdentifiers) {
+                    if (strncmp(portId, portIdentifier, MIN(portLength, 3)) == 0) {
+                        // we found one of the COM/USB/IP related ports
+                        if (portLength == 4) {
+                            // validate that we have an ascii number
+                            if (isdigit((char)temp)) {
+                                foundPossiblePort = true;
+                                break;
+                            }
+                        } else if (portLength >= sizeof(portIdentifier)) {
+                            if ((char)temp == '>') {
+                                readyForCommand = true;
+                                Debug("SBF: Ready for command");
+                            }
+                        } else {
+                            foundPossiblePort = true;
+                        }
+                        break;
+                    }
+                }
+                if (!foundPossiblePort) {
+                    portLength = 0;
+                }
             }
             break;
         case sbf_msg_parser_t::PREAMBLE2:
diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h
index f52a3d4713..93104d90e6 100644
--- a/libraries/AP_GPS/AP_GPS_SBF.h
+++ b/libraries/AP_GPS/AP_GPS_SBF.h
@@ -68,11 +68,15 @@ private:
     uint32_t _init_blob_time;
     char *_initial_sso;
     const char* _sso_normal = ", PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod, msec100\n";
-    const char* _initialisation_blob[4] = {
+    static constexpr const char* _initialisation_blob[] = {
     "srd, Moderate, UAV\n",
     "sem, PVT, 5\n",
     "spm, Rover, all\n",
-    "sso, Stream2, Dsk1, postprocess+event+comment+ReceiverStatus, msec100\n"};
+    "sso, Stream2, Dsk1, postprocess+event+comment+ReceiverStatus, msec100\n",
+#if defined (GPS_SBF_EXTRA_CONFIG)
+    GPS_SBF_EXTRA_CONFIG
+#endif
+    };
     uint32_t _config_last_ack_time;
 
     const char* _port_enable = "\nSSSSSSSSSS\n";
@@ -239,4 +243,9 @@ private:
         INVALIDCONFIG = (1 << 10),  // set if one or more configuration file (permission or channel configuration) is invalid or absent.
         OUTOFGEOFENCE = (1 << 11),  // set if the receiver is currently out of its permitted region of operation (geo-fencing).
     };
+
+    static constexpr const char *portIdentifiers[] = { "COM", "USB", "IP1", "NTR", "IPS", "IPR" };
+    char portIdentifier[5];
+    uint8_t portLength;
+    bool readyForCommand;
 };