Create higher precision GST sentence

Work around for issue #513

Author: nseidle

Firmware/RTK_Everywhere/Developer.ino

@@ -277,6 +277,7 @@ void pointperfectPrintKeyInformation() {systemPrintln("**PPL Not Compiled**");}
 #ifndef COMPILE_LG290P
 
 void lg290pHandler(uint8_t * buffer, int length) {}
+void lg290pModifyGst(char *nmeaSentence, uint16_t *sentenceLength) {}
 
 #endif // COMPILE_LG290P
 

Firmware/RTK_Everywhere/GNSS_LG290P.ino

@@ -1080,7 +1080,6 @@ uint8_t GNSS_LG290P::getFixType()
 //----------------------------------------
 float GNSS_LG290P::getHorizontalAccuracy()
 {
-    // Coming soon from EPE
     if (online.gnss)
         return (_lg290p->get2DError());
     return 0;
@@ -2217,4 +2216,134 @@ bool lg290pMessageEnabled(char *nmeaSentence, int sentenceLength)
 
     // If we can't ID this message, allow it by default. The device configuration should control most message flow.
     return (true);
+}
+
+// Re-create the GST sentence to increase the horizontal and vertical precision number of decimals
+// ie, 0.0 becomes 0.009
+// See issue: https://github.com/sparkfun/SparkFun_RTK_Everywhere_Firmware/issues/513
+// $GNGST,223954.500,1.7,1.5,1.0,109.3,1.412,1.055,2.4*7E
+// Modifies the sentence length with new length
+void lg290pModifyGst(char *nmeaSentence, uint16_t *sentenceLength)
+{
+    // This issue currently only applies to LG290P firmware version 4
+    // Version 3 does not support GST messages. Version 5 should fix the issue.
+    if (lg290pFirmwareVersion != 4)
+        return;
+
+    if (online.gnss == false)
+        return;
+
+    GNSS_LG290P *lg290p = (GNSS_LG290P *)gnss;
+
+    // Identify sentence type
+    char sentenceType[strlen("GST") + 1] = {0};
+    strncpy(sentenceType, &nmeaSentence[3],
+            3); // Copy three letters, starting in spot 3. Null terminated from array initializer.
+
+    // We only care about GST sentences
+    if (strncmp(sentenceType, "GST", sizeof(sentenceType)) != 0)
+        return;
+
+    const int latitudeErrorComma = 6;
+    const int longitudeErrorComma = 7;
+    const int altitudeErrorComma = 8;
+
+    uint8_t latitudeStart = 0;
+    uint8_t latitudeStop = 0;
+    uint8_t longitudeStart = 0;
+    uint8_t longitudeStop = 0;
+    uint8_t altitudeStart = 0;
+    uint8_t checksumStart = 0;
+
+    if (settings.enableImuCompensationDebug == true && !inMainMenu)
+        systemPrintf("Original GNGST:\r\n%s\r\n", nmeaSentence);
+
+    int commaCount = 0;
+    for (int x = 0; x < strnlen(nmeaSentence, *sentenceLength); x++) // Assumes sentence is null terminated
+    {
+        if (nmeaSentence[x] == ',')
+        {
+            commaCount++;
+            if (commaCount == latitudeErrorComma)
+                latitudeStart = x + 1;
+            if (commaCount == latitudeErrorComma + 1)
+                latitudeStop = x;
+            if (commaCount == longitudeErrorComma)
+                longitudeStart = x + 1;
+            if (commaCount == longitudeErrorComma + 1)
+                longitudeStop = x;
+            if (commaCount == altitudeErrorComma)
+                altitudeStart = x + 1;
+        }
+        if (nmeaSentence[x] == '*')
+        {
+            checksumStart = x;
+            break;
+        }
+    }
+
+    if (latitudeStart == 0 || latitudeStop == 0 || longitudeStart == 0 || longitudeStop == 0 || altitudeStart == 0 ||
+        checksumStart == 0)
+    {
+        systemPrintln("Delineator not found");
+        return;
+    }
+
+    char newSentence[150] = {0};
+
+    if (sizeof(newSentence) < *sentenceLength)
+    {
+        systemPrintln("newSentence not big enough!");
+        return;
+    }
+
+    char errorString[strlen("0.000") + 1] = {0};
+
+    // strncat terminates
+
+    // Add start of message up to latitude
+    strncat(newSentence, nmeaSentence, latitudeStart);
+
+    // Convert error from EPE message to string. We don't have pure lat error, only 2D and 3D.
+    snprintf(errorString, sizeof(errorString), "%0.3f", lg290p->getHorizontalAccuracy());
+
+    // Add latitude error
+    strncat(newSentence, errorString, sizeof(newSentence) - 1);
+
+    // Add interstitial between end of lat and beginning of lon
+    strncat(newSentence, nmeaSentence + latitudeStop, longitudeStart - latitudeStop);
+
+    // Add same error for longitude error
+    strncat(newSentence, errorString, sizeof(newSentence) - 1);
+
+    // Add interstitial between end of lon and beginning of alt
+    strncat(newSentence, nmeaSentence + longitudeStop, altitudeStart - longitudeStop);
+
+    // Convert error from EPE message to string. We don't have pure altitude error, use double 2D error as stand-in.
+    snprintf(errorString, sizeof(errorString), "%0.3f", lg290p->getHorizontalAccuracy() * 2);
+
+    // Add altitude error. We don't really have altitude so use 3D error in its place.
+    strncat(newSentence, errorString, sizeof(newSentence) - 1);
+
+    // From: http://engineeringnotes.blogspot.com/2015/02/generate-crc-for-nmea-strings-arduino.html
+    byte CRC = 0; // XOR chars between '$' and '*'
+    for (byte x = 1; x < strlen(newSentence); x++)
+        CRC = CRC ^ newSentence[x];
+
+    // Convert CRC to string, add * and CR LF
+    snprintf(errorString, sizeof(errorString), "*%02X\r\n", CRC);
+
+    // Add CRC
+    strncat(newSentence, errorString, sizeof(newSentence) - 1);
+
+    // Increase length of sentence
+    *sentenceLength = strlen(newSentence);
+
+    // Overwrite the original NMEA
+    strncpy(nmeaSentence, newSentence, *sentenceLength);
+
+    nmeaSentence[*sentenceLength] = '\0'; // Terminate string
+
+    if (settings.enableImuCompensationDebug == true && !inMainMenu)
+        systemPrintf("Corrected GNGST:\r\n%s\r\n", nmeaSentence);
 }

Firmware/RTK_Everywhere/GNSS_None.h

@@ -429,6 +429,10 @@ class GNSS_None : public GNSS
     {
     }
 
+    void modifyGst(char *nmeaSentence, uint16_t *sentenceLength)
+    {
+    }
+
     // Print the module type and firmware version
     void printModuleInfo()
     {

Firmware/RTK_Everywhere/Tasks.ino

@@ -650,6 +650,9 @@ void processUart1Message(SEMP_PARSE_STATE *parse, uint16_t type)
                 parse->buffer[0] = 0;
                 parse->length = 0;
             }
+
+            // Gimme three decimals!
+            lg290pModifyGst((char *)parse->buffer, &parse->length);
         }
     }