Don't skip GPS serial speeds, and always land on GPS_BAUDRATE (#5195)

* Don't skip GPS serial speeds, and always land on GPS_BAUDRATE

* Update log message to match.

* print the value instead

src/gps/GPS.cpp

@@ -421,7 +421,7 @@ bool GPS::setup()
         if (tx_gpio && gnssModel == GNSS_MODEL_UNKNOWN) {
 
             // if GPS_BAUDRATE is specified in variant (i.e. not 9600), skip to the specified rate.
-            if (speedSelect == 0 && GPS_BAUDRATE != serialSpeeds[speedSelect]) {
+            if (speedSelect == 0 && probeTries == 2 && GPS_BAUDRATE != serialSpeeds[speedSelect]) {
                 speedSelect = std::find(serialSpeeds, std::end(serialSpeeds), GPS_BAUDRATE) - serialSpeeds;
             }
 
@@ -431,7 +431,7 @@ bool GPS::setup()
                 if (++speedSelect == sizeof(serialSpeeds) / sizeof(int)) {
                     speedSelect = 0;
                     if (--probeTries == 0) {
-                        LOG_WARN("Giving up on GPS probe and setting to 9600.");
+                        LOG_WARN("Giving up on GPS probe and setting to %d", GPS_BAUDRATE);
                         return true;
                     }
                 }

src/gps/GPS.h

@@ -76,7 +76,7 @@ class GPS : private concurrency::OSThread
     uint8_t fixType = 0;      // fix type from GPGSA
 #endif
   private:
-    const int serialSpeeds[6] = {9600, 115200, 38400, 4800, 57600, 9600};
+    const int serialSpeeds[6] = {9600, 115200, 38400, 4800, 57600, GPS_BAUDRATE};
     uint32_t lastWakeStartMsec = 0, lastSleepStartMsec = 0, lastFixStartMsec = 0;
     uint32_t rx_gpio = 0;
     uint32_t tx_gpio = 0;