AP_GPS: SBF: Validate that configuration was accepted

Note: The config string of "spm, Rover, StandAlone+SBAS+DGPS+RTK\n" is
incompatible with AsteRx-M FW 3.6.3 and will result in refusing to
arm/pass configuration checks

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -39,6 +39,8 @@ do {                                            \
  # define Debug(fmt, args ...)
 #endif
 
+#define SBF_EXCESS_COMMAND_BYTES 5 // 2 start bytes + validity byte + space byte + endline byte
+
 AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
                        AP_HAL::UARTDriver *_port) :
     AP_GPS_Backend(_gps, _state, _port)
@@ -56,15 +58,11 @@ AP_GPS_SBF::read(void)
     uint32_t now = AP_HAL::millis();
 
     if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
-        _init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) {
+        _init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
         const char *init_str = _initialisation_blob[_init_blob_index];
-        if (validcommand) {
-            _init_blob_index++;
-            validcommand = false;
-            _init_blob_time = 0;
-        }
 
         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;
@@ -96,8 +94,8 @@ AP_GPS_SBF::parse(uint8_t temp)
             if (temp == SBF_PREAMBLE2) {
                 sbf_msg.sbf_state = sbf_msg_parser_t::CRC1;
             } else if (temp == 'R') {
-                validcommand = true;
+                Debug("SBF got a response\n");
-                sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
+                sbf_msg.sbf_state = sbf_msg_parser_t::COMMAND_LINE;
             }
             else
             {
@@ -164,6 +162,43 @@ AP_GPS_SBF::parse(uint8_t temp)
                 }
             }
             break;
+        case sbf_msg_parser_t::COMMAND_LINE:
+            if (sbf_msg.read < (sizeof(sbf_msg.data) - 1)) {
+                sbf_msg.data.bytes[sbf_msg.read] = temp;
+            } else {
+                // we don't have enough buffer to compare the commands
+                // most probable cause is that a user injected a longer command then
+                // we have buffer for, or it could be a corruption, either way we
+                // simply ignore the result
+                sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
+                break;
+            }
+            sbf_msg.read++;
+            if (temp == '\n') {
+                sbf_msg.data.bytes[sbf_msg.read] = 0;
+
+                // received the result, lets assess it
+                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)) {
+                        if (!strncmp(_initialisation_blob[_init_blob_index], (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++;
+                        } else {
+                            Debug("SBF Ack command (unexpected): %s\n", sbf_msg.data.bytes);
+                        }
+                    }
+                } else {
+                    // rejected command, send it out as a debug
+                    Debug("SBF NACK Command: %s\n", sbf_msg.data.bytes);
+                }
+                // resume normal parsing
+                sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1;
+                break;
+            }
+            break;
     }
 
     return false;
@@ -339,4 +374,16 @@ void AP_GPS_SBF::broadcast_configuration_failure_reason(void) const
             gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF is not currently logging", state.instance + 1);
         }
     }
+
+    if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE &&
+        _init_blob_index < ARRAY_SIZE(_initialisation_blob)) {
+        gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: SBF is not fully configured (%d/%d)", state.instance + 1,
+                        _init_blob_index, ARRAY_SIZE(_initialisation_blob));
+    }
+}
+
+bool AP_GPS_SBF::is_configured (void) {
+    return (!gps._raw_data || (RxState & SBF_DISK_ACTIVITY)) &&
+            (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE ||
+             _init_blob_index >= ARRAY_SIZE(_initialisation_blob));
 }

libraries/AP_GPS/AP_GPS_SBF.h

@@ -39,7 +39,7 @@ public:
 
     const char *name() const override { return "SBF"; }
 
-    bool is_configured (void) override { return (!gps._raw_data || (RxState & SBF_DISK_ACTIVITY)); }
+    bool is_configured (void) override;
 
     void broadcast_configuration_failure_reason(void) const override;
 
@@ -65,7 +65,6 @@ private:
    
     uint32_t crc_error_counter = 0;
     uint32_t last_injected_data_ms = 0;
-    bool validcommand = false;
     uint32_t RxState;
 
     enum sbf_ids {
@@ -174,7 +173,8 @@ private:
             BLOCKID2,
             LENGTH1,
             LENGTH2,
-            DATA
+            DATA,
+            COMMAND_LINE // used to parse command responses
         } sbf_state;
         uint16_t preamble;
         uint16_t crc;