Add storage for multiple SFRBX data callbacks

Author: PaulZC

src/u-blox_GNSS.cpp

@@ -4121,17 +4121,23 @@ void DevUBLOXGNSS::processUBXpacket(ubxPacket *msg)
         packetUBXRXMSFRBX->moduleQueried = true;
 
         // Check if we need to copy the data for the callback
-        if ((packetUBXRXMSFRBX->callbackData != nullptr)                                  // If RAM has been allocated for the copy of the data
-            && (packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
+        if (packetUBXRXMSFRBX->callbackData != nullptr) // If RAM has been allocated for the copies of the data
         {
-          memcpy(&packetUBXRXMSFRBX->callbackData->gnssId, &packetUBXRXMSFRBX->data.gnssId, sizeof(UBX_RXM_SFRBX_data_t));
-          packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid = true;
+          for (uint32_t i = 0; i < UBX_RXM_SFRBX_CALLBACK_BUFFERS; i++) // Check all available buffers
+          {
+            if ((packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid & (1 << i)) == 0) // AND the buffer is empty
+            {
+              memcpy(&packetUBXRXMSFRBX->callbackData[i].gnssId, &packetUBXRXMSFRBX->data.gnssId, sizeof(UBX_RXM_SFRBX_data_t));
+              packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid |= (1 << i);
+              break; // Only copy once - into first available buffer
+            }
+          }
         }
 
         // Check if we need to copy the data for the message callbacks
-        if (packetUBXRXMSFRBX->callbackMessageData != nullptr) // If RAM has been allocated for the copy of the data
+        if (packetUBXRXMSFRBX->callbackMessageData != nullptr) // If RAM has been allocated for the copies of the data
         {
-          for (uint16_t i = 0; i < UBX_RXM_SFRBX_MESSAGE_CALLBACK_BUFFERS; i++) // Check all available buffers
+          for (uint32_t i = 0; i < UBX_RXM_SFRBX_CALLBACK_BUFFERS; i++) // Check all available buffers
           {
             if ((packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackMessageCopyValid & (1 << i)) == 0) // AND the buffer is empty
             {
@@ -4142,13 +4148,13 @@ void DevUBLOXGNSS::processUBXpacket(ubxPacket *msg)
               packetUBXRXMSFRBX->callbackMessageData[i].lengthLSB = msg->len & 0xFF;
               packetUBXRXMSFRBX->callbackMessageData[i].lengthMSB = msg->len >> 8;
 
-              memcpy(packetUBXRXMSFRBX->callbackMessageData[i].payload, msg->payload, msg->len);
+              memcpy(&packetUBXRXMSFRBX->callbackMessageData[i].payload, msg->payload, msg->len);
 
               packetUBXRXMSFRBX->callbackMessageData[i].checksumA = msg->checksumA;
               packetUBXRXMSFRBX->callbackMessageData[i].checksumB = msg->checksumB;
 
               packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackMessageCopyValid |= (1 << i);
-              break; // abort when added
+              break; // Only copy once - into first available buffer
             }
           }
         }
@@ -5820,22 +5826,25 @@ void DevUBLOXGNSS::checkCallbacks(void)
 
   if (packetUBXRXMSFRBX != nullptr) // If RAM has been allocated for message storage
   {
-    if (packetUBXRXMSFRBX->callbackData != nullptr) // If RAM has been allocated for the copy of the data
+    if (packetUBXRXMSFRBX->callbackData != nullptr) // If RAM has been allocated for the copies of the data
     {
-      if (packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid == true) // If the copy of the data is valid
+      for (uint32_t i = 0; i < UBX_RXM_SFRBX_CALLBACK_BUFFERS; i++)
       {
-        if (packetUBXRXMSFRBX->callbackPointerPtr != nullptr) // If the pointer to the callback has been defined
+        if ((packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid & (1 << i)) != 0) // If the copy of the data is valid
         {
-          packetUBXRXMSFRBX->callbackPointerPtr(packetUBXRXMSFRBX->callbackData); // Call the callback
+          if (packetUBXRXMSFRBX->callbackPointerPtr != nullptr) // If the pointer to the callback has been defined
+          {
+            packetUBXRXMSFRBX->callbackPointerPtr(&packetUBXRXMSFRBX->callbackData[i]); // Call the callback
+          }
+          packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid &= ~(1 << i); // Mark the data as stale
         }
-        packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
       }
     }
-    if (packetUBXRXMSFRBX->callbackMessageData != nullptr) // If RAM has been allocated for the copy of the data
+    if (packetUBXRXMSFRBX->callbackMessageData != nullptr) // If RAM has been allocated for the copies of the data
     {
-      for (uint16_t i = 0; i < UBX_RXM_SFRBX_MESSAGE_CALLBACK_BUFFERS; i++)
+      for (uint32_t i = 0; i < UBX_RXM_SFRBX_CALLBACK_BUFFERS; i++)
       {
-        if ((packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackMessageCopyValid & (1 << i)) > 0) // If the copy of the data is valid
+        if ((packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackMessageCopyValid & (1 << i)) != 0) // If the copy of the data is valid
         {
           if (packetUBXRXMSFRBX->callbackMessagePointerPtr != nullptr) // If the pointer to the callback has been defined
           {
@@ -13163,7 +13172,7 @@ bool DevUBLOXGNSS::setAutoRXMSFRBXcallbackPtr(void (*callbackPointerPtr)(UBX_RXM
 
   if (packetUBXRXMSFRBX->callbackData == nullptr) // Check if RAM has been allocated for the callback copy
   {
-    packetUBXRXMSFRBX->callbackData = new UBX_RXM_SFRBX_data_t; // Allocate RAM for the main struct
+    packetUBXRXMSFRBX->callbackData = new UBX_RXM_SFRBX_data_t[UBX_RXM_SFRBX_CALLBACK_BUFFERS]; // Allocate RAM for the main struct
   }
 
   if (packetUBXRXMSFRBX->callbackData == nullptr)
@@ -13188,7 +13197,7 @@ bool DevUBLOXGNSS::setAutoRXMSFRBXmessageCallbackPtr(void (*callbackMessagePoint
 
   if (packetUBXRXMSFRBX->callbackMessageData == nullptr) // Check if RAM has been allocated for the callback copy
   {
-    packetUBXRXMSFRBX->callbackMessageData = new UBX_RXM_SFRBX_message_data_t[UBX_RXM_SFRBX_MESSAGE_CALLBACK_BUFFERS]; // Allocate RAM for the main struct
+    packetUBXRXMSFRBX->callbackMessageData = new UBX_RXM_SFRBX_message_data_t[UBX_RXM_SFRBX_CALLBACK_BUFFERS]; // Allocate RAM for the main struct
   }
 
   if (packetUBXRXMSFRBX->callbackMessageData == nullptr)

src/u-blox_GNSS.h

@@ -928,6 +928,7 @@ class DevUBLOXGNSS
 
   bool setRXMCORcallbackPtr(void (*callbackPointerPtr)(UBX_RXM_COR_data_t *)); // RXM COR
 
+  // Note: RXM-SFRBX is output-only. It cannot be polled. Strictly getRXMSFRBX should be deprecated
   bool getRXMSFRBX(uint16_t maxWait = kUBLOXGNSSDefaultMaxWait);                                                                                                       // RXM SFRBX
   bool setAutoRXMSFRBX(bool enabled, uint8_t layer = VAL_LAYER_RAM_BBR, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait);                                                  // Enable/disable automatic RXM SFRBX reports at the navigation frequency
   bool setAutoRXMSFRBX(bool enabled, bool implicitUpdate, uint8_t layer = VAL_LAYER_RAM_BBR, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait);                             // Enable/disable automatic RXM SFRBX reports at the navigation frequency, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update

src/u-blox_structs.h

@@ -1426,10 +1426,11 @@ typedef struct
 // RXM-specific structs
 
 // UBX-RXM-SFRBX (0x02 0x13): Broadcast navigation data subframe
+// Note: SFRBX is output-only. Polling is not possible
 // Note: length is variable
 // Note: on protocol version 17: numWords is (0..16)
 //       on protocol version 18+: numWords is (0..10)
-#define UBX_RXM_SFRBX_MESSAGE_CALLBACK_BUFFERS 12
+#define UBX_RXM_SFRBX_CALLBACK_BUFFERS 14 // 14 data buffers, 14 message buffers, 3 flags, in uint32_t
 const uint8_t UBX_RXM_SFRBX_MAX_WORDS = 16;
 const uint16_t UBX_RXM_SFRBX_MAX_LEN = 8 + (4 * UBX_RXM_SFRBX_MAX_WORDS);
 
@@ -1465,14 +1466,14 @@ struct ubxSFRBXAutomaticFlags
 {
   union
   {
-    uint16_t all;
+    uint32_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 : 1;                                             // Is the copy of the data struct used by the callback valid/fresh?
-      uint16_t callbackMessageCopyValid : UBX_RXM_SFRBX_MESSAGE_CALLBACK_BUFFERS; // Are the copies of the data structs used by the callback valid/fresh?
+      uint32_t automatic : 1;                                             // Will this message be delivered and parsed "automatically" (without polling)
+      uint32_t implicitUpdate : 1;                                        // Is the update triggered by accessing stale data (=true) or by a call to checkUblox (=false)
+      uint32_t addToFileBuffer : 1;                                       // Should the raw UBX data be added to the file buffer?
+      uint32_t callbackCopyValid : UBX_RXM_SFRBX_CALLBACK_BUFFERS;        // Are the copies of the data struct used by the callback valid/fresh?
+      uint32_t callbackMessageCopyValid : UBX_RXM_SFRBX_CALLBACK_BUFFERS; // Are the copies of the data structs used by the callback valid/fresh?
     } bits;
   } flags;
 };
@@ -1483,9 +1484,9 @@ typedef struct
   UBX_RXM_SFRBX_data_t data;
   bool moduleQueried;
   void (*callbackPointerPtr)(UBX_RXM_SFRBX_data_t *);
-  UBX_RXM_SFRBX_data_t *callbackData;
+  UBX_RXM_SFRBX_data_t *callbackData; // This is an array of buffers
   void (*callbackMessagePointerPtr)(UBX_RXM_SFRBX_message_data_t *);
-  UBX_RXM_SFRBX_message_data_t *callbackMessageData;
+  UBX_RXM_SFRBX_message_data_t *callbackMessageData; // This is an array of buffers
 } UBX_RXM_SFRBX_t;
 
 // UBX-RXM-MEASX (0x02 0x14): Receiver Manager Messages: i.e. Satellite Status, RTC Status.