Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Don't send SBF config unless there is free space in the port

Browse Source
This commit is contained in:
Michael du Breuil 2021-02-15 13:00:09 -07:00 committed by WickedShell
parent c0da87c066
commit 4c4e613f6a
1 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -91,10 +91,13 @@ AP_GPS_SBF::read(void)
uint32_t now = AP_HAL::millis();
if (now > _init_blob_time) {
if (now > _config_last_ack_time + 2000) {
const size_t port_enable_len = strlen(_port_enable);
if (port_enable_len <= port->txspace()) {
// 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));
port->write((const uint8_t*)_port_enable, port_enable_len);
_config_last_ack_time = now;
}
} else if (readyForCommand) {
if (config_string == nullptr) {
switch (config_step) {
@ -126,12 +129,15 @@ AP_GPS_SBF::read(void)
}
if (config_string != nullptr) {
const size_t config_length = strlen(config_string);
if (config_length <= port->txspace()) {
Debug("SBF sending init string: %s", config_string);
port->write((const uint8_t*)config_string, strlen(config_string));
port->write((const uint8_t*)config_string, config_length);
readyForCommand = false;
}
}
}
}
} else if (gps._raw_data == 2) { // only manage disarm/rearms when the user opts into it
if (hal.util->get_soft_armed()) {
_has_been_armed = true;