AP_GPS: cycle through baud rates for SBF/GSOF/NOVA/SITL

This commit is contained in:
Peter Barker 2022-09-28 21:40:44 +10:00 committed by Peter Barker
parent a56ed66aa3
commit 3a3f9bdb84
1 changed files with 23 additions and 24 deletions

View File

@ -693,6 +693,29 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
// all remaining drivers automatically cycle through baud rates to detect
// the correct baud rate, and should have the selected baud broadcast
dstate->auto_detected_baud = true;
const uint32_t now = AP_HAL::millis();
if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) {
// try the next baud rate
// incrementing like this will skip the first element in array of bauds
// this is okay, and relied upon
dstate->current_baud++;
if (dstate->current_baud == ARRAY_SIZE(_baudrates)) {
dstate->current_baud = 0;
}
uint32_t baudrate = _baudrates[dstate->current_baud];
_port[instance]->begin(baudrate);
_port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
dstate->last_baud_change_ms = now;
if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
send_blob_start(instance);
}
}
if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
send_blob_update(instance);
}
switch (_type[instance]) {
#if AP_GPS_SBF_ENABLED
@ -718,30 +741,6 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance)
break;
}
const uint32_t now = AP_HAL::millis();
if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) {
// try the next baud rate
// incrementing like this will skip the first element in array of bauds
// this is okay, and relied upon
dstate->current_baud++;
if (dstate->current_baud == ARRAY_SIZE(_baudrates)) {
dstate->current_baud = 0;
}
uint32_t baudrate = _baudrates[dstate->current_baud];
_port[instance]->begin(baudrate);
_port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
dstate->last_baud_change_ms = now;
if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
send_blob_start(instance);
}
}
if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) {
send_blob_update(instance);
}
if (initblob_state[instance].remaining != 0) {
// don't run detection engines if we haven't sent out the initblobs
return nullptr;