From f3cc732e7dce6efb38217d1be80128954ad92bbb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Thomas=20G=C3=B6ttgens?= <tgoettgens@gmail.com>
Date: Tue, 17 May 2022 19:44:41 +0200
Subject: [PATCH] Print error codes and returns during UBX config

---
 src/gps/GPS.cpp | 60 +++++++++++++++++++++++++++++++++++++++++++------
 src/gps/GPS.h   |  2 ++
 2 files changed, 55 insertions(+), 7 deletions(-)

diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp
index fe0228222..a6cd31cc3 100644
--- a/src/gps/GPS.cpp
+++ b/src/gps/GPS.cpp
@@ -22,6 +22,52 @@ GPS *gps;
 /// only init that port once.
 static bool didSerialInit;
 
+bool GPS::getACK(uint8_t c, uint8_t i) {
+  uint8_t b;
+  uint8_t ack = 0;
+  uint8_t ackP[2] = {c, i};
+  uint8_t buf[250];
+  unsigned long startTime = millis();
+
+  buf[0] = 0xB5;
+  buf[1] = 0x62;
+  buf[2] = 0x05;
+  buf[3] = 0x01;
+  buf[4] = 0x02;
+  buf[5] = 0x00;
+  buf[8] = 0x00;
+  buf[9] = 0x00;
+
+  for (int i = 2; i < 6; i++) {
+    buf[8] += buf[i];
+    buf[9] += buf[8];
+  }
+
+  for (int i = 0; i < 2; i++) {
+    buf[6+i] = ackP[i];
+    buf[8] += buf[6+i];
+    buf[9] += buf[8];
+  }
+
+  while (1) {
+    if (ack > 9) {
+      return true;
+    }
+    if (millis() - startTime > 2000) {
+      return false;
+    }
+    if (_serial_gps->available()) {
+      b = _serial_gps->read();
+      if (b == buf[ack]) {
+        ack++;
+      }
+      else {
+        ack = 0;
+      }
+    }
+  }
+}
+
 bool GPS::setupGPS()
 {
     if (_serial_gps && !didSerialInit) {
@@ -64,49 +110,49 @@ bool GPS::setupGPS()
             0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x07, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00,
             0x91, 0xAF};
         _serial_gps->write(_message_nmea,sizeof(_message_nmea));
-        delay(250);
+        if (!getACK(0x06, 0x00)) DEBUG_MSG("WARNING: Unable to set UART.\n");
 
         // disable GGL
         byte _message_GGL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00,
             0xF0, 0x01, 0x01, 0x00, 0x01, 0x01, 0x01, 0x01,
             0x05,0x3A};
         _serial_gps->write(_message_GGL,sizeof(_message_GGL));
-        delay(250);
+        if (!getACK(0x06, 0x01)) DEBUG_MSG("WARNING: Unable to disable NMEA GGL.\n");
 
         // disable GSA
         byte _message_GSA[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00,
             0xF0, 0x02, 0x01, 0x00, 0x01, 0x01, 0x01, 0x01,
             0x06,0x41};
         _serial_gps->write(_message_GSA,sizeof(_message_GSA));
-        delay(250);
+        if (!getACK(0x06, 0x01)) DEBUG_MSG("WARNING: Unable to disable NMEA GSA.\n");
 
         // disable GSV
         byte _message_GSV[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00,
             0xF0, 0x03, 0x01, 0x00, 0x01, 0x01, 0x01, 0x01,
             0x07,0x48};
         _serial_gps->write(_message_GSV,sizeof(_message_GSV));
-        delay(250);
+        if (!getACK(0x06, 0x01)) DEBUG_MSG("WARNING: Unable to disable NMEA GSV.\n");
 
         // disable VTG
         byte _message_VTG[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00,
             0xF0, 0x05, 0x01, 0x00, 0x01, 0x01, 0x01, 0x01,
             0x09,0x56};
         _serial_gps->write(_message_VTG,sizeof(_message_VTG));
-        delay(250);
+        if (!getACK(0x06, 0x01)) DEBUG_MSG("WARNING: Unable to disable NMEA VTG.\n");
 
         // enable RMC
         byte _message_RMC[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00,
             0xF0, 0x04, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
             0x09,0x54};
         _serial_gps->write(_message_RMC,sizeof(_message_RMC));
-        delay(250);
+        if (!getACK(0x06, 0x01)) DEBUG_MSG("WARNING: Unable to enable NMEA RMC.\n");
 
         // enable GGA
         byte _message_GGA[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00,
             0xF0, 0x00, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
             0x05, 0x38};
         _serial_gps->write(_message_GGA,sizeof(_message_GGA));
-        delay(250);
+        if (!getACK(0x06, 0x01)) DEBUG_MSG("WARNING: Unable to enable NMEA GGA.\n");
 #endif
     }
 
diff --git a/src/gps/GPS.h b/src/gps/GPS.h
index 5724889fe..b4716af76 100644
--- a/src/gps/GPS.h
+++ b/src/gps/GPS.h
@@ -137,6 +137,8 @@ class GPS : private concurrency::OSThread
 
     GpsOperation getGpsOp() const;
 
+    bool getACK(uint8_t cl, uint8_t id);
+
     /**
      * Tell users we have new GPS readings
      */