From d1dcf8f5a08fa09c2a9ca6ce0507d43464c956ae Mon Sep 17 00:00:00 2001
From: Tom Fifield <tom@tomfifield.net>
Date: Sat, 2 Nov 2024 11:42:53 +0800
Subject: [PATCH] Try GPS_BAUDRATE first, not only.

---
 src/gps/GPS.cpp | 48 ++++++++++++++++++------------------------------
 src/gps/GPS.h   |  5 +++++
 2 files changed, 23 insertions(+), 30 deletions(-)

diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp
index e6d93ded0c..b3f8d5a30a 100644
--- a/src/gps/GPS.cpp
+++ b/src/gps/GPS.cpp
@@ -426,41 +426,29 @@ bool GPS::setup()
     if (!didSerialInit) {
         int msglen = 0;
         if (tx_gpio && gnssModel == GNSS_MODEL_UNKNOWN) {
-
-            // if GPS_BAUDRATE is specified in variant, skip to the specified rate.
-            if (GPS_BAUDRATE_FIXED) {
-                gnssModel = probe(GPS_BAUDRATE);
-                if (gnssModel == GNSS_MODEL_UNKNOWN && probeTries == 1) {
-                    LOG_WARN("GPS probe failed, setting to %d", GPS_BAUDRATE);
-                    return true;
-                } else {
-                    ++probeTries;
-                    return false;
-                }
-            } else {
-                if (probeTries == 0) {
-                    LOG_DEBUG("Probing for GPS at %d", serialSpeeds[speedSelect]);
-                    gnssModel = probe(serialSpeeds[speedSelect]);
-                    if (gnssModel == GNSS_MODEL_UNKNOWN) {
-                        if (++speedSelect == sizeof(serialSpeeds) / sizeof(int)) {
-                            speedSelect = 0;
-                            ++probeTries;
-                        }
+            if (probeTries == 0) {
+                LOG_DEBUG("Probing for GPS at %d", serialSpeeds[speedSelect]);
+                gnssModel = probe(serialSpeeds[speedSelect]);
+                if (gnssModel == GNSS_MODEL_UNKNOWN) {
+                    if (++speedSelect == sizeof(serialSpeeds) / sizeof(int)) {
+                        speedSelect = 0;
+                        ++probeTries;
                     }
                 }
-                // Rare Serial Speeds
-                if (probeTries == 1) {
-                    LOG_DEBUG("Probing for GPS at %d", rareSerialSpeeds[speedSelect]);
-                    gnssModel = probe(rareSerialSpeeds[speedSelect]);
-                    if (gnssModel == GNSS_MODEL_UNKNOWN) {
-                        if (++speedSelect == sizeof(rareSerialSpeeds) / sizeof(int)) {
-                            LOG_WARN("Giving up on GPS probe and setting to %d", GPS_BAUDRATE);
-                            return true;
-                        }
+            }
+            // Rare Serial Speeds
+            if (probeTries == 1) {
+                LOG_DEBUG("Probing for GPS at %d", rareSerialSpeeds[speedSelect]);
+                gnssModel = probe(rareSerialSpeeds[speedSelect]);
+                if (gnssModel == GNSS_MODEL_UNKNOWN) {
+                    if (++speedSelect == sizeof(rareSerialSpeeds) / sizeof(int)) {
+                        LOG_WARN("Giving up on GPS probe and setting to %d", GPS_BAUDRATE);
+                        return true;
                     }
                 }
-                return false;
             }
+            return false;
+
         } else {
             gnssModel = GNSS_MODEL_UNKNOWN;
         }
diff --git a/src/gps/GPS.h b/src/gps/GPS.h
index f1abcf4b2f..b95d0f76ba 100644
--- a/src/gps/GPS.h
+++ b/src/gps/GPS.h
@@ -76,7 +76,12 @@ class GPS : private concurrency::OSThread
     uint8_t fixType = 0;      // fix type from GPGSA
 #endif
   private:
+#if GPS_BAUDRATE_FIXED
+    // if GPS_BAUDRATE is specified in variant, try that first.
+    const int serialSpeeds[4] = {GPS_BAUDRATE, 9600, 115200, 38400};
+#else
     const int serialSpeeds[3] = {9600, 115200, 38400};
+#endif
     const int rareSerialSpeeds[3] = {4800, 57600, GPS_BAUDRATE};
 
     uint32_t lastWakeStartMsec = 0, lastSleepStartMsec = 0, lastFixStartMsec = 0;