Skip to content

Commit f71111f

Browse files
PaulZCPaulZC
committed
Add #169 
1 parent 7aaffa2 commit f71111f

File tree

3 files changed

+324
-11
lines changed

3 files changed

+324
-11
lines changed

Diff for: ‎src/u-blox_GNSS.cpp

+255-11
Original file line numberDiff line numberDiff line change
@@ -363,6 +363,16 @@ void DevUBLOXGNSS::end(void)
363363
delete packetUBXRXMRAWX;
364364
packetUBXRXMRAWX = nullptr;
365365
}
366+
367+
if (packetUBXRXMMEASX != nullptr)
368+
{
369+
if (packetUBXRXMMEASX->callbackData != nullptr)
370+
{
371+
delete packetUBXRXMMEASX->callbackData;
372+
}
373+
delete packetUBXRXMMEASX;
374+
packetUBXRXMMEASX = nullptr;
375+
}
366376
#endif
367377

368378
if (packetUBXTIMTM2 != nullptr)
@@ -1091,6 +1101,7 @@ bool DevUBLOXGNSS::autoLookup(uint8_t Class, uint8_t ID, uint16_t *maxSize)
10911101
{UBX_CLASS_RXM, UBX_RXM_RAWX, packetUBXRXMRAWX, UBX_RXM_RAWX_MAX_LEN},
10921102
{UBX_CLASS_RXM, UBX_RXM_QZSSL6, packetUBXRXMQZSSL6message, UBX_RXM_QZSSL6_MAX_LEN},
10931103
{UBX_CLASS_RXM, UBX_RXM_COR, packetUBXRXMCOR, UBX_RXM_COR_LEN},
1104+
{UBX_CLASS_RXM, UBX_RXM_MEASX, packetUBXRXMMEASX, UBX_RXM_MEASX_MAX_LEN},
10941105
#endif
10951106
{UBX_CLASS_TIM, UBX_TIM_TM2, packetUBXTIMTM2, UBX_TIM_TM2_LEN},
10961107
#ifndef SFE_UBLOX_DISABLE_ESF
@@ -3539,6 +3550,59 @@ void DevUBLOXGNSS::processUBXpacket(ubxPacket *msg)
35393550
}
35403551
}
35413552
}
3553+
else if (msg->id == UBX_RXM_MEASX)
3554+
// Note: length is variable
3555+
{
3556+
// Parse various byte fields into storage - but only if we have memory allocated for it
3557+
if (packetUBXRXMMEASX != nullptr)
3558+
{
3559+
packetUBXRXMMEASX->data.header.version = extractByte(msg, 0);
3560+
packetUBXRXMMEASX->data.header.gpsTOW = extractLong(msg, 4);
3561+
packetUBXRXMMEASX->data.header.gloTOW = extractLong(msg, 8);
3562+
packetUBXRXMMEASX->data.header.bdsTOW = extractLong(msg, 12);
3563+
packetUBXRXMMEASX->data.header.qzssTOW = extractLong(msg, 20);
3564+
packetUBXRXMMEASX->data.header.gpsTOWacc = extractInt(msg, 24);
3565+
packetUBXRXMMEASX->data.header.gloTOWacc = extractInt(msg, 26);
3566+
packetUBXRXMMEASX->data.header.bdsTOWacc = extractInt(msg, 28);
3567+
packetUBXRXMMEASX->data.header.qzssTOWacc = extractInt(msg, 32);
3568+
packetUBXRXMMEASX->data.header.numSV = extractByte(msg, 34);
3569+
packetUBXRXMMEASX->data.header.flags.all = extractByte(msg, 35);
3570+
3571+
for (uint8_t i = 0; (i < UBX_RXM_MEASX_MAX_BLOCKS) && (i < packetUBXRXMMEASX->data.header.numSV) && ((((uint16_t)i) * 24) < (msg->len - 44)); i++)
3572+
{
3573+
uint16_t offset = (((uint16_t)i) * 24) + 44;
3574+
packetUBXRXMMEASX->data.blocks[i].gnssId = extractByte(msg, offset + 0);
3575+
packetUBXRXMMEASX->data.blocks[i].svId = extractByte(msg, offset + 1);
3576+
packetUBXRXMMEASX->data.blocks[i].cNo = extractByte(msg, offset + 2);
3577+
packetUBXRXMMEASX->data.blocks[i].mpathIndic = extractByte(msg, offset + 3);
3578+
packetUBXRXMMEASX->data.blocks[i].dopplerMS = extractSignedLong(msg, offset + 4);
3579+
packetUBXRXMMEASX->data.blocks[i].dopplerHz = extractSignedLong(msg, offset + 8);
3580+
packetUBXRXMMEASX->data.blocks[i].wholeChips = extractInt(msg, offset + 12);
3581+
packetUBXRXMMEASX->data.blocks[i].fracChips = extractInt(msg, offset + 14);
3582+
packetUBXRXMMEASX->data.blocks[i].codePhase = extractLong(msg, offset + 16);
3583+
packetUBXRXMMEASX->data.blocks[i].intCodePhase = extractByte(msg, offset + 20);
3584+
packetUBXRXMMEASX->data.blocks[i].pseuRangeRMSErr = extractByte(msg, offset + 21);
3585+
}
3586+
3587+
// Mark all datums as fresh (not read before)
3588+
packetUBXRXMMEASX->moduleQueried = true;
3589+
3590+
// Check if we need to copy the data for the callback
3591+
if ((packetUBXRXMMEASX->callbackData != nullptr) // If RAM has been allocated for the copy of the data
3592+
&& (packetUBXRXMMEASX->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
3593+
{
3594+
memcpy(&packetUBXRXMMEASX->callbackData->header.version, &packetUBXRXMMEASX->data.header.version, sizeof(UBX_RXM_MEASX_data_t));
3595+
packetUBXRXMMEASX->automaticFlags.flags.bits.callbackCopyValid = true;
3596+
}
3597+
3598+
// Check if we need to copy the data into the file buffer
3599+
if (packetUBXRXMMEASX->automaticFlags.flags.bits.addToFileBuffer)
3600+
{
3601+
storePacket(msg);
3602+
}
3603+
}
3604+
}
3605+
break;
35423606
break;
35433607
#endif
35443608
case UBX_CLASS_TIM:
@@ -4959,6 +5023,17 @@ void DevUBLOXGNSS::checkCallbacks(void)
49595023
}
49605024
packetUBXRXMRAWX->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
49615025
}
5026+
5027+
if (packetUBXRXMMEASX != nullptr) // If RAM has been allocated for message storage
5028+
if (packetUBXRXMMEASX->callbackData != nullptr) // If RAM has been allocated for the copy of the data
5029+
if (packetUBXRXMMEASX->automaticFlags.flags.bits.callbackCopyValid == true) // If the copy of the data is valid
5030+
{
5031+
if (packetUBXRXMMEASX->callbackPointerPtr != nullptr) // If the pointer to the callback has been defined
5032+
{
5033+
packetUBXRXMMEASX->callbackPointerPtr(packetUBXRXMMEASX->callbackData); // Call the callback
5034+
}
5035+
packetUBXRXMMEASX->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
5036+
}
49625037
#endif
49635038

49645039
if (packetUBXTIMTM2 != nullptr) // If RAM has been allocated for message storage
@@ -6453,11 +6528,11 @@ void DevUBLOXGNSS::softwareEnableGNSS(bool enable)
64536528
packetCfg.id = UBX_CFG_RST;
64546529
packetCfg.len = 4;
64556530
packetCfg.startingSpot = 0;
6456-
payloadCfg[0] = 0; // hot start
6457-
payloadCfg[1] = 0; // hot start
6531+
payloadCfg[0] = 0; // hot start
6532+
payloadCfg[1] = 0; // hot start
64586533
payloadCfg[2] = enable ? 0x09 : 0x08; // 0x09 = start GNSS, 0x08 = stop GNSS
6459-
payloadCfg[3] = 0; // reserved
6460-
sendCommand(&packetCfg, 0); // don't expect ACK
6534+
payloadCfg[3] = 0; // reserved
6535+
sendCommand(&packetCfg, 0); // don't expect ACK
64616536
}
64626537

64636538
// Reset module to factory defaults
@@ -6981,13 +7056,13 @@ bool DevUBLOXGNSS::resetOdometer(uint16_t maxWait)
69817056
uint32_t getEnableGNSSConfigKey(sfe_ublox_gnss_ids_e id)
69827057
{
69837058
const uint32_t gnssConfigKeys[(uint8_t)SFE_UBLOX_GNSS_ID_UNKNOWN] = {
6984-
UBLOX_CFG_SIGNAL_GPS_ENA,
6985-
UBLOX_CFG_SIGNAL_SBAS_ENA,
6986-
UBLOX_CFG_SIGNAL_GAL_ENA,
6987-
UBLOX_CFG_SIGNAL_BDS_ENA,
6988-
0, // IMES has no ENA key
6989-
UBLOX_CFG_SIGNAL_QZSS_ENA,
6990-
UBLOX_CFG_SIGNAL_GLO_ENA};
7059+
UBLOX_CFG_SIGNAL_GPS_ENA,
7060+
UBLOX_CFG_SIGNAL_SBAS_ENA,
7061+
UBLOX_CFG_SIGNAL_GAL_ENA,
7062+
UBLOX_CFG_SIGNAL_BDS_ENA,
7063+
0, // IMES has no ENA key
7064+
UBLOX_CFG_SIGNAL_QZSS_ENA,
7065+
UBLOX_CFG_SIGNAL_GLO_ENA};
69917066

69927067
if (id >= SFE_UBLOX_GNSS_ID_UNKNOWN)
69937068
return 0;
@@ -11474,6 +11549,175 @@ void DevUBLOXGNSS::logRXMRAWX(bool enabled)
1147411549
return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
1147511550
packetUBXRXMRAWX->automaticFlags.flags.bits.addToFileBuffer = (uint8_t)enabled;
1147611551
}
11552+
11553+
// ***** RXM MEASX automatic support
11554+
11555+
bool DevUBLOXGNSS::getRXMMEASX(uint16_t maxWait)
11556+
{
11557+
if (packetUBXRXMMEASX == nullptr)
11558+
initPacketUBXRXMMEASX(); // Check that RAM has been allocated for the MEASX data
11559+
if (packetUBXRXMMEASX == nullptr) // Bail if the RAM allocation failed
11560+
return (false);
11561+
11562+
if (packetUBXRXMMEASX->automaticFlags.flags.bits.automatic && packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate)
11563+
{
11564+
// The GPS is automatically reporting, we just check whether we got unread data
11565+
checkUbloxInternal(&packetCfg, UBX_CLASS_RXM, UBX_RXM_MEASX);
11566+
return packetUBXRXMMEASX->moduleQueried;
11567+
}
11568+
else if (packetUBXRXMMEASX->automaticFlags.flags.bits.automatic && !packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate)
11569+
{
11570+
// Someone else has to call checkUblox for us...
11571+
return (false);
11572+
}
11573+
else
11574+
{
11575+
// The GPS is not automatically reporting navigation position so we have to poll explicitly
11576+
packetCfg.cls = UBX_CLASS_RXM;
11577+
packetCfg.id = UBX_RXM_MEASX;
11578+
packetCfg.len = 0;
11579+
packetCfg.startingSpot = 0;
11580+
11581+
// The data is parsed as part of processing the response
11582+
sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait);
11583+
11584+
if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED)
11585+
return (true);
11586+
11587+
if (retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN)
11588+
{
11589+
return (true);
11590+
}
11591+
11592+
return (false);
11593+
}
11594+
}
11595+
11596+
// Enable or disable automatic navigation message generation by the GNSS. This changes the way getRXMMEASX
11597+
// works.
11598+
bool DevUBLOXGNSS::setAutoRXMMEASX(bool enable, uint16_t maxWait)
11599+
{
11600+
return setAutoRXMMEASXrate(enable ? 1 : 0, true, maxWait);
11601+
}
11602+
11603+
// Enable or disable automatic navigation message generation by the GNSS. This changes the way getRXMMEASX
11604+
// works.
11605+
bool DevUBLOXGNSS::setAutoRXMMEASX(bool enable, bool implicitUpdate, uint16_t maxWait)
11606+
{
11607+
return setAutoRXMMEASXrate(enable ? 1 : 0, implicitUpdate, maxWait);
11608+
}
11609+
11610+
// Enable or disable automatic navigation message generation by the GNSS. This changes the way getRXMMEASX
11611+
// works.
11612+
bool DevUBLOXGNSS::setAutoRXMMEASXrate(uint8_t rate, bool implicitUpdate, uint16_t maxWait)
11613+
{
11614+
if (packetUBXRXMMEASX == nullptr)
11615+
initPacketUBXRXMMEASX(); // Check that RAM has been allocated for the data
11616+
if (packetUBXRXMMEASX == nullptr) // Only attempt this if RAM allocation was successful
11617+
return false;
11618+
11619+
if (rate > 127)
11620+
rate = 127;
11621+
11622+
uint32_t key = UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_I2C;
11623+
if (_commType == COMM_TYPE_SPI)
11624+
key = UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_SPI;
11625+
else if (_commType == COMM_TYPE_SERIAL)
11626+
{
11627+
if (!_UART2)
11628+
key = UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_UART1;
11629+
else
11630+
key = UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_UART2;
11631+
}
11632+
11633+
bool ok = setVal8(key, rate, VAL_LAYER_ALL, maxWait);
11634+
if (ok)
11635+
{
11636+
packetUBXRXMMEASX->automaticFlags.flags.bits.automatic = (rate > 0);
11637+
packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
11638+
}
11639+
packetUBXRXMMEASX->moduleQueried = false;
11640+
return ok;
11641+
}
11642+
11643+
// Enable automatic navigation message generation by the GNSS.
11644+
bool DevUBLOXGNSS::setAutoRXMMEASXcallbackPtr(void (*callbackPointerPtr)(UBX_RXM_MEASX_data_t *), uint16_t maxWait)
11645+
{
11646+
// Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually.
11647+
bool result = setAutoRXMMEASX(true, false, maxWait);
11648+
if (!result)
11649+
return (result); // Bail if setAuto failed
11650+
11651+
if (packetUBXRXMMEASX->callbackData == nullptr) // Check if RAM has been allocated for the callback copy
11652+
{
11653+
packetUBXRXMMEASX->callbackData = new UBX_RXM_MEASX_data_t; // Allocate RAM for the main struct
11654+
}
11655+
11656+
if (packetUBXRXMMEASX->callbackData == nullptr)
11657+
{
11658+
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
11659+
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
11660+
_debugSerial->println(F("setAutoRXMMEASXcallbackPtr: RAM alloc failed!"));
11661+
#endif
11662+
return (false);
11663+
}
11664+
11665+
packetUBXRXMMEASX->callbackPointerPtr = callbackPointerPtr;
11666+
return (true);
11667+
}
11668+
11669+
// In case no config access to the GNSS is possible and VELNED is send cyclically already
11670+
// set config to suitable parameters
11671+
bool DevUBLOXGNSS::assumeAutoRXMMEASX(bool enabled, bool implicitUpdate)
11672+
{
11673+
if (packetUBXRXMMEASX == nullptr)
11674+
initPacketUBXRXMMEASX(); // Check that RAM has been allocated for the data
11675+
if (packetUBXRXMMEASX == nullptr) // Only attempt this if RAM allocation was successful
11676+
return false;
11677+
11678+
bool changes = packetUBXRXMMEASX->automaticFlags.flags.bits.automatic != enabled || packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate != implicitUpdate;
11679+
if (changes)
11680+
{
11681+
packetUBXRXMMEASX->automaticFlags.flags.bits.automatic = enabled;
11682+
packetUBXRXMMEASX->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
11683+
}
11684+
return changes;
11685+
}
11686+
11687+
// PRIVATE: Allocate RAM for packetUBXRXMMEASX and initialize it
11688+
bool DevUBLOXGNSS::initPacketUBXRXMMEASX()
11689+
{
11690+
packetUBXRXMMEASX = new UBX_RXM_MEASX_t; // Allocate RAM for the main struct
11691+
if (packetUBXRXMMEASX == nullptr)
11692+
{
11693+
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
11694+
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
11695+
_debugSerial->println(F("initPacketUBXRXMMEASX: RAM alloc failed!"));
11696+
#endif
11697+
return (false);
11698+
}
11699+
packetUBXRXMMEASX->automaticFlags.flags.all = 0;
11700+
packetUBXRXMMEASX->callbackPointerPtr = nullptr;
11701+
packetUBXRXMMEASX->callbackData = nullptr;
11702+
packetUBXRXMMEASX->moduleQueried = false;
11703+
return (true);
11704+
}
11705+
11706+
// Mark all the data as read/stale
11707+
void DevUBLOXGNSS::flushRXMMEASX()
11708+
{
11709+
if (packetUBXRXMMEASX == nullptr)
11710+
return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
11711+
packetUBXRXMMEASX->moduleQueried = false; // Mark all datums as stale (read before)
11712+
}
11713+
11714+
// Log this data in file buffer
11715+
void DevUBLOXGNSS::logRXMMEASX(bool enabled)
11716+
{
11717+
if (packetUBXRXMMEASX == nullptr)
11718+
return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
11719+
packetUBXRXMMEASX->automaticFlags.flags.bits.addToFileBuffer = (uint8_t)enabled;
11720+
}
1147711721
#endif
1147811722

1147911723
// ***** TIM TM2 automatic support

Diff for: ‎src/u-blox_GNSS.h

+11
Original file line numberDiff line numberDiff line change
@@ -635,6 +635,15 @@ class DevUBLOXGNSS
635635
bool assumeAutoRXMRAWX(bool enabled, bool implicitUpdate = true); // In case no config access to the GPS is possible and RXM RAWX is send cyclically already
636636
void flushRXMRAWX(); // Mark all the data as read/stale
637637
void logRXMRAWX(bool enabled = true); // Log data to file buffer
638+
639+
bool getRXMMEASX(uint16_t maxWait = kUBLOXGNSSDefaultMaxWait); // RXM RAWX
640+
bool setAutoRXMMEASX(bool enabled, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait); // Enable/disable automatic RXM RAWX reports at the navigation frequency
641+
bool setAutoRXMMEASX(bool enabled, bool implicitUpdate, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait); // 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
642+
bool setAutoRXMMEASXrate(uint8_t rate, bool implicitUpdate = true, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait); // Set the rate for automatic RAWX reports
643+
bool setAutoRXMMEASXcallbackPtr(void (*callbackPointerPtr)(UBX_RXM_MEASX_data_t *), uint16_t maxWait = kUBLOXGNSSDefaultMaxWait); // Enable automatic RAWX reports at the navigation frequency. Data is accessed from the callback.
644+
bool assumeAutoRXMMEASX(bool enabled, bool implicitUpdate = true); // In case no config access to the GPS is possible and RXM RAWX is send cyclically already
645+
void flushRXMMEASX(); // Mark all the data as read/stale
646+
void logRXMMEASX(bool enabled = true); // Log data to file buffer
638647
#endif
639648

640649
// Timing messages (TIM)
@@ -962,6 +971,7 @@ class DevUBLOXGNSS
962971
UBX_RXM_COR_t *packetUBXRXMCOR = nullptr; // Pointer to struct. RAM will be allocated for this if/when necessary
963972
UBX_RXM_SFRBX_t *packetUBXRXMSFRBX = nullptr; // Pointer to struct. RAM will be allocated for this if/when necessary
964973
UBX_RXM_RAWX_t *packetUBXRXMRAWX = nullptr; // Pointer to struct. RAM will be allocated for this if/when necessary
974+
UBX_RXM_MEASX_t *packetUBXRXMMEASX = nullptr; // Pointer to struct. RAM will be allocated for this if/when necessary
965975
#endif
966976

967977
UBX_TIM_TM2_t *packetUBXTIMTM2 = nullptr; // Pointer to struct. RAM will be allocated for this if/when necessary
@@ -1044,6 +1054,7 @@ class DevUBLOXGNSS
10441054
bool initPacketUBXRXMCOR(); // Allocate RAM for packetUBXRXMCOR and initialize it
10451055
bool initPacketUBXRXMSFRBX(); // Allocate RAM for packetUBXRXMSFRBX and initialize it
10461056
bool initPacketUBXRXMRAWX(); // Allocate RAM for packetUBXRXMRAWX and initialize it
1057+
bool initPacketUBXRXMMEASX(); // Allocate RAM for packetUBXRXMMEASX and initialize it
10471058
bool initPacketUBXTIMTM2(); // Allocate RAM for packetUBXTIMTM2 and initialize it
10481059
bool initPacketUBXESFALG(); // Allocate RAM for packetUBXESFALG and initialize it
10491060
bool initPacketUBXESFSTATUS(); // Allocate RAM for packetUBXESFSTATUS and initialize it

0 commit comments

Comments
 (0)