From 11079e37bafd779dfa07edab41ed32b5ea15572e Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Thu, 27 Jun 2024 14:26:46 -0600
Subject: [PATCH] Add overlength debug messages

---
 Firmware/RTK_Everywhere/Tilt.ino       | 48 ++++++++++++++++++++++++--
 Firmware/RTK_Everywhere/menuSystem.ino |  2 --
 2 files changed, 45 insertions(+), 5 deletions(-)

diff --git a/Firmware/RTK_Everywhere/Tilt.ino b/Firmware/RTK_Everywhere/Tilt.ino
index e93b03421..460add5ed 100644
--- a/Firmware/RTK_Everywhere/Tilt.ino
+++ b/Firmware/RTK_Everywhere/Tilt.ino
@@ -473,6 +473,9 @@ void applyCompensationGNS(char *nmeaSentence, int arraySize)
     uint8_t undulationStop = 0;
     uint8_t checksumStart = 0;
 
+    if (settings.enableImuCompensationDebug == true && !inMainMenu)
+        systemPrintf("Original GNGNS:\r\n%s\r\n", nmeaSentence);
+
     int commaCount = 0;
     for (int x = 0; x < strnlen(nmeaSentence, arraySize); x++) // Assumes sentence is null terminated
     {
@@ -606,8 +609,17 @@ void applyCompensationGNS(char *nmeaSentence, int arraySize)
     // Add CRC
     strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
 
+    if (strlen(newSentence) > arraySize)
+    {
+        if (settings.enableImuCompensationDebug == true && !inMainMenu)
+            systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", arraySize, strlen(newSentence));
+    }
+
     // Overwrite the original NMEA
     strncpy(nmeaSentence, newSentence, arraySize);
+
+    if (settings.enableImuCompensationDebug == true && !inMainMenu)
+        systemPrintf("Compensated GNGNS:\r\n%s\r\n", nmeaSentence);
 }
 
 // Modify a GLL sentence with tilt compensation
@@ -619,6 +631,9 @@ void applyCompensationGLL(char *nmeaSentence, int arraySize)
     if (tiltIsCorrecting() == false)
         return;
 
+    if (settings.enableImuCompensationDebug == true && !inMainMenu)
+        systemPrintf("Original GNGLL:\r\n%s\r\n", nmeaSentence);
+
     char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence
 
     const int latitudeComma = 1;
@@ -700,8 +715,17 @@ void applyCompensationGLL(char *nmeaSentence, int arraySize)
     // Add CRC
     strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
 
+    if (strlen(newSentence) > arraySize)
+    {
+        if (settings.enableImuCompensationDebug == true && !inMainMenu)
+            systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", arraySize, strlen(newSentence));
+    }
+
     // Overwrite the original NMEA
     strncpy(nmeaSentence, newSentence, arraySize);
+
+    if (settings.enableImuCompensationDebug == true && !inMainMenu)
+        systemPrintf("Compensated GNGLL:\r\n%s\r\n", nmeaSentence);
 }
 
 // Modify a RMC sentence with tilt compensation
@@ -713,6 +737,9 @@ void applyCompensationRMC(char *nmeaSentence, int arraySize)
     if (tiltIsCorrecting() == false)
         return;
 
+    if (settings.enableImuCompensationDebug == true && !inMainMenu)
+        systemPrintf("Original GNRMC:\r\n%s\r\n", nmeaSentence);
+
     char coordinateStringDDMM[strlen("10511.12345678") + 1] = {0}; // UM980 outputs 8 decimals in GGA sentence
 
     const int latitudeComma = 3;
@@ -794,8 +821,17 @@ void applyCompensationRMC(char *nmeaSentence, int arraySize)
     // Add CRC
     strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
 
+    if (strlen(newSentence) > arraySize)
+    {
+        if (settings.enableImuCompensationDebug == true && !inMainMenu)
+            systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", arraySize, strlen(newSentence));
+    }
+
     // Overwrite the original NMEA
     strncpy(nmeaSentence, newSentence, arraySize);
+
+    if (settings.enableImuCompensationDebug == true && !inMainMenu)
+        systemPrintf("Compensated GNRMC:\r\n%s\r\n", nmeaSentence);
 }
 
 // Modify a GGA sentence with tilt compensation
@@ -825,7 +861,7 @@ void applyCompensationGGA(char *nmeaSentence, int arraySize)
     uint8_t checksumStart = 0;
 
     if (settings.enableImuCompensationDebug == true && !inMainMenu)
-        systemPrintf("Original GNGGA: %s\r\n", nmeaSentence);
+        systemPrintf("Original GNGGA:\r\n%s\r\n", nmeaSentence);
 
     int commaCount = 0;
     for (int x = 0; x < strnlen(nmeaSentence, arraySize); x++) // Assumes sentence is null terminated
@@ -941,7 +977,7 @@ void applyCompensationGGA(char *nmeaSentence, int arraySize)
     }
 
     // Convert altitude double to string
-    snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.3f", newAltitude);
+    snprintf(coordinateStringDDMM, sizeof(coordinateStringDDMM), "%0.4f", newAltitude);
 
     // Add tilt-compensated Altitude
     strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
@@ -960,11 +996,17 @@ void applyCompensationGGA(char *nmeaSentence, int arraySize)
     // Add CRC
     strncat(newSentence, coordinateStringDDMM, sizeof(newSentence) - 1);
 
+    if (strlen(newSentence) > arraySize)
+    {
+        if (settings.enableImuCompensationDebug == true && !inMainMenu)
+            systemPrintf("New compensated sentence too long! Orig: %d New: %d\r\n", arraySize, strlen(newSentence));
+    }
+
     // Overwrite the original NMEA
     strncpy(nmeaSentence, newSentence, arraySize);
 
     if (settings.enableImuCompensationDebug == true && !inMainMenu)
-        systemPrintf("Compensated GNGGA: %s\r\n", nmeaSentence);
+        systemPrintf("Compensated GNGGA:\r\n%s\r\n", nmeaSentence);
 }
 
 #endif // COMPILE_IM19_IMU
diff --git a/Firmware/RTK_Everywhere/menuSystem.ino b/Firmware/RTK_Everywhere/menuSystem.ino
index 690fed9a7..055f4d1ff 100644
--- a/Firmware/RTK_Everywhere/menuSystem.ino
+++ b/Firmware/RTK_Everywhere/menuSystem.ino
@@ -1385,9 +1385,7 @@ void menuInstrument()
 
             if (getNewSetting("Enter the antenna height (a.k.a. pole length) in meters", -15.0, 15.0, &antennaHeight) ==
                 INPUT_RESPONSE_VALID)
-            {
                 settings.antennaHeight_mm = truncf(antennaHeight * 1000.0);
-            }
         }
         else if (incoming == 2)
         {