Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: SBF: Validate that configuration was accepted

Browse Source 
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
This commit is contained in:
Michael du Breuil 2017-09-21 16:20:40 -07:00 committed by Francisco Ferreira
parent 20988b9f05
commit 80b71b9ced
2 changed files with 58 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -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));
}

6
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -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;