diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 9b15335435200..b67fea52eb88b 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -770,11 +770,15 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) // 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; + if (dstate->probe_baud == 0) { + dstate->probe_baud = _port[instance]->get_baud_rate(); + } else { + dstate->current_baud++; + if (dstate->current_baud == ARRAY_SIZE(_baudrates)) { + dstate->current_baud = 0; + } + dstate->probe_baud = _baudrates[dstate->current_baud]; } - uint32_t baudrate = _baudrates[dstate->current_baud]; uint16_t rx_size=0, tx_size=0; if (_type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) { tx_size = 2048; @@ -782,7 +786,7 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) if (_type[instance] == GPS_TYPE_UBLOX_RTK_BASE) { rx_size = 2048; } - _port[instance]->begin(baudrate, rx_size, tx_size); + _port[instance]->begin(dstate->probe_baud, rx_size, tx_size); _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); dstate->last_baud_change_ms = now; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 1427626957926..ac7bfbd2656b9 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -677,6 +677,7 @@ class AP_GPS struct detect_state { uint32_t last_baud_change_ms; uint8_t current_baud; + uint32_t probe_baud; bool auto_detected_baud; struct UBLOX_detect_state ublox_detect_state; struct SIRF_detect_state sirf_detect_state; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 8147be24492cb..aa6d272947455 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -139,7 +139,7 @@ void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) cons "GPS %d: probing for %s at %d baud", instance + 1, name(), - int(gps._baudrates[dstate.current_baud])); + int(dstate.probe_baud)); } else { hal.util->snprintf(buffer, buflen, "GPS %d: specified as %s",