From 80b71b9ced0faf46a40a53a2fcef950a0dcee0a5 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Thu, 21 Sep 2017 16:20:40 -0700 Subject: [PATCH] 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 | 63 ++++++++++++++++++++++++++++----- libraries/AP_GPS/AP_GPS_SBF.h | 6 ++-- 2 files changed, 58 insertions(+), 11 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 03d0243d64..f250e139d1 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/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; - sbf_msg.sbf_state = sbf_msg_parser_t::PREAMBLE1; + Debug("SBF got a response\n"); + 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)); } diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index edee33d3e4..0ad5510e41 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/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;