mosaic-X5 SD : Replace ldi,DSK1 with exeSBFOnce,,DiskStatus

Author: PaulZC

Firmware/RTK_Everywhere/GNSS_Mosaic.h

@@ -580,13 +580,14 @@ class GNSS_MOSAIC : GNSS
     float  _latStdDev;
     float  _lonStdDev;
     bool   _receiverSetupSeen;
+    bool   _diskStatusSeen;
     std::vector<uint8_t> svInTracking;
     //std::vector<uint8_t> svInPVT;
 
     // Constructor
     GNSS_MOSAIC() : _determiningFixedPosition(true), _clkBias_ms(0),
         _latStdDev(999.9), _lonStdDev(999.9), _receiverSetupSeen(false),
-        _radioExtBytesReceived_millis(0),
+        _radioExtBytesReceived_millis(0), _diskStatusSeen(false),
          GNSS()
     {
             svInTracking.clear();
@@ -1051,6 +1052,8 @@ class GNSS_MOSAIC : GNSS
     bool updateSD();
 
     void waitSBFReceiverSetup(unsigned long timeout);
+
+    void waitSBFDiskStatus(unsigned long timeout);
 };
 
 #endif  // __GNSS_MOSAIC_H__

Firmware/RTK_Everywhere/GNSS_Mosaic.ino

@@ -2648,24 +2648,37 @@ bool GNSS_MOSAIC::updateSD()
     if (!previousCardPresent) // If the card is not present, skip the list disk info
         return false;
 
-    char diskInfo[200];
-    bool response =
-        sendAndWaitForIdle("ldi,DSK1\n\r", "DiskInfo", 1000, 25, &diskInfo[0], sizeof(diskInfo), false); // No debug
-    if (response)
-    {
-        char *ptr = strstr(diskInfo, " total=\"");
-        if (ptr == nullptr)
-            return false;
-        ptr += strlen(" total=\"");
-        sscanf(ptr, "%llu\"", &sdCardSize);
-        ptr = strstr(ptr, " free=\"");
-        if (ptr == nullptr)
-            return false;
-        ptr += strlen(" free=\"");
-        sscanf(ptr, "%llu\"", &sdFreeSpace);
-    }
+    // Use ldi,DSK1 to read the disk information
+    // This is inefficient as the DiskInfo lists all files on the SD card
+    // exeSBFOnce DiskStatus is much more efficient
+    //
+    // char diskInfo[200];
+    // bool response =
+    //     sendAndWaitForIdle("ldi,DSK1\n\r", "DiskInfo", 1000, 25, &diskInfo[0], sizeof(diskInfo), false); // No debug
+    // if (response)
+    // {
+    //     char *ptr = strstr(diskInfo, " total=\"");
+    //     if (ptr == nullptr)
+    //         return false;
+    //     ptr += strlen(" total=\"");
+    //     sscanf(ptr, "%llu\"", &sdCardSize);
+    //     ptr = strstr(ptr, " free=\"");
+    //     if (ptr == nullptr)
+    //         return false;
+    //     ptr += strlen(" free=\"");
+    //     sscanf(ptr, "%llu\"", &sdFreeSpace);
+    // }
 
-    return response;
+    _diskStatusSeen = false;
+
+    // Request the DiskStatus SBF block using a esoc (exeSBFOnce) command on COM4
+    String request = "esoc,COM4,DiskStatus\n\r";
+    serial2GNSS->write(request.c_str(), request.length());
+
+    // Wait for up to 1 second for the DiskStatus
+    waitSBFDiskStatus(1000);
+
+    return _diskStatusSeen;
 }
 
 //----------------------------------------
@@ -2702,6 +2715,40 @@ void GNSS_MOSAIC::waitSBFReceiverSetup(unsigned long timeout)
     sempStopParser(&sbfParse);
 }
 
+//----------------------------------------
+void GNSS_MOSAIC::waitSBFDiskStatus(unsigned long timeout)
+{
+    SEMP_PARSE_ROUTINE const sbfParserTable[] = {sempSbfPreamble};
+    const int sbfParserCount = sizeof(sbfParserTable) / sizeof(sbfParserTable[0]);
+    const char *const sbfParserNames[] = {
+        "SBF",
+    };
+    const int sbfParserNameCount = sizeof(sbfParserNames) / sizeof(sbfParserNames[0]);
+
+    SEMP_PARSE_STATE *sbfParse;
+
+    // Initialize the SBF parser for the mosaic-X5
+    sbfParse = sempBeginParser(sbfParserTable, sbfParserCount, sbfParserNames, sbfParserNameCount,
+                               0,                       // Scratchpad bytes
+                               500,                     // Buffer length
+                               processSBFDiskStatus,    // eom Call Back
+                               "Sbf");                  // Parser Name
+    if (!sbfParse)
+        reportFatalError("Failed to initialize the SBF parser");
+
+    unsigned long startTime = millis();
+    while ((millis() < (startTime + timeout)) && (_diskStatusSeen == false))
+    {
+        if (serial2GNSS->available())
+        {
+            // Update the parser state based on the incoming byte
+            sempParseNextByte(sbfParse, serial2GNSS->read());
+        }
+    }
+
+    sempStopParser(&sbfParse);
+}
+
 //==========================================================================
 // Parser support routines
 //==========================================================================
@@ -2798,6 +2845,37 @@ void processSBFReceiverSetup(SEMP_PARSE_STATE *parse, uint16_t type)
     }
 }
 
+//----------------------------------------
+void processSBFDiskStatus(SEMP_PARSE_STATE *parse, uint16_t type)
+{
+    // If this is DiskStatus, extract some data
+    if (sempSbfGetBlockNumber(parse) == 4059)
+    {
+        if (sempSbfGetU1(parse, 14) < 1) // Check N is at least 1
+            return;
+
+        uint64_t diskUsageMSB = sempSbfGetU2(parse, 20 + 2); // DiskUsageMSB
+        uint64_t diskUsageLSB = sempSbfGetU4(parse, 20 + 4); // DiskUsageLSB
+
+        if ((diskUsageMSB == 65535) && (diskUsageLSB == 4294967295))
+            return;
+
+        uint64_t diskSizeMB = sempSbfGetU4(parse, 20 + 8); // DiskSize in megabytes
+
+        if (diskSizeMB == 0)
+            return;
+
+        uint64_t diskUsage = (diskUsageMSB * 4294967296) + diskUsageLSB;
+        
+        sdCardSize = diskSizeMB * 1048576; // Convert to bytes
+
+        sdFreeSpace = sdCardSize - diskUsage;
+
+        GNSS_MOSAIC *mosaic = (GNSS_MOSAIC *)gnss;
+        mosaic->_diskStatusSeen = true;
+    }
+}
+
 //----------------------------------------
 // Call back from within the SBF parser, for end of valid message
 // Process a complete message incoming from parser