Add support for RTCM 1005 - work in progress

PaulZC · PaulZC · commit f1bebf32b6cb · 2023-05-04T20:45:12.000+01:00
diff --git a/examples/ZED-F9P/Example24_getLatestRTCM1005/Example24_getLatestRTCM1005.ino b/examples/ZED-F9P/Example24_getLatestRTCM1005/Example24_getLatestRTCM1005.ino
@@ -0,0 +1,98 @@
+/*
+  Get the RTCM 1005 sentence using getLatestRTCM1005
+  By: Paul Clark
+  SparkFun Electronics
+  Date: May 4th, 2023
+  License: MIT. See license file for more information.
+
+  This example shows how to turn on/off the RTCM sentences being output over I2C.
+  It then demonstrates how to use the new getLatestRTCM1005 function to retrieve the latest RTCM 1005 message.
+  getLatestRTCM1005 returns immediately - it is not blocking.
+  It returns:
+    0 if no data is available
+    1 if the data is valid but is stale (you have read it before)
+    2 if the data is valid and fresh
+
+  Feel like supporting open source hardware?
+  Buy a board from SparkFun!
+  SparkFun GPS-RTK2 - ZED-F9P (GPS-15136)    https://www.sparkfun.com/products/15136
+  SparkFun GPS-RTK-SMA - ZED-F9P (GPS-16481) https://www.sparkfun.com/products/16481
+
+  Hardware Connections:
+  Plug a Qwiic cable into the GNSS and a RedBoard
+  If you don't have a platform with a Qwiic connection use the SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425)
+  Open the serial monitor at 115200 baud to see the output
+*/
+
+#include <Wire.h> //Needed for I2C to GNSS
+
+#include <SparkFun_u-blox_GNSS_v3.h> //Click here to get the library: http://librarymanager/All#SparkFun_u-blox_GNSS_v3
+SFE_UBLOX_GNSS myGNSS;
+
+void setup()
+{
+
+  Serial.begin(115200);
+  Serial.println(F("SparkFun u-blox GNSS Example"));
+
+  Wire.begin();
+
+  //myGNSS.enableDebugging(); // Uncomment this line to enable debug messages on Serial
+
+  while (myGNSS.begin() == false)
+  {
+    Serial.println(F("u-blox GNSS not detected at default I2C address. Please check wiring."));
+    delay(1000);
+  }
+
+  //Disable or enable various RTCM sentences over the I2C interface
+  myGNSS.setI2COutput(COM_TYPE_NMEA | COM_TYPE_UBX | COM_TYPE_RTCM3); // Turn on UBX, NMEA and RTCM sentences on I2C
+  myGNSS.newCfgValset(VAL_LAYER_RAM_BBR); // Use cfgValset to disable / enable individual RTCM messages
+  myGNSS.addCfgValset(UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_I2C, 1); // Enable RTCM 1005 at current navigation rate
+  if (myGNSS.sendCfgValset()) // Send the configuration VALSET
+    Serial.println(F("RTCM messages were configured successfully"));
+  else
+    Serial.println(F("RTCM message configuration failed!"));
+
+  //myGNSS.saveConfiguration(VAL_CFG_SUBSEC_IOPORT | VAL_CFG_SUBSEC_MSGCONF); //Optional: Save only the ioPort and message settings to NVM
+
+  //myGNSS.setRTCMOutputPort(Serial); // Uncomment this line to echo all RTCM data to Serial for debugging
+
+  if (sizeof(double) != 8) // Check double is 64-bit
+    Serial.println(F("double is not 64-bit. ECEF resolution may be limited!"));
+}
+
+void loop()
+{
+  // getLatestRTCM1005 calls checkUblox for us. We don't need to do it here
+
+  RTCM_1005_data_t data; // Storage for the RTCM 1005 data
+  uint8_t result = myGNSS.getLatestRTCM1005(&data); // Get the latest RTCM 1005 data (if any)
+  
+  if (result == 0)
+  {
+    Serial.println(F("No RTCM 1005 data available"));
+  }
+  else if (result == 1)
+  {
+    Serial.println(F("RTCM 1005 data is available but is stale"));
+  }
+  else // if (result == 2)
+  {
+    double x = data.AntennaReferencePointECEFX;
+    x /= 10000.0; // Convert to m
+    double y = data.AntennaReferencePointECEFY;
+    y /= 10000.0; // Convert to m
+    double z = data.AntennaReferencePointECEFZ;
+    z /= 10000.0; // Convert to m
+
+    Serial.print(F("Latest RTCM 1005: ARP ECEF-X: "));
+    Serial.print(x, 4); // 4 decimal places
+    Serial.print(F("  Y: "));
+    Serial.print(y, 4); // 4 decimal places
+    Serial.print(F("  Z: "));
+    Serial.println(z, 4); // 4 decimal places
+  }
+
+  delay(250);
+}
diff --git a/src/u-blox_GNSS.cpp b/src/u-blox_GNSS.cpp
@@ -622,6 +622,16 @@ void DevUBLOXGNSS::end(void)
   }
 #endif
 
+  if (storageRTCM1005 != nullptr)
+  {
+    if (storageRTCM1005->callbackData != nullptr)
+    {
+      delete storageRTCM1005->callbackData;
+    }
+    delete storageRTCM1005;
+    storageRTCM1005 = nullptr;
+  }
+
   if (sfe_ublox_ubx_logging_list_head != nullptr)
   {
     while (sfe_ublox_ubx_logging_list_head->next != nullptr)
@@ -2168,6 +2178,35 @@ void DevUBLOXGNSS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t req
           // If there is space in the RTCM buffer, store the data there too
           if (rtcmBufferSpaceAvailable() >= _storageRTCM->messageLength + 6)
             storeRTCMBytes(_storageRTCM->dataMessage, _storageRTCM->messageLength + 6);
+
+          // Check "Auto" RTCM
+          if ((messageType == 1005) && (_storageRTCM->messageLength == RTCM_1005_MSG_LEN_BYTES) && (storageRTCM1005 != nullptr))
+          {
+            storageRTCM1005->data.MessageNumber =                     extractUnsignedBits16(&_storageRTCM->dataMessage[3], 0, 12);
+            storageRTCM1005->data.ReferenceStationID =                extractUnsignedBits16(&_storageRTCM->dataMessage[3], 12, 12);
+            storageRTCM1005->data.ITRFRealizationYear =               extractUnsignedBits16(&_storageRTCM->dataMessage[3], 24, 6);
+            storageRTCM1005->data.GPSIndicator =                      extractUnsignedBits16(&_storageRTCM->dataMessage[3], 30, 1);
+            storageRTCM1005->data.GLONASSIndicator =                  extractUnsignedBits16(&_storageRTCM->dataMessage[3], 31, 1);
+            storageRTCM1005->data.GalileoIndicator =                  extractUnsignedBits16(&_storageRTCM->dataMessage[3], 32, 1);
+            storageRTCM1005->data.ReferenceStationIndicator =         extractUnsignedBits16(&_storageRTCM->dataMessage[3], 33, 1);
+            storageRTCM1005->data.AntennaReferencePointECEFX =        extractSignedBits64(&_storageRTCM->dataMessage[3], 34, 38);
+            storageRTCM1005->data.SingleReceiverOscillatorIndicator = extractUnsignedBits16(&_storageRTCM->dataMessage[3], 72, 1);
+            storageRTCM1005->data.Reserved =                          extractUnsignedBits16(&_storageRTCM->dataMessage[3], 73, 1);
+            storageRTCM1005->data.AntennaReferencePointECEFY =        extractSignedBits64(&_storageRTCM->dataMessage[3], 74, 38);
+            storageRTCM1005->data.QuarterCycleIndicator =             extractUnsignedBits16(&_storageRTCM->dataMessage[3], 112, 2);
+            storageRTCM1005->data.AntennaReferencePointECEFY =        extractSignedBits64(&_storageRTCM->dataMessage[3], 114, 38);
+
+            storageRTCM1005->automaticFlags.flags.bits.dataValid = 1; // Mark the data as valid and unread
+            storageRTCM1005->automaticFlags.flags.bits.dataRead = 0;
+
+            if (storageRTCM1005->callbackData != nullptr) // Should we copy the data for the callback?
+              if (storageRTCM1005->callbackPointerPtr != nullptr) // Has the callback been defined?
+                if (storageRTCM1005->automaticFlags.flags.bits.callbackDataValid == 0) // Only overwrite the callback copy if it has been read
+                {
+                  memcpy(storageRTCM1005->callbackData, &_storageRTCM->dataMessage[3], RTCM_1005_MSG_LEN_BYTES);
+                  storageRTCM1005->automaticFlags.flags.bits.callbackDataValid = 1;
+                }
+          }
         }
         else
         {
@@ -15706,6 +15745,83 @@ void DevUBLOXGNSS::crc24q(uint8_t incoming, uint32_t *checksum)
 
 #endif
 
+// Return the most recent RTCM 1005: 0 = no data, 1 = stale data, 2 = fresh data
+uint8_t DevUBLOXGNSS::getLatestRTCM1005(RTCM_1005_data_t *data)
+{
+  if (!initStorageRTCM())
+    return 0;
+  if (storageRTCM1005 == nullptr)
+    initStorageRTCM1005();        // Check that RAM has been allocated for the message
+  if (storageRTCM1005 == nullptr) // Bail if the RAM allocation failed
+    return 0;
+
+  checkUbloxInternal(&packetCfg, 0, 0); // Call checkUbloxInternal to parse any incoming data. Don't overwrite the requested Class and ID
+
+  uint8_t result = 0;
+  if (storageRTCM1005->automaticFlags.flags.bits.dataValid == 1) // Is the copy valid?
+  {
+    result = 1;
+    if (storageRTCM1005->automaticFlags.flags.bits.dataRead == 0) // Has the data already been read?
+    {
+      result = 2;
+      storageRTCM1005->automaticFlags.flags.bits.dataRead = 1; // Mark the data as read
+    }
+  }
+
+  return (result);
+}
+
+bool DevUBLOXGNSS::setRTCM1005callbackPtr(void (*callbackPointerPtr)(RTCM_1005_data_t *))
+{
+  if (!initStorageRTCM())
+    return false;
+  if (storageRTCM1005 == nullptr)
+    initStorageRTCM1005();        // Check that RAM has been allocated for the message
+  if (storageRTCM1005 == nullptr) // Bail if the RAM allocation failed
+    return (false);
+
+  if (storageRTCM1005->callbackData == nullptr) // Check if RAM has been allocated for the callback copy
+  {
+    storageRTCM1005->callbackData = new RTCM_1005_data_t;
+  }
+
+  if (storageRTCM1005->callbackData == nullptr)
+  {
+#ifndef SFE_UBLOX_REDUCED_PROG_MEM
+    if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
+      _debugSerial.println(F("setRTCM1005callbackPtr: RAM alloc failed!"));
+#endif
+    return (false);
+  }
+
+  storageRTCM1005->callbackPointerPtr = callbackPointerPtr;
+  return (true);
+}
+
+// Private: allocate RAM for incoming RTCM 1005 messages and initialize it
+bool DevUBLOXGNSS::initStorageRTCM1005()
+{
+  if (storageRTCM1005 != nullptr) // Check if storage already exists
+    return true;
+
+  storageRTCM1005 = new RTCM_1005_t; // Allocate RAM for the main struct
+  if (storageRTCM1005 == nullptr)
+  {
+#ifndef SFE_UBLOX_REDUCED_PROG_MEM
+    if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
+      _debugSerial.println(F("initStorageRTCM1005: RAM alloc failed!"));
+#endif
+    return (false);
+  }
+
+  storageRTCM1005->callbackPointerPtr = nullptr; // Clear the callback pointers
+  storageRTCM1005->callbackData = nullptr;
+
+  storageRTCM1005->automaticFlags.flags.all = 0; // Mark the data as invalid/stale and unread
+
+  return (true);
+}
+
 // ***** CFG RATE Helper Functions
 
 // Set the rate at which the module will give us an updated navigation solution
@@ -17560,3 +17676,104 @@ double DevUBLOXGNSS::extractDouble(ubxPacket *msg, uint16_t spotToStart)
   converter.unsigned64 = extractLongLong(msg, spotToStart);
   return (converter.dbl);
 }
+
+// Given a pointer, extract an unsigned integer, starting at bit start with width
+uint16_t DevUBLOXGNSS::extractUnsignedBits16(uint8_t *ptr, uint16_t start, uint16_t width)
+{
+  uint16_t result = 0;
+  uint16_t count = 0;
+  uint8_t bitMask = 0x80;
+
+  // Loop until we reach the start bit
+  while (count < start)
+  {
+    bitMask >> 1; // Shift the bit mask
+    count++; // Increment the count
+    if (bitMask == 0) // Have we counted 8 bits?
+    {
+      ptr++; // Point to the next byte
+      bitMask = 0x80; // Reset the bit mask
+    }
+  }
+
+  // We have reached the start bit and ptr is pointing at the correct byte
+  // Now extract width bytes, incrementing ptr and shifting bitMask as we go
+  while (count < (start + width))
+  {
+    if (*ptr & bitMask) // Is the bit set?
+      result |= 1; // Set the corresponding bit in result
+
+    bitMask >> 1; // Shift the bit mask
+    count++; // Increment the count
+
+    if (bitMask == 0) // Have we counted 8 bits?
+    {
+      ptr++; // Point to the next byte
+      bitMask = 0x80; // Reset the bit mask
+    }
+
+    if (count < (start + width)) // Do we need to shift result?
+      result <<= 1; // Shift the result
+  }
+
+  return result;
+}
+
+// Given a pointer, extract a signed integer, starting at bit start with width
+int64_t DevUBLOXGNSS::extractSignedBits64(uint8_t *ptr, uint16_t start, uint16_t width)
+{
+
+  unsignedSigned64 result;
+  result.unsigned64 = 0;
+
+  unsignedSigned64 twosComplement;
+  twosComplement.unsigned64 = 0xFFFFFFFFFFFFFFFF;
+
+  bool isNegative;
+
+  uint16_t count = 0;
+  uint8_t bitMask = 0x80;
+
+  // Loop until we reach the start bit
+  while (count < start)
+  {
+    bitMask >> 1; // Shift the bit mask
+    count++; // Increment the count
+    if (bitMask == 0) // Have we counted 8 bits?
+    {
+      ptr++; // Point to the next byte
+      bitMask = 0x80; // Reset the bit mask
+    }
+  }
+
+  isNegative = *ptr & bitMask; // Record the first bit - indicates in the number is negative
+
+  // We have reached the start bit and ptr is pointing at the correct byte
+  // Now extract width bytes, incrementing ptr and shifting bitMask as we go
+  while (count < (start + width))
+  {
+    if (*ptr & bitMask) // Is the bit set?
+      result.unsigned64 |= 1; // Set the corresponding bit in result
+
+    bitMask >> 1; // Shift the bit mask
+    count++; // Increment the count
+    twosComplement.unsigned64 <<= 1; // Shift the two's complement mask
+
+    if (bitMask == 0) // Have we counted 8 bits?
+    {
+      ptr++; // Point to the next byte
+      bitMask = 0x80; // Reset the bit mask
+    }
+
+    if (count < (start + width)) // Do we need to shift result?
+      result.unsigned64 <<= 1; // Shift the result
+  }
+
+  // Handle negative number
+  if (isNegative)
+    result.unsigned64 |= twosComplement.unsigned64; // OR in the two's complement mask
+
+  return result.signed64;
+}
+
+
diff --git a/src/u-blox_GNSS.h b/src/u-blox_GNSS.h
@@ -1254,8 +1254,13 @@ class DevUBLOXGNSS
   bool setNMEAGNZDAcallbackPtr(void (*callbackPointerPtr)(NMEA_ZDA_data_t *)); // Enable a callback on the arrival of a GNZDA message
 #endif
 
-  // Helper functions for RTCM logging
+  // RTCM
+
 #ifndef SFE_UBLOX_DISABLE_RTCM_LOGGING
+  uint8_t getLatestRTCM1005(RTCM_1005_data_t *data); // Return the most recent RTCM 1005: 0 = no data, 1 = stale data, 2 = fresh data
+  bool setRTCM1005callbackPtr(void (*callbackPointerPtr)(RTCM_1005_data_t *)); // Configure a callback for the RTCM 1005 Message
+
+  // Helper functions for RTCM logging
   bool setRTCMLoggingMask(uint32_t messages = SFE_UBLOX_FILTER_RTCM_ALL); // Add selected RTCM messages to file buffer - if enabled. Default to adding ALL messages to the file buffer
   uint32_t getRTCMLoggingMask();                                          // Return which RTCM messages are selected for logging to the file buffer - if enabled
 #endif
@@ -1276,6 +1281,10 @@ class DevUBLOXGNSS
   float extractFloat(ubxPacket *msg, uint16_t spotToStart);       // Get signed 32-bit float (R4) from payload
   double extractDouble(ubxPacket *msg, uint16_t spotToStart);     // Get signed 64-bit double (R8) from payload
 
+  // Functions to help extract RTCM bit fields
+  uint16_t extractUnsignedBits16(uint8_t *ptr, uint16_t start, uint16_t width);
+  int64_t extractSignedBits64(uint8_t *ptr, uint16_t start, uint16_t width);
+
   // Pointers to storage for the "automatic" messages
   // RAM is allocated for these if/when required.
 
@@ -1342,6 +1351,8 @@ class DevUBLOXGNSS
   NMEA_GNZDA_t *storageNMEAGNZDA = nullptr; // Pointer to struct. RAM will be allocated for this if/when necessary
 #endif
 
+  RTCM_1005_t *storageRTCM1005 = nullptr; // Pointer to struct. RAM will be allocated for this if/when necessary
+
   uint16_t rtcmFrameCounter = 0; // Tracks the type of incoming byte inside RTCM frame
 
 protected:
@@ -1419,6 +1430,8 @@ class DevUBLOXGNSS
   bool initStorageRTCM(); // Allocate RAM for incoming RTCM messages and initialize it
   bool initStorageNMEA(); // Allocate RAM for incoming non-Auto NMEA messages and initialize it
 
+  bool initStorageRTCM1005(); // Allocate RAM for incoming RTCM 1005 messages and initialize it
+
   // Variables
   SparkFun_UBLOX_GNSS::GNSSDeviceBus *_sfeBus;
 
diff --git a/src/u-blox_structs.h b/src/u-blox_structs.h
