Add storage for multiple ESF MEAS callback copies

Author: PaulZC

src/u-blox_GNSS.cpp

@@ -469,7 +469,7 @@ void DevUBLOXGNSS::end(void)
   {
     if (packetUBXESFMEAS->callbackData != nullptr)
     {
-      delete packetUBXESFMEAS->callbackData;
+      delete[] packetUBXESFMEAS->callbackData;
     }
     delete packetUBXESFMEAS;
     packetUBXESFMEAS = nullptr;
@@ -4466,11 +4466,17 @@ void DevUBLOXGNSS::processUBXpacket(ubxPacket *msg)
           packetUBXESFMEAS->data.calibTtag = extractLong(msg, 8 + (packetUBXESFMEAS->data.flags.bits.numMeas * 4));
 
         // Check if we need to copy the data for the callback
-        if ((packetUBXESFMEAS->callbackData != nullptr)                                  // If RAM has been allocated for the copy of the data
-            && (packetUBXESFMEAS->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
+        if (packetUBXESFMEAS->callbackData != nullptr) // If RAM has been allocated for the copy of the data
         {
-          memcpy(&packetUBXESFMEAS->callbackData->timeTag, &packetUBXESFMEAS->data.timeTag, sizeof(UBX_ESF_MEAS_data_t));
-          packetUBXESFMEAS->automaticFlags.flags.bits.callbackCopyValid = true;
+          for (uint16_t i = 0; i < UBX_ESF_MEAS_CALLBACK_BUFFERS; i++)
+          {
+            if ((packetUBXESFMEAS->automaticFlags.flags.bits.callbackCopyValid & (1 << i)) == 0) // AND the data is stale
+            {
+              memcpy(&packetUBXESFMEAS->callbackData[i].timeTag, &packetUBXESFMEAS->data.timeTag, sizeof(UBX_ESF_MEAS_data_t));
+              packetUBXESFMEAS->automaticFlags.flags.bits.callbackCopyValid |= (1 << i);
+              break; // Only copy once
+            }
+          }
         }
 
         // Check if we need to copy the data into the file buffer
@@ -5937,13 +5943,16 @@ void DevUBLOXGNSS::checkCallbacks(void)
 
   if (packetUBXESFMEAS != nullptr)                                               // If RAM has been allocated for message storage
     if (packetUBXESFMEAS->callbackData != nullptr)                               // If RAM has been allocated for the copy of the data
-      if (packetUBXESFMEAS->automaticFlags.flags.bits.callbackCopyValid == true) // If the copy of the data is valid
+      for (uint16_t i = 0; i < UBX_ESF_MEAS_CALLBACK_BUFFERS; i++)
       {
-        if (packetUBXESFMEAS->callbackPointerPtr != nullptr) // If the pointer to the callback has been defined
+        if ((packetUBXESFMEAS->automaticFlags.flags.bits.callbackCopyValid & (1 << i)) != 0) // If the copy of the data is valid
         {
-          packetUBXESFMEAS->callbackPointerPtr(packetUBXESFMEAS->callbackData); // Call the callback
+          if (packetUBXESFMEAS->callbackPointerPtr != nullptr) // If the pointer to the callback has been defined
+          {
+            packetUBXESFMEAS->callbackPointerPtr(&packetUBXESFMEAS->callbackData[i]); // Call the callback
+          }
+          packetUBXESFMEAS->automaticFlags.flags.bits.callbackCopyValid &= ~(1 << i); // Mark the data as stale
         }
-        packetUBXESFMEAS->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
       }
 
   if (packetUBXESFRAW != nullptr)                                               // If RAM has been allocated for message storage
@@ -14693,7 +14702,7 @@ bool DevUBLOXGNSS::setAutoESFMEAScallbackPtr(void (*callbackPointerPtr)(UBX_ESF_
 
   if (packetUBXESFMEAS->callbackData == nullptr) // Check if RAM has been allocated for the callback copy
   {
-    packetUBXESFMEAS->callbackData = new UBX_ESF_MEAS_data_t; // Allocate RAM for the main struct
+    packetUBXESFMEAS->callbackData = new UBX_ESF_MEAS_data_t[UBX_ESF_MEAS_CALLBACK_BUFFERS]; // Allocate RAM for the main struct
   }
 
   if (packetUBXESFMEAS->callbackData == nullptr)

src/u-blox_structs.h

@@ -2237,6 +2237,7 @@ typedef struct
 // UBX-ESF-MEAS (0x10 0x02): External sensor fusion measurements
 // Note: length is variable
 // Note: ESF RAW data cannot be polled. It is "Output" only
+#define UBX_ESF_MEAS_CALLBACK_BUFFERS 6
 const uint16_t UBX_ESF_MEAS_MAX_LEN = 8 + (4 * DEF_MAX_NUM_ESF_MEAS) + 4;
 
 typedef struct
@@ -2273,12 +2274,27 @@ typedef struct
   uint32_t calibTtag; // OPTIONAL: Receiver local time calibrated: ms
 } UBX_ESF_MEAS_data_t;
 
+struct ubxESFMEASAutomaticFlags
+{
+  union
+  {
+    uint16_t all;
+    struct
+    {
+      uint16_t automatic : 1;                                     // Will this message be delivered and parsed "automatically" (without polling)
+      uint16_t implicitUpdate : 1;                                // Is the update triggered by accessing stale data (=true) or by a call to checkUblox (=false)
+      uint16_t addToFileBuffer : 1;                               // Should the raw UBX data be added to the file buffer?
+      uint16_t callbackCopyValid : UBX_ESF_MEAS_CALLBACK_BUFFERS; // Are the copies of the data struct used by the callback valid/fresh?
+    } bits;
+  } flags;
+};
+
 typedef struct
 {
-  ubxAutomaticFlags automaticFlags;
+  ubxESFMEASAutomaticFlags automaticFlags;
   UBX_ESF_MEAS_data_t data;
   void (*callbackPointerPtr)(UBX_ESF_MEAS_data_t *);
-  UBX_ESF_MEAS_data_t *callbackData;
+  UBX_ESF_MEAS_data_t *callbackData; // This is an array of buffers
 } UBX_ESF_MEAS_t;
 
 // UBX-ESF-RAW (0x10 0x03): Raw sensor measurements