From a8c536cf6f5f6a00f1892415d6454b58fd1b0f99 Mon Sep 17 00:00:00 2001 From: Ferdinand von Hagen Date: Sun, 27 Nov 2022 14:08:41 +0100 Subject: [PATCH] Implement MEASX message support Implements MEASX message support according to the Adding_New_Messages.md document. --- src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 281 ++++++++++++++++++- src/SparkFun_u-blox_GNSS_Arduino_Library.h | 12 + src/u-blox_structs.h | 59 ++++ 3 files changed, 351 insertions(+), 1 deletion(-) diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp index 16ea35f..4f2b3a5 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp @@ -381,6 +381,16 @@ void SFE_UBLOX_GNSS::end(void) packetUBXRXMRAWX = NULL; // Redundant? } + if (packetUBXRXMMEASX != NULL) + { + if (packetUBXRXMMEASX->callbackData != NULL) + { + delete packetUBXRXMMEASX->callbackData; + } + delete packetUBXRXMMEASX; + packetUBXRXMMEASX = NULL; // Redundant? + } + if (packetUBXCFGRATE != NULL) { delete packetUBXCFGRATE; @@ -1410,6 +1420,10 @@ bool SFE_UBLOX_GNSS::checkAutomatic(uint8_t Class, uint8_t ID) if (packetUBXRXMRAWX != NULL) result = true; break; + case UBX_RXM_MEASX: + if (packetUBXRXMMEASX != NULL) + result = true; + break; case UBX_RXM_PMP: if ((packetUBXRXMPMP != NULL) || (packetUBXRXMPMPmessage != NULL)) result = true; @@ -1596,6 +1610,8 @@ uint16_t SFE_UBLOX_GNSS::getMaxPayloadSize(uint8_t Class, uint8_t ID) case UBX_RXM_RAWX: maxSize = UBX_RXM_RAWX_MAX_LEN; break; + case UBX_RXM_MEASX: + maxSize = UBX_RXM_MEASX_MAX_LEN; case UBX_RXM_PMP: maxSize = UBX_RXM_PMP_MAX_LEN; break; @@ -4081,6 +4097,58 @@ void SFE_UBLOX_GNSS::processUBXpacket(ubxPacket *msg) } } } + else if (msg->id == UBX_RXM_MEASX) + // Note: length is variable + { + // Parse various byte fields into storage - but only if we have memory allocated for it + if (packetUBXRXMMEASX != NULL) + { + packetUBXRXMMEASX->data.header.version = extractByte(msg, 0); + packetUBXRXMMEASX->data.header.gpsTOW = extractLong(msg, 4); + packetUBXRXMMEASX->data.header.gloTOW = extractLong(msg, 8); + packetUBXRXMMEASX->data.header.bdsTOW = extractLong(msg, 12); + packetUBXRXMMEASX->data.header.qzssTOW = extractLong(msg, 20); + packetUBXRXMMEASX->data.header.gpsTOWacc = extractInt(msg, 24); + packetUBXRXMMEASX->data.header.gloTOWacc = extractInt(msg, 26); + packetUBXRXMMEASX->data.header.bdsTOWacc = extractInt(msg, 28); + packetUBXRXMMEASX->data.header.qzssTOWacc = extractInt(msg, 32); + packetUBXRXMMEASX->data.header.numSV = extractByte(msg, 34); + packetUBXRXMMEASX->data.header.flags.all = extractByte(msg, 35); + + for (uint8_t i = 0; (i < UBX_RXM_MEASX_MAX_BLOCKS) && (i < packetUBXRXMMEASX->data.header.numSV) && ((((uint16_t)i) * 24) < (msg->len - 44)); i++) + { + uint16_t offset = (((uint16_t)i) * 24) + 44; + packetUBXRXMMEASX->data.blocks[i].gnssId = extractByte(msg, offset + 0); + packetUBXRXMMEASX->data.blocks[i].svId = extractByte(msg, offset + 1); + packetUBXRXMMEASX->data.blocks[i].cNo = extractByte(msg, offset + 2); + packetUBXRXMMEASX->data.blocks[i].mpathIndic = extractByte(msg, offset + 3); + packetUBXRXMMEASX->data.blocks[i].dopplerMS = extractSignedLong(msg, offset + 4); + packetUBXRXMMEASX->data.blocks[i].dopplerHz = extractSignedLong(msg, offset + 8); + packetUBXRXMMEASX->data.blocks[i].wholeChips = extractInt(msg, offset + 12); + packetUBXRXMMEASX->data.blocks[i].fracChips = extractInt(msg, offset + 14); + packetUBXRXMMEASX->data.blocks[i].codePhase = extractLong(msg, offset + 16); + packetUBXRXMMEASX->data.blocks[i].intCodePhase = extractByte(msg, offset + 20); + packetUBXRXMMEASX->data.blocks[i].pseuRangeRMSErr = extractByte(msg, offset + 21); + } + + // Mark all datums as fresh (not read before) + packetUBXRXMMEASX->moduleQueried = true; + + // Check if we need to copy the data for the callback + if ((packetUBXRXMMEASX->callbackData != NULL) // If RAM has been allocated for the copy of the data + && (packetUBXRXMMEASX->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale + { + memcpy(&packetUBXRXMMEASX->callbackData->header.version, &packetUBXRXMMEASX->data.header.version, sizeof(UBX_RXM_MEASX_data_t)); + packetUBXRXMMEASX->automaticFlags.flags.bits.callbackCopyValid = true; + } + + // Check if we need to copy the data into the file buffer + if (packetUBXRXMMEASX->automaticFlags.flags.bits.addToFileBuffer) + { + storePacket(msg); + } + } + } break; case UBX_CLASS_CFG: if (msg->id == UBX_CFG_PRT && msg->len == UBX_CFG_PRT_LEN) @@ -5669,6 +5737,25 @@ void SFE_UBLOX_GNSS::checkCallbacks(void) packetUBXRXMRAWX->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale } + if ((packetUBXRXMMEASX != NULL) // If RAM has been allocated for message storage + && (packetUBXRXMMEASX->callbackData != NULL) // If RAM has been allocated for the copy of the data + && (packetUBXRXMMEASX->automaticFlags.flags.bits.callbackCopyValid == true)) // If the copy of the data is valid + { + if (packetUBXRXMMEASX->callbackPointer != NULL) // If the pointer to the callback has been defined + { + // if (_printDebug == true) + // _debugSerial->println(F("checkCallbacks: calling callback for RXM RAWX")); + packetUBXRXMMEASX->callbackPointer(*packetUBXRXMMEASX->callbackData); // Call the callback + } + if (packetUBXRXMMEASX->callbackPointerPtr != NULL) // If the pointer to the callback has been defined + { + // if (_printDebug == true) + // _debugSerial->println(F("checkCallbacks: calling callbackPtr for RXM RAWX")); + packetUBXRXMMEASX->callbackPointerPtr(packetUBXRXMMEASX->callbackData); // Call the callback + } + packetUBXRXMMEASX->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale + } + if ((packetUBXTIMTM2 != NULL) // If RAM has been allocated for message storage && (packetUBXTIMTM2->callbackData != NULL) // If RAM has been allocated for the copy of the data && (packetUBXTIMTM2->automaticFlags.flags.bits.callbackCopyValid == true)) // If the copy of the data is valid @@ -13653,7 +13740,7 @@ void SFE_UBLOX_GNSS::logRXMSFRBX(bool enabled) bool SFE_UBLOX_GNSS::getRXMRAWX(uint16_t maxWait) { if (packetUBXRXMRAWX == NULL) - initPacketUBXRXMRAWX(); // Check that RAM has been allocated for the RAWX data + initPacketUBXRXMRAWX(); // Check that RAM has been allocated for the TM2 data if (packetUBXRXMRAWX == NULL) // Bail if the RAM allocation failed return (false); @@ -13840,6 +13927,198 @@ void SFE_UBLOX_GNSS::logRXMRAWX(bool enabled) packetUBXRXMRAWX->automaticFlags.flags.bits.addToFileBuffer = (uint8_t)enabled; } +// ***** RXM MEASX automatic support + +bool SFE_UBLOX_GNSS::getRXMMEASX(uint16_t maxWait) +{ + if (packetUBXRXMMEASX == NULL) + initPacketUBXRXMMEASX(); // Check that RAM has been allocated for the MEASX data + if (packetUBXRXMMEASX == NULL) // Bail if the RAM allocation failed + return (false); + + if (packetUBXRXMMEASX->automaticFlags.flags.bits.automatic && packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate) + { + // The GPS is automatically reporting, we just check whether we got unread data + checkUbloxInternal(&packetCfg, UBX_CLASS_RXM, UBX_RXM_MEASX); + return packetUBXRXMMEASX->moduleQueried; + } + else if (packetUBXRXMMEASX->automaticFlags.flags.bits.automatic && !packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate) + { + // Someone else has to call checkUblox for us... + return (false); + } + else + { + // The GPS is not automatically reporting navigation position so we have to poll explicitly + packetCfg.cls = UBX_CLASS_RXM; + packetCfg.id = UBX_RXM_MEASX; + packetCfg.len = 0; + packetCfg.startingSpot = 0; + + // The data is parsed as part of processing the response + sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait); + + if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED) + return (true); + + if (retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) + { + return (true); + } + + return (false); + } +} + +// Enable or disable automatic navigation message generation by the GNSS. This changes the way getRXMMEASX +// works. +bool SFE_UBLOX_GNSS::setAutoRXMMEASX(bool enable, uint16_t maxWait) +{ + return setAutoRXMMEASXrate(enable ? 1 : 0, true, maxWait); +} + +// Enable or disable automatic navigation message generation by the GNSS. This changes the way getRXMMEASX +// works. +bool SFE_UBLOX_GNSS::setAutoRXMMEASX(bool enable, bool implicitUpdate, uint16_t maxWait) +{ + return setAutoRXMMEASXrate(enable ? 1 : 0, implicitUpdate, maxWait); +} + +// Enable or disable automatic navigation message generation by the GNSS. This changes the way getRXMMEASX +// works. +bool SFE_UBLOX_GNSS::setAutoRXMMEASXrate(uint8_t rate, bool implicitUpdate, uint16_t maxWait) +{ + if (packetUBXRXMMEASX == NULL) + initPacketUBXRXMMEASX(); // Check that RAM has been allocated for the data + if (packetUBXRXMMEASX == NULL) // Only attempt this if RAM allocation was successful + return false; + + if (rate > 127) + rate = 127; + + packetCfg.cls = UBX_CLASS_CFG; + packetCfg.id = UBX_CFG_MSG; + packetCfg.len = 3; + packetCfg.startingSpot = 0; + payloadCfg[0] = UBX_CLASS_RXM; + payloadCfg[1] = UBX_RXM_MEASX; + payloadCfg[2] = rate; // rate relative to navigation freq. + + bool ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK + if (ok) + { + packetUBXRXMMEASX->automaticFlags.flags.bits.automatic = (rate > 0); + packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate = implicitUpdate; + } + packetUBXRXMMEASX->moduleQueried = false; + return ok; +} + +// Enable automatic navigation message generation by the GNSS. +bool SFE_UBLOX_GNSS::setAutoRXMMEASXcallback(void (*callbackPointer)(UBX_RXM_MEASX_data_t), uint16_t maxWait) +{ + // Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually. + bool result = setAutoRXMMEASX(true, false, maxWait); + if (!result) + return (result); // Bail if setAuto failed + + if (packetUBXRXMMEASX->callbackData == NULL) // Check if RAM has been allocated for the callback copy + { + packetUBXRXMMEASX->callbackData = new UBX_RXM_MEASX_data_t; // Allocate RAM for the main struct + } + + if (packetUBXRXMMEASX->callbackData == NULL) + { +#ifndef SFE_UBLOX_REDUCED_PROG_MEM + if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging + _debugSerial->println(F("setAutoRXMMEASXcallback: RAM alloc failed!")); +#endif + return (false); + } + + packetUBXRXMMEASX->callbackPointer = callbackPointer; + return (true); +} + +bool SFE_UBLOX_GNSS::setAutoRXMMEASXcallbackPtr(void (*callbackPointerPtr)(UBX_RXM_MEASX_data_t *), uint16_t maxWait) +{ + // Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually. + bool result = setAutoRXMMEASX(true, false, maxWait); + if (!result) + return (result); // Bail if setAuto failed + + if (packetUBXRXMMEASX->callbackData == NULL) // Check if RAM has been allocated for the callback copy + { + packetUBXRXMMEASX->callbackData = new UBX_RXM_MEASX_data_t; // Allocate RAM for the main struct + } + + if (packetUBXRXMMEASX->callbackData == NULL) + { +#ifndef SFE_UBLOX_REDUCED_PROG_MEM + if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging + _debugSerial->println(F("setAutoRXMMEASXcallbackPtr: RAM alloc failed!")); +#endif + return (false); + } + + packetUBXRXMMEASX->callbackPointerPtr = callbackPointerPtr; + return (true); +} + +// In case no config access to the GNSS is possible and VELNED is send cyclically already +// set config to suitable parameters +bool SFE_UBLOX_GNSS::assumeAutoRXMMEASX(bool enabled, bool implicitUpdate) +{ + if (packetUBXRXMMEASX == NULL) + initPacketUBXRXMMEASX(); // Check that RAM has been allocated for the data + if (packetUBXRXMMEASX == NULL) // Only attempt this if RAM allocation was successful + return false; + + bool changes = packetUBXRXMMEASX->automaticFlags.flags.bits.automatic != enabled || packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate != implicitUpdate; + if (changes) + { + packetUBXRXMMEASX->automaticFlags.flags.bits.automatic = enabled; + packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate = implicitUpdate; + } + return changes; +} + +// PRIVATE: Allocate RAM for packetUBXRXMMEASX and initialize it +bool SFE_UBLOX_GNSS::initPacketUBXRXMMEASX() +{ + packetUBXRXMMEASX = new UBX_RXM_MEASX_t; // Allocate RAM for the main struct + if (packetUBXRXMMEASX == NULL) + { +#ifndef SFE_UBLOX_REDUCED_PROG_MEM + if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging + _debugSerial->println(F("initPacketUBXRXMMEASX: RAM alloc failed!")); +#endif + return (false); + } + packetUBXRXMMEASX->automaticFlags.flags.all = 0; + packetUBXRXMMEASX->callbackPointer = NULL; + packetUBXRXMMEASX->callbackPointerPtr = NULL; + packetUBXRXMMEASX->callbackData = NULL; + packetUBXRXMMEASX->moduleQueried = false; + return (true); +} + +// Mark all the data as read/stale +void SFE_UBLOX_GNSS::flushRXMMEASX() +{ + if (packetUBXRXMMEASX == NULL) + return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!) + packetUBXRXMMEASX->moduleQueried = false; // Mark all datums as stale (read before) +} + +// Log this data in file buffer +void SFE_UBLOX_GNSS::logRXMMEASX(bool enabled) +{ + if (packetUBXRXMMEASX == NULL) + return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!) + packetUBXRXMMEASX->automaticFlags.flags.bits.addToFileBuffer = (uint8_t)enabled; +} + // ***** CFG automatic support // Get the latest CFG PRT - as used by isConnected diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.h b/src/SparkFun_u-blox_GNSS_Arduino_Library.h index 18f2562..8df4ad8 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.h +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.h @@ -1225,6 +1225,16 @@ class SFE_UBLOX_GNSS void flushRXMRAWX(); // Mark all the data as read/stale void logRXMRAWX(bool enabled = true); // Log data to file buffer + bool getRXMMEASX(uint16_t maxWait = defaultMaxWait); // RXM RAWX + bool setAutoRXMMEASX(bool enabled, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic RXM RAWX reports at the navigation frequency + bool setAutoRXMMEASX(bool enabled, bool implicitUpdate, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic RXM RAWX 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 setAutoRXMMEASXrate(uint8_t rate, bool implicitUpdate = true, uint16_t maxWait = defaultMaxWait); // Set the rate for automatic RAWX reports + bool setAutoRXMMEASXcallback(void (*callbackPointer)(UBX_RXM_MEASX_data_t), uint16_t maxWait = defaultMaxWait); // Enable automatic RAWX reports at the navigation frequency. Data is accessed from the callback. + bool setAutoRXMMEASXcallbackPtr(void (*callbackPointerPtr)(UBX_RXM_MEASX_data_t *), uint16_t maxWait = defaultMaxWait); // Enable automatic RAWX reports at the navigation frequency. Data is accessed from the callback. + bool assumeAutoRXMMEASX(bool enabled, bool implicitUpdate = true); // In case no config access to the GPS is possible and RXM RAWX is send cyclically already + void flushRXMMEASX(); // Mark all the data as read/stale + void logRXMMEASX(bool enabled = true); // Log data to file buffer + // Configuration (CFG) // Add "auto" support for CFG PRT - because we use it for isConnected (to stop it being mugged by other messages) @@ -1562,6 +1572,7 @@ class SFE_UBLOX_GNSS UBX_RXM_COR_t *packetUBXRXMCOR = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary UBX_RXM_SFRBX_t *packetUBXRXMSFRBX = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary UBX_RXM_RAWX_t *packetUBXRXMRAWX = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary + UBX_RXM_MEASX_t *packetUBXRXMMEASX = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary UBX_CFG_PRT_t *packetUBXCFGPRT = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary UBX_CFG_RATE_t *packetUBXCFGRATE = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary @@ -1655,6 +1666,7 @@ class SFE_UBLOX_GNSS bool initPacketUBXRXMCOR(); // Allocate RAM for packetUBXRXMCOR and initialize it bool initPacketUBXRXMSFRBX(); // Allocate RAM for packetUBXRXMSFRBX and initialize it bool initPacketUBXRXMRAWX(); // Allocate RAM for packetUBXRXMRAWX and initialize it + bool initPacketUBXRXMMEASX(); // Allocate RAM for packetUBXRXMMEASX and initialize it bool initPacketUBXCFGPRT(); // Allocate RAM for packetUBXCFGPRT and initialize it bool initPacketUBXCFGRATE(); // Allocate RAM for packetUBXCFGRATE and initialize it bool initPacketUBXTIMTM2(); // Allocate RAM for packetUBXTIMTM2 and initialize it diff --git a/src/u-blox_structs.h b/src/u-blox_structs.h index 06160ce..013e224 100644 --- a/src/u-blox_structs.h +++ b/src/u-blox_structs.h @@ -1521,6 +1521,65 @@ typedef struct UBX_RXM_RAWX_data_t *callbackData; } UBX_RXM_RAWX_t; +// UBX-RXM-MEASX (0x02 0x14): Receiver Manager Messages: i.e. Satellite Status, RTC Status. +const uint8_t UBX_RXM_MEASX_MAX_BLOCKS = 92; +const uint16_t UBX_RXM_MEASX_MAX_LEN = 44 + (24 * UBX_RXM_MEASX_MAX_BLOCKS); + +typedef struct { + uint8_t version; // Message version (0x01 for this version) + uint8_t reserved1[3]; + uint32_t gpsTOW; + uint32_t gloTOW; + uint32_t bdsTOW; + uint8_t reserved2[4]; + uint32_t qzssTOW; + uint16_t gpsTOWacc; + uint16_t gloTOWacc; + uint16_t bdsTOWacc; + uint8_t reserved3[2]; + uint16_t qzssTOWacc; + uint8_t numSV; + union + { + uint8_t all; + struct + { + uint8_t towSet : 2; // TOW set (0 = no, 1 or 2 = yes) + } bits; + } flags; + uint8_t reserved4[8]; +} UBX_RXM_MEASX_header_t; + +typedef struct { + uint8_t gnssId; + uint8_t svId; + uint8_t cNo; + uint8_t mpathIndic; + int32_t dopplerMS; + int32_t dopplerHz; + uint16_t wholeChips; + uint16_t fracChips; + uint32_t codePhase; + uint8_t intCodePhase; + uint8_t pseuRangeRMSErr; + uint8_t reserved5[2]; +} UBX_RXM_MEASX_block_t; + +typedef struct { + UBX_RXM_MEASX_header_t header; + UBX_RXM_MEASX_block_t blocks[UBX_RXM_MEASX_MAX_BLOCKS]; +} UBX_RXM_MEASX_data_t; + +typedef struct +{ + ubxAutomaticFlags automaticFlags; + UBX_RXM_MEASX_data_t data; + bool moduleQueried; + void (*callbackPointer)(UBX_RXM_MEASX_data_t); + void (*callbackPointerPtr)(UBX_RXM_MEASX_data_t *); + UBX_RXM_MEASX_data_t *callbackData; +} UBX_RXM_MEASX_t; + // UBX-RXM-COR (0x02 0x34): Differential correction input status const uint16_t UBX_RXM_COR_LEN = 12;