From cb4700f022e2b3e400c3b58b8090d7d16802a495 Mon Sep 17 00:00:00 2001
From: bastian2001 <bas.c.spr@gmail.com>
Date: Mon, 8 Apr 2024 01:10:02 +0200
Subject: [PATCH] prefer UBX over NMEA when both are supported

---
 radio/src/gps.cpp     | 11 +++++++++--
 radio/src/gps_ubx.cpp |  5 +----
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/radio/src/gps.cpp b/radio/src/gps.cpp
index e822a7b1e23..62d9bac1ee4 100644
--- a/radio/src/gps.cpp
+++ b/radio/src/gps.cpp
@@ -106,6 +106,7 @@ static void autodetectProtocol(uint8_t c)
 {
   static tmr10ms_t time;
   static uint8_t state = 0;
+  static tmr10ms_t firstPacketNMEA = 0;
 
   switch (state)  {
     case 0: // Init
@@ -113,7 +114,12 @@ static void autodetectProtocol(uint8_t c)
       state = 1;
     case 1: // Wait for a valid packet
       if (gpsNewFrameNMEA(c)) {
-        gpsProtocol = GPS_PROTOCOL_NMEA;
+        if (!firstPacketNMEA) {
+          firstPacketNMEA = time;
+        } else if (time - firstPacketNMEA > 200) {
+          // continuous stream of NMEA packets for 2 seconds, but no UBX packets
+          gpsProtocol = GPS_PROTOCOL_NMEA;
+        }
         state = 0;
         return;
       }
@@ -125,8 +131,9 @@ static void autodetectProtocol(uint8_t c)
       }
 
       uint32_t new_time = get_tmr10ms();
-      if (new_time - time > 20)  {
+      if (new_time - time > 50) {
         // No message received
+        firstPacketNMEA = 0;
         changeBaudrate();
         time = new_time;
       }
diff --git a/radio/src/gps_ubx.cpp b/radio/src/gps_ubx.cpp
index 1d3890dad90..f59273de069 100644
--- a/radio/src/gps_ubx.cpp
+++ b/radio/src/gps_ubx.cpp
@@ -129,10 +129,7 @@ static void configureGps(bool detect)
 {
   static int state = 0;
 
-  if (detect) {
-    state = 0;
-    return;
-  }
+  if (detect) state = 0;
 
   auto txCompleted = gpsSerialDrv->txCompleted;
   if (txCompleted && !txCompleted(gpsSerialCtx)) return;