From 561acb372e26c584ded5b760719803dbf91ac937 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Thu, 21 Sep 2017 19:49:41 -0700
Subject: [PATCH] AP_GPS: SBF send port enable string on start

Also prohibts SBF, GSOF, NOVA drivers from getting config strings not
meant for them
---
 libraries/AP_GPS/AP_GPS.cpp     |  4 ++--
 libraries/AP_GPS/AP_GPS_SBF.cpp | 18 +++++++++++++-----
 libraries/AP_GPS/AP_GPS_SBF.h   |  3 +++
 3 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index fc6274cd93..54c37e0e81 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -501,12 +501,12 @@ void AP_GPS::detect_instance(uint8_t instance)
         _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
         dstate->last_baud_change_ms = now;
 
-        if (_auto_config == GPS_AUTO_CONFIG_ENABLE) {
+        if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) {
             send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
         }
     }
 
-    if (_auto_config == GPS_AUTO_CONFIG_ENABLE) {
+    if (_auto_config == GPS_AUTO_CONFIG_ENABLE && new_gps == nullptr) {
         send_blob_update(instance);
     }
 
diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp
index 702a18b6d3..76e5be8a7f 100644
--- a/libraries/AP_GPS/AP_GPS_SBF.cpp
+++ b/libraries/AP_GPS/AP_GPS_SBF.cpp
@@ -55,7 +55,8 @@ 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*)_initialisation_blob[0], strlen(_initialisation_blob[0]));
+    port->write((const uint8_t*)_port_enable, strlen(_port_enable));
+    _config_last_ack_time = AP_HAL::millis();
 }
 
 // Process all bytes available from the stream
@@ -70,10 +71,16 @@ AP_GPS_SBF::read(void)
         const char *init_str = _initialisation_blob[_init_blob_index];
 
         if (now > _init_blob_time) {
-            Debug("SBF sending init blob: %s\n", init_str);
-            port->write((const uint8_t*)init_str, strlen(init_str));
-            // if this is too low a race condition on start occurs and the GPS isn't detected
-            _init_blob_time = now + 2000;
+            if (now > _config_last_ack_time + 2500) {
+                // 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 {
+                Debug("SBF sending init string: %s", init_str);
+                port->write((const uint8_t*)init_str, strlen(init_str));
+            }
+            _init_blob_time = now + 1000;
         }
     }
 
@@ -194,6 +201,7 @@ AP_GPS_SBF::parse(uint8_t temp)
                                      sbf_msg.read - SBF_EXCESS_COMMAND_BYTES)) {
                             Debug("SBF Ack Command: %s\n", sbf_msg.data.bytes);
                             _init_blob_index++;
+                            _config_last_ack_time = AP_HAL::millis();
                         } else {
                             Debug("SBF Ack command (unexpected): %s\n", sbf_msg.data.bytes);
                         }
diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h
index 9665e54ed9..0126c0782b 100644
--- a/libraries/AP_GPS/AP_GPS_SBF.h
+++ b/libraries/AP_GPS/AP_GPS_SBF.h
@@ -65,6 +65,9 @@ private:
     "sem, PVT, 5\n",
     "spm, Rover, all\n",
     "sso, Stream2, Dsk1, postprocess+event, msec100\n"};
+    uint32_t _config_last_ack_time;
+
+    const char* _port_enable = "\nSSSSSSSSSS\n";
    
     uint32_t crc_error_counter = 0;
     uint32_t last_injected_data_ms = 0;