Add support for FugroTimeOffset

Firmware/RTK_mosaic-T_Firmware/Begin.ino

@@ -273,7 +273,7 @@ void updateErrorLED()
 
 void updateLockLED()
 {
-    digitalWrite(pin_lockLED, (fabs(gnssClockBias_ms) < settings.rxClkBiasLockLimit_ms) ? HIGH : LOW);
+    digitalWrite(pin_lockLED, (fabs(tcxoClockBias_ms) < settings.rxClkBiasLockLimit_ms) ? HIGH : LOW);
 }
 
 // Depending on platform and previous power down state, set system state
@@ -507,6 +507,45 @@ void updateTCXO()
 {
     if (online.tcxo)
     {
-        myTCXO.setFrequencyByBiasMillis(gnssClockBias_ms, settings.Pk, settings.Ik);
+        myTCXO.setFrequencyByBiasMillis(tcxoClockBias_ms, settings.Pk, settings.Ik);
     }
 }
+
+// This function updates the tcxoClockBias_ms used to discipline the TCXO frequency
+// updateTCXOClockBias is only called by STATE_GNSS_FINETIME when gnssPVTUpdated was true
+// So we know that gnssClockBias_ms is valid
+// Use gnssClockBias_ms as the default
+// If we have Fugro from FugroTimeOffset, use that
+// If we have Galileo from FugroTimeOffset, use that
+void updateTCXOClockBias()
+{
+    tcxoClockBias_ms = gnssClockBias_ms; // Default to the PVTGeodetic RxClkBias
+    snprintf(rxClkBiasSource, sizeof(rxClkBiasSource), "PVT");
+
+    int ts = getFugroTimeSystemFromName("Fugro");
+    if (fugroClkBiases[ts].updated) // If we have the Fugro bias, use that
+    {
+        tcxoClockBias_ms = fugroClkBiases[ts].RxClkBias_ms;
+        fugroClkBiases[ts].updated = false;
+        snprintf(rxClkBiasSource, sizeof(rxClkBiasSource), "Fugro");
+    }
+
+    ts = getFugroTimeSystemFromName("Galileo");
+    if (fugroClkBiases[ts].updated) // If we have the Galileo bias, use that
+    {
+        tcxoClockBias_ms = fugroClkBiases[ts].RxClkBias_ms;
+        fugroClkBiases[ts].updated = false;
+        snprintf(rxClkBiasSource, sizeof(rxClkBiasSource), "Galileo");
+    }
+}
+
+int getFugroTimeSystemFromName(const char *name)
+{
+    for (int i = 0; i < NUM_FUGRO_CLK_BIASES; i++)
+    {
+        if (strcmp(name, fugroClkBiases[i].name) == 0)
+            return i;
+    }
+
+    return 0; // Should never happen!
+}

Firmware/RTK_mosaic-T_Firmware/Display.ino

@@ -161,15 +161,15 @@ void updateDisplay()
             oled->print(textLine);
             yPos += 8;
 
-            if ((gnssClockBias_ms >= 1.0) || (gnssClockBias_ms <= -1.0))
+            if ((tcxoClockBias_ms >= 1.0) || (tcxoClockBias_ms <= -1.0))
                 snprintf(textLine, sizeof(textLine), "Bias  %.3fms",
-                    (float)gnssClockBias_ms);
-            else if ((gnssClockBias_ms >= 0.001) || (gnssClockBias_ms <= -0.001))
+                    (float)tcxoClockBias_ms);
+            else if ((tcxoClockBias_ms >= 0.001) || (tcxoClockBias_ms <= -0.001))
                 snprintf(textLine, sizeof(textLine), "Bias  %.3fus",
-                    (float)(gnssClockBias_ms * 1000.0));
+                    (float)(tcxoClockBias_ms * 1000.0));
             else
                 snprintf(textLine, sizeof(textLine), "Bias  %.3fns",
-                    (float)(gnssClockBias_ms * 1000000.0));
+                    (float)(tcxoClockBias_ms * 1000000.0));
             oled->setCursor(0, yPos);
             oled->print(textLine);
 
@@ -217,6 +217,11 @@ void displayGNSSFail(uint16_t displayTime)
     displayMessage("GNSS Failed", displayTime);
 }
 
+void displayBadBias(uint16_t displayTime)
+{
+    displayMessage("Bad RxClkBias Restarting", displayTime);
+}
+
 void displayNoRingBuffer(uint16_t displayTime)
 {
     if (online.display == true)

Firmware/RTK_mosaic-T_Firmware/GNSS.ino

@@ -108,6 +108,24 @@ void beginGNSS()
     online.gnss = true;
 }
 
+// Kickstart the GNSS timing system - e.g. to resync on bad bias
+bool setTimingSystem()
+{
+    if (!online.gnss)
+        return false;
+
+    int retries = 3; // GNSS is already begun. We shouldn't need to retry.
+
+    while (!sendWithResponse("sts,auto\n\r", "TimingSystem") && (retries > 0)) // Set timing system to auto
+    {
+        systemPrintln("No response from mosaic. Retrying - with escape sequence...");
+        sendWithResponse("SSSSSSSSSSSSSSSSSSSS\n\r", "COM4>"); // Send escape sequence
+        retries--;
+    }
+
+    return (retries > 0);
+}
+
 // Initialize GNSS
 // Disable PPS. Set clock sync threshold. Set output messages. Copy config file.
 // This only needs to be done once.
@@ -159,7 +177,7 @@ bool initializeGNSS()
         return false;
     }
 
-    if (!sendWithResponse("sso, Stream2, COM1, IPStatus, OnChange\n\r", "SBFOutput"))
+    if (!sendWithResponse("sso, Stream2, COM1, IPStatus+FugroTimeOffset, OnChange\n\r", "SBFOutput"))
     {
         systemPrintln("GNSS FAIL (SBFOutput Stream2)");
         return false;

Firmware/RTK_mosaic-T_Firmware/NVM.ino

@@ -91,7 +91,9 @@ void recordSystemSettingsToFile(File *settingsFile)
     settingsFile->printf("%s=%0.6f\r\n", "ppsPulseWidth_ms", settings.ppsPulseWidth_ms);
 
     settingsFile->printf("%s=%ld\r\n", "tcxoControl", settings.tcxoControl);
+    settingsFile->printf("%s=%0.3e\r\n", "rxClkBiasInitialLimit_ms", settings.rxClkBiasInitialLimit_ms);
     settingsFile->printf("%s=%0.3e\r\n", "rxClkBiasLockLimit_ms", settings.rxClkBiasLockLimit_ms);
+    settingsFile->printf("%s=%d\r\n", "rxClkBiasLimitCount", settings.rxClkBiasLimitCount);
     settingsFile->printf("%s=%0.3e\r\n", "Pk", settings.Pk);
     settingsFile->printf("%s=%0.3e\r\n", "Ik", settings.Ik);
 
@@ -348,8 +350,12 @@ bool parseLine(char *str, Settings *settings)
 
     else if (strcmp(settingName, "tcxoControl") == 0)
         settings->tcxoControl = d;
+    else if (strcmp(settingName, "rxClkBiasInitialLimit_ms") == 0)
+        settings->rxClkBiasInitialLimit_ms = d;
     else if (strcmp(settingName, "rxClkBiasLockLimit_ms") == 0)
         settings->rxClkBiasLockLimit_ms = d;
+    else if (strcmp(settingName, "rxClkBiasLimitCount") == 0)
+        settings->rxClkBiasLimitCount = d;
     else if (strcmp(settingName, "Pk") == 0)
         settings->Pk = d;
     else if (strcmp(settingName, "Ik") == 0)

Firmware/RTK_mosaic-T_Firmware/RTK_mosaic-T_Firmware.ino

@@ -90,29 +90,55 @@ unsigned long syncRTCInterval = 1000; // To begin, sync RTC every second. Interv
 
 //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
 
-// These globals are updated regularly via the storePVTdata callback
-unsigned long gnssPVTArrivalMillis = 0;
-bool gnssPVTUpdated = false;
+// These globals are updated regularly via the SBF parser
+
+// ReceiverTime 5914
 unsigned long gnssTimeArrivalMillis = 0;
 bool gnssTimeUpdated[2] = { false, false }; // RTC, TCXO
-double gnssLatitude_d = 0.0;
-double gnssLongitude_d = 0.0;
-float gnssAltitude_m = 0.0;
 uint32_t gnssTOW_ms = 0;
 uint8_t gnssDay = 0;
 uint8_t gnssMonth = 0;
 uint16_t gnssYear = 0;
 uint8_t gnssHour = 0;
 uint8_t gnssMinute = 0;
 uint8_t gnssSecond = 0;
-uint8_t gnssTimeSys = 255; // Unknown
-uint8_t gnssError = 255; // Unknown
 bool gnssWNSet = false;
 bool gnssToWSet = false;
 bool gnssFineTime = false;
+
+// PVTGeodetic 4007
+unsigned long gnssPVTArrivalMillis = 0;
+bool gnssPVTUpdated = false;
+double gnssLatitude_d = 0.0;
+double gnssLongitude_d = 0.0;
+float gnssAltitude_m = 0.0;
+uint8_t gnssTimeSys = 255; // Unknown
+uint8_t gnssError = 255; // Unknown
 double gnssClockBias_ms = 0.0;
+
+// IPStatus 4058
 IPAddress gnssIP = IPAddress((uint32_t)0);
 
+// FugroTimeOffset 4255
+typedef struct {
+    const uint8_t TimeSystem;
+    const char name[8];
+    double RxClkBias_ms;
+    bool updated;
+} fugroClkBias;
+fugroClkBias fugroClkBiases[] = {
+    { 0, "GPS", 0.0, false },
+    { 1, "Galileo", 0.0, false },
+    { 3, "GLONASS", 0.0, false },
+    { 4, "BeiDou", 0.0, false },
+    { 5, "QZSS", 0.0, false },
+    { 100, "Fugro", 0.0, false },
+};
+#define NUM_FUGRO_CLK_BIASES (sizeof(fugroClkBiases) / sizeof(fugroClkBias))
+
+double tcxoClockBias_ms; // Updated by updateTCXOClockBias
+char rxClkBiasSource[8]; // PVT or Fugro or Galileo
+
 //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
 
 // GPS parse table
@@ -235,7 +261,6 @@ uint16_t failedParserMessages_SBF = 0;
 
 bool rtcSyncd = false;      // Set to true when the RTC has been sync'd
 bool ppsStarted = false;    // Set to true when PPS have started. Cleared by menu changes.
-int tcxoUpdates = 0;        // Keep count of TCXO control word updates. Save the control word every hour
 
 //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
 
@@ -264,7 +289,7 @@ volatile bool deadManWalking;
     settings.enableHeapReport = true;                   \
     settings.enableTaskReports = true;                  \
     settings.enablePrintState = true;                   \
-    settings.enablePrintConditions = 1;                 \
+    settings.enablePrintConditions = 2;                 \
     settings.enablePrintIdleTime = true;                \
     settings.enablePrintBadMessages = true;             \
     settings.enablePrintRingBufferOffsets = true;       \

Firmware/RTK_mosaic-T_Firmware/States.ino

@@ -89,26 +89,54 @@ void updateSystemState()
             {
                 gnssPVTUpdated = false;
 
-                updateLockLED();
+                updateTCXOClockBias(); // Update the tcxoClockBias_ms from the best source
+
+                updateLockLED(); // Update Lock LED based on tcxoClockBias_ms
                 updateErrorLED();
 
+                static uint8_t clkBiasLimitCount = 0;
+
                 // If the clock bias is < the lock limit, start PPS
-                if ((fabs(gnssClockBias_ms) < settings.rxClkBiasLockLimit_ms)
-                    && !ppsStarted && !gnssError)
+                if (fabs(tcxoClockBias_ms) < settings.rxClkBiasLockLimit_ms)
                 {
-                    if (configureGNSSPPS())
+                    if (clkBiasLimitCount < settings.rxClkBiasLimitCount)
+                        clkBiasLimitCount++;
+                    if (!ppsStarted && !gnssError && (clkBiasLimitCount >= settings.rxClkBiasLimitCount) && configureGNSSPPS())
+                    {
                         ppsStarted = true;
+                    }
+                }
+                else
+                {
+                    clkBiasLimitCount = 0;
                 }
 
-                if (gnssError)
-                    changeState(STATE_GNSS_ERROR_AFTER_FINETIME);
-            }
-            else if (gnssTimeUpdated[1]) // Wait for the time to be updated
-            {
-                gnssTimeUpdated[1] = false;
+                static uint8_t badBiasCount = 0;
+
+                // If the clock bias is > rxClkBiasInitialLimit_ms, reinitialize
+                if (fabs(tcxoClockBias_ms) > settings.rxClkBiasInitialLimit_ms)
+                {
+                    if (badBiasCount < settings.rxClkBiasLimitCount)
+                        badBiasCount++;
+                    if (badBiasCount >= settings.rxClkBiasLimitCount)
+                    {
+                        clkBiasLimitCount = 0; // Reset the counts before changing state
+                        badBiasCount = 0;
+                        displayBadBias(1000);
+                        setTimingSystem(); // Kickstart the timing system
+                        changeState(STATE_GNSS_CONFIGURED);
+                    }
+                }
+                else
+                {
+                    badBiasCount = 0;
+                }
 
-                // The message rate limits this to 1Hz
-                updateTCXO();
+                // Update the TCXO if the bias is OK. The message rate limits this to 1Hz
+                if (badBiasCount == 0)
+                    updateTCXO();
+
+                static int tcxoUpdates = 0; // Keep count of TCXO control word updates. Save the control word every hour
 
                 if (ppsStarted)
                 {
@@ -126,7 +154,18 @@ void updateSystemState()
                     }
                 }
                 else
+                {
                     tcxoUpdates = 0;
+                }
+
+                // Change state on error - stop updating the TCXO
+                if (gnssError)
+                {
+                    clkBiasLimitCount = 0; // Reset the counts before changing state
+                    badBiasCount = 0;
+                    tcxoUpdates = 0;
+                    changeState(STATE_GNSS_ERROR_AFTER_FINETIME);
+                }
             }
         }
         break;

Firmware/RTK_mosaic-T_Firmware/Tasks.ino

@@ -661,6 +661,35 @@ void processConsumerMessage(PARSE_STATE *parse, uint8_t type)
             theIP |= ((uint32_t)parse->buffer[35]) << 24;
             gnssIP = IPAddress(theIP);
         }
+        else if ((parse->message & 0x1FFF) == 4255) // FugroTimeOffset
+        {
+            int N = parse->buffer[14]; // Number of FugroTOSub sub-blocks in this block
+            int SBLength = parse->buffer[15]; // Length of a FugroTOSub sub-block
+
+            for (int b = 0; b < N; b++) // For each block
+            {
+                uint8_t TimeSystem = parse->buffer[20 + (b * SBLength) + 2];
+
+                union {
+                    double dbl;
+                    uint64_t unsigned64;
+                } dblUnsigned64;
+
+                dblUnsigned64.unsigned64 = 0;
+                for (int i = 0; i < 8; i++)
+                    dblUnsigned64.unsigned64 |= ((uint64_t)parse->buffer[20 + (b * SBLength) + 4 + i]) << (i * 8);
+
+                for (int TS = 0; TS < NUM_FUGRO_CLK_BIASES; TS++)
+                {
+                    if (fugroClkBiases[TS].TimeSystem == TimeSystem)
+                    {
+                        fugroClkBiases[TS].RxClkBias_ms = dblUnsigned64.dbl;
+                        fugroClkBiases[TS].updated = true;
+                        break;
+                    }
+                }
+            }
+        }
     }
 }