Add support for multiple SFRBX message callbacks - for the PointPerfect Library

PaulZC · PaulZC · commit bfd03a716b51 · 2023-12-18T17:28:35.000Z
diff --git a/src/u-blox_GNSS.cpp b/src/u-blox_GNSS.cpp
@@ -375,6 +375,10 @@ void DevUBLOXGNSS::end(void)
     {
       delete packetUBXRXMSFRBX->callbackData;
     }
+    if (packetUBXRXMSFRBX->callbackMessageData != nullptr)
+    {
+      delete[] packetUBXRXMSFRBX->callbackMessageData;
+    }
     delete packetUBXRXMSFRBX;
     packetUBXRXMSFRBX = nullptr;
   }
@@ -4124,6 +4128,31 @@ void DevUBLOXGNSS::processUBXpacket(ubxPacket *msg)
           packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid = true;
         }
 
+        // 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
+        {
+          for (uint16_t i = 0; i < UBX_RXM_SFRBX_MESSAGE_CALLBACK_BUFFERS; i++) // Check all available buffers
+          {
+            if ((packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackMessageCopyValid & (1 << i)) == 0) // AND the buffer is empty
+            {
+              packetUBXRXMSFRBX->callbackMessageData[i].sync1 = UBX_SYNCH_1;
+              packetUBXRXMSFRBX->callbackMessageData[i].sync2 = UBX_SYNCH_2;
+              packetUBXRXMSFRBX->callbackMessageData[i].cls = UBX_CLASS_RXM;
+              packetUBXRXMSFRBX->callbackMessageData[i].ID = UBX_RXM_SFRBX;
+              packetUBXRXMSFRBX->callbackMessageData[i].lengthLSB = msg->len & 0xFF;
+              packetUBXRXMSFRBX->callbackMessageData[i].lengthMSB = msg->len >> 8;
+
+              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
+            }
+          }
+        }
+
         // Check if we need to copy the data into the file buffer
         if (packetUBXRXMSFRBX->automaticFlags.flags.bits.addToFileBuffer)
         {
@@ -5789,8 +5818,10 @@ void DevUBLOXGNSS::checkCallbacks(void)
         packetUBXRXMCOR->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
       }
 
-  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 != 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->automaticFlags.flags.bits.callbackCopyValid == true) // If the copy of the data is valid
       {
         if (packetUBXRXMSFRBX->callbackPointerPtr != nullptr) // If the pointer to the callback has been defined
@@ -5799,6 +5830,22 @@ void DevUBLOXGNSS::checkCallbacks(void)
         }
         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
+    {
+      for (uint16_t i = 0; i < UBX_RXM_SFRBX_MESSAGE_CALLBACK_BUFFERS; i++)
+      {
+        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
+          {
+            packetUBXRXMSFRBX->callbackMessagePointerPtr(&packetUBXRXMSFRBX->callbackMessageData[i]); // Call the callback
+          }
+          packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackMessageCopyValid &= ~(1 << i); // Mark the data as stale
+        }
+      }
+    }
+  }
 
   if (packetUBXRXMRAWX != nullptr)                                               // If RAM has been allocated for message storage
     if (packetUBXRXMRAWX->callbackData != nullptr)                               // If RAM has been allocated for the copy of the data
@@ -6107,7 +6154,7 @@ bool DevUBLOXGNSS::pushRawData(uint8_t *dataBytes, size_t numDataBytes, bool cal
       bytesLeftToWrite -= (size_t)bytesToWrite;
       dataBytes += bytesToWrite;
 
-      if (callProcessBuffer) // Try and prevent data loss during large pushes by calling checkUbloxSerial between chunks
+      if (callProcessBuffer)                // Try and prevent data loss during large pushes by calling checkUbloxSerial between chunks
         checkUbloxSerial(&packetCfg, 0, 0); // Don't call checkUbloxInternal as we already have the lock!
     }
   }
@@ -9015,7 +9062,6 @@ const char *DevUBLOXGNSS::getUniqueChipIdStr(UBX_SEC_UNIQID_data_t *data, uint16
   return ((const char *)uniqueId);
 }
 
-
 // CONFIGURATION INTERFACE (protocol v27 and above)
 
 // Given a key, load the payload with data that can then be extracted to 8, 16, or 32 bits
@@ -13132,6 +13178,31 @@ bool DevUBLOXGNSS::setAutoRXMSFRBXcallbackPtr(void (*callbackPointerPtr)(UBX_RXM
   packetUBXRXMSFRBX->callbackPointerPtr = callbackPointerPtr;
   return (true);
 }
+// Use this if you want all of the SFRBX message (including sync chars, checksum, etc.) to push to the PointPerfect Library
+bool DevUBLOXGNSS::setAutoRXMSFRBXmessageCallbackPtr(void (*callbackMessagePointerPtr)(UBX_RXM_SFRBX_message_data_t *), uint8_t layer, uint16_t maxWait)
+{
+  // Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually.
+  bool result = setAutoRXMSFRBX(true, false, layer, maxWait);
+  if (!result)
+    return (result); // Bail if setAuto failed
+
+  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
+  }
+
+  if (packetUBXRXMSFRBX->callbackMessageData == nullptr)
+  {
+#ifndef SFE_UBLOX_REDUCED_PROG_MEM
+    if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
+      _debugSerial.println(F("setAutoRXMSFRBXmessageCallbackPtr: RAM alloc failed!"));
+#endif
+    return (false);
+  }
+
+  packetUBXRXMSFRBX->callbackMessagePointerPtr = callbackMessagePointerPtr;
+  return (true);
+}
 
 // In case no config access to the GNSS is possible and SFRBX is send cyclically already
 // set config to suitable parameters
@@ -13166,6 +13237,8 @@ bool DevUBLOXGNSS::initPacketUBXRXMSFRBX()
   packetUBXRXMSFRBX->automaticFlags.flags.all = 0;
   packetUBXRXMSFRBX->callbackPointerPtr = nullptr;
   packetUBXRXMSFRBX->callbackData = nullptr;
+  packetUBXRXMSFRBX->callbackMessagePointerPtr = nullptr;
+  packetUBXRXMSFRBX->callbackMessageData = nullptr;
   packetUBXRXMSFRBX->moduleQueried = false;
   return (true);
 }
@@ -17806,11 +17879,11 @@ bool DevUBLOXGNSS::setLNAMode(sfe_ublox_lna_mode_e mode, uint8_t layer, uint16_t
 }
 bool DevUBLOXGNSS::getGPSL5HealthOverride(bool *override, uint8_t layer, uint16_t maxWait)
 {
-  return getVal8(UBLOX_CFG_SIGNAL_GPS_L5_HEALTH_OVERRIDE, (uint8_t *)override, layer, maxWait); // Get the GPS L5 health override status
+  return getVal8(UBLOX_CFG_SIGNAL_GPS_L5_HEALTH_OVERRIDE, (uint8_t *) override, layer, maxWait); // Get the GPS L5 health override status
 }
 bool DevUBLOXGNSS::setGPSL5HealthOverride(bool override, uint8_t layer, uint16_t maxWait)
 {
-  return setVal8(UBLOX_CFG_SIGNAL_GPS_L5_HEALTH_OVERRIDE, (uint8_t)override, layer, maxWait); // Set the GPS L5 health override status
+  return setVal8(UBLOX_CFG_SIGNAL_GPS_L5_HEALTH_OVERRIDE, (uint8_t) override, layer, maxWait); // Set the GPS L5 health override status
 }
 
 #ifndef SFE_UBLOX_DISABLE_ESF
diff --git a/src/u-blox_GNSS.h b/src/u-blox_GNSS.h
@@ -933,6 +933,7 @@ class DevUBLOXGNSS
   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
   bool setAutoRXMSFRBXrate(uint8_t rate, bool implicitUpdate = true, uint8_t layer = VAL_LAYER_RAM_BBR, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait);                  // Set the rate for automatic SFRBX reports
   bool setAutoRXMSFRBXcallbackPtr(void (*callbackPointerPtr)(UBX_RXM_SFRBX_data_t *), uint8_t layer = VAL_LAYER_RAM_BBR, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait); // Enable automatic SFRBX reports at the navigation frequency. Data is accessed from the callback.
+  bool setAutoRXMSFRBXmessageCallbackPtr(void (*callbackMessagePointerPtr)(UBX_RXM_SFRBX_message_data_t *), uint8_t layer = VAL_LAYER_RAM_BBR, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait); // Use this if you want all of the SFRBX message (including sync chars, checksum, etc.) to push to the PointPerfect Library
   bool assumeAutoRXMSFRBX(bool enabled, bool implicitUpdate = true);                                                                                                   // In case no config access to the GPS is possible and RXM SFRBX is send cyclically already
   void flushRXMSFRBX();                                                                                                                                                // Mark all the data as read/stale
   void logRXMSFRBX(bool enabled = true);                                                                                                                               // Log data to file buffer
diff --git a/src/u-blox_structs.h b/src/u-blox_structs.h
@@ -431,7 +431,7 @@ typedef struct
     uint8_t all;
     struct
     {
-      uint8_t invalidLlh : 1; // 1 = Invalid lon, lat, height and hMSL
+      uint8_t invalidLlh : 1;        // 1 = Invalid lon, lat, height and hMSL
       uint8_t lastCorrectionAge : 4; // Age of the most recently received differential correction:
                                      // 0: Not available
                                      // 1: Age between 0 and 1 second
@@ -1429,6 +1429,7 @@ typedef struct
 // 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
 const uint8_t UBX_RXM_SFRBX_MAX_WORDS = 16;
 const uint16_t UBX_RXM_SFRBX_MAX_LEN = 8 + (4 * UBX_RXM_SFRBX_MAX_WORDS);
 
@@ -1445,13 +1446,46 @@ typedef struct
   uint32_t dwrd[UBX_RXM_SFRBX_MAX_WORDS]; // The data words
 } UBX_RXM_SFRBX_data_t;
 
+// Define a struct to hold the entire SFRBX message so the whole thing can be pushed to the PointPerfect Library
+// Remember that the length of the payload will be variable.
 typedef struct
 {
-  ubxAutomaticFlags automaticFlags;
+  uint8_t sync1; // 0xB5
+  uint8_t sync2; // 0x62
+  uint8_t cls;
+  uint8_t ID;
+  uint8_t lengthLSB;
+  uint8_t lengthMSB;
+  uint8_t payload[UBX_RXM_SFRBX_MAX_LEN];
+  uint8_t checksumA;
+  uint8_t checksumB;
+} UBX_RXM_SFRBX_message_data_t;
+
+struct ubxSFRBXAutomaticFlags
+{
+  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 : 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?
+    } bits;
+  } flags;
+};
+
+typedef struct
+{
+  ubxSFRBXAutomaticFlags automaticFlags;
   UBX_RXM_SFRBX_data_t data;
   bool moduleQueried;
   void (*callbackPointerPtr)(UBX_RXM_SFRBX_data_t *);
   UBX_RXM_SFRBX_data_t *callbackData;
+  void (*callbackMessagePointerPtr)(UBX_RXM_SFRBX_message_data_t *);
+  UBX_RXM_SFRBX_message_data_t *callbackMessageData;
 } UBX_RXM_SFRBX_t;
 
 // UBX-RXM-MEASX (0x02 0x14): Receiver Manager Messages: i.e. Satellite Status, RTC Status.
@@ -1798,7 +1832,7 @@ typedef struct
   uint8_t reserved2[2]; // Reserved
   uint32_t pinIrq;      // Mask of pins value using the PIO Irq
   uint32_t pullH;       // Mask of pins value using the PIO pull high resistor
-  uint32_t pullL;        // Mask of pins value using the PIO pull low resistor
+  uint32_t pullL;       // Mask of pins value using the PIO pull low resistor
 } UBX_MON_HW_data_t;
 
 typedef struct
@@ -2793,8 +2827,8 @@ struct rtcmAutomaticFlags
     uint8_t all;
     struct
     {
-      uint8_t dataValid : 1; // Is the copy of the data used by the get function valid/fresh? 0 = invalid, 1 = valid
-      uint8_t dataRead : 1;  // Has the data been read? 0 = unread, 1 = read
+      uint8_t dataValid : 1;         // Is the copy of the data used by the get function valid/fresh? 0 = invalid, 1 = valid
+      uint8_t dataRead : 1;          // Has the data been read? 0 = unread, 1 = read
       uint8_t callbackDataValid : 1; // Is the copy of the data used by the callback valid/fresh? 0 = invalid/stale, 1 = valid/fresh
     } bits;
   } flags;
@@ -2815,19 +2849,19 @@ const uint16_t RTCM_1005_MSG_LEN_BYTES = 19;
 
 typedef struct
 {
-  uint16_t MessageNumber; // Message Number (“1005” = 0x3ED)
-  uint16_t ReferenceStationID; // Reference Station ID
-  uint8_t ITRFRealizationYear; // ITRF Realization Year
-  bool GPSIndicator; // GPS Indicator
-  bool GLONASSIndicator; // GLONASS Indicator
-  bool GalileoIndicator; // Galileo Indicator
-  bool ReferenceStationIndicator; // Reference-Station Indicator
-  int64_t AntennaReferencePointECEFX; // Antenna Reference Point ECEF-X (0.0001m)
+  uint16_t MessageNumber;                 // Message Number (“1005” = 0x3ED)
+  uint16_t ReferenceStationID;            // Reference Station ID
+  uint8_t ITRFRealizationYear;            // ITRF Realization Year
+  bool GPSIndicator;                      // GPS Indicator
+  bool GLONASSIndicator;                  // GLONASS Indicator
+  bool GalileoIndicator;                  // Galileo Indicator
+  bool ReferenceStationIndicator;         // Reference-Station Indicator
+  int64_t AntennaReferencePointECEFX;     // Antenna Reference Point ECEF-X (0.0001m)
   bool SingleReceiverOscillatorIndicator; // Single Receiver Oscillator Indicator
-  bool Reserved; // Reserved
-  int64_t AntennaReferencePointECEFY; // Antenna Reference Point ECEF-Y (0.0001m)
-  uint8_t QuarterCycleIndicator; // Quarter Cycle Indicator
-  int64_t AntennaReferencePointECEFZ; // Antenna Reference Point ECEF-Z (0.0001m)
+  bool Reserved;                          // Reserved
+  int64_t AntennaReferencePointECEFY;     // Antenna Reference Point ECEF-Y (0.0001m)
+  uint8_t QuarterCycleIndicator;          // Quarter Cycle Indicator
+  int64_t AntennaReferencePointECEFZ;     // Antenna Reference Point ECEF-Z (0.0001m)
 } RTCM_1005_data_t;
 
 typedef struct
@@ -2844,18 +2878,18 @@ const uint16_t RTCM_1006_MSG_LEN_BYTES = 21;
 
 typedef struct
 {
-  uint16_t MessageNumber; // Message Number (“1006” = 0x3EE)
-  uint16_t ReferenceStationID; // Reference Station ID
-  uint8_t ITRFRealizationYear; // ITRF Realization Year
-  bool GPSIndicator; // GPS Indicator
-  bool GLONASSIndicator; // GLONASS Indicator
-  bool GalileoIndicator; // Galileo Indicator
-  bool ReferenceStationIndicator; // Reference-Station Indicator
-  int64_t AntennaReferencePointECEFX; // Antenna Reference Point ECEF-X (0.0001m)
+  uint16_t MessageNumber;                 // Message Number (“1006” = 0x3EE)
+  uint16_t ReferenceStationID;            // Reference Station ID
+  uint8_t ITRFRealizationYear;            // ITRF Realization Year
+  bool GPSIndicator;                      // GPS Indicator
+  bool GLONASSIndicator;                  // GLONASS Indicator
+  bool GalileoIndicator;                  // Galileo Indicator
+  bool ReferenceStationIndicator;         // Reference-Station Indicator
+  int64_t AntennaReferencePointECEFX;     // Antenna Reference Point ECEF-X (0.0001m)
   bool SingleReceiverOscillatorIndicator; // Single Receiver Oscillator Indicator
-  bool Reserved; // Reserved
-  int64_t AntennaReferencePointECEFY; // Antenna Reference Point ECEF-Y (0.0001m)
-  uint8_t QuarterCycleIndicator; // Quarter Cycle Indicator
-  int64_t AntennaReferencePointECEFZ; // Antenna Reference Point ECEF-Z (0.0001m)
-  uint16_t AntennaHeight; // Antenna Height above the marker used in the survey campaign (0.0001m)
+  bool Reserved;                          // Reserved
+  int64_t AntennaReferencePointECEFY;     // Antenna Reference Point ECEF-Y (0.0001m)
+  uint8_t QuarterCycleIndicator;          // Quarter Cycle Indicator
+  int64_t AntennaReferencePointECEFZ;     // Antenna Reference Point ECEF-Z (0.0001m)
+  uint16_t AntennaHeight;                 // Antenna Height above the marker used in the survey campaign (0.0001m)
 } RTCM_1006_data_t;
