From ae13e7dd2ef4bbf7329d7aa9923bc23631330a7f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 28 Feb 2024 14:46:10 +1100
Subject: [PATCH] AP_GPS: do initial probe at default baudrate

this makes for much faster probe for most users
---
 libraries/AP_GPS/AP_GPS.cpp      | 14 +++++++++-----
 libraries/AP_GPS/AP_GPS.h        |  1 +
 libraries/AP_GPS/GPS_Backend.cpp |  2 +-
 3 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 273f381b5e..c34054fd6d 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -780,11 +780,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;
@@ -792,7 +796,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 5bb632b663..04b47a6513 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -680,6 +680,7 @@ private:
     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 451789fe40..67b8610a1c 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: detected as %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",