Skip to content

Commit 7014e5b

Browse files
mazgchmazgch
authored
store multiple RXM-QZSSL6 messages
1 parent 11b9cf4 commit 7014e5b

File tree

3 files changed

+45
-126
lines changed

3 files changed

+45
-126
lines changed

‎src/SparkFun_u-blox_GNSS_Arduino_Library.cpp‎

Lines changed: 30 additions & 117 deletions
Original file line numberDiff line numberDiff line change
@@ -321,21 +321,11 @@ void SFE_UBLOX_GNSS::end(void)
321321
packetUBXRXMPMPmessage = NULL; // Redundant?
322322
}
323323

324-
if (packetUBXRXMQZSSL6 != NULL)
325-
{
326-
if (packetUBXRXMQZSSL6->callbackData != NULL)
327-
{
328-
delete packetUBXRXMQZSSL6->callbackData;
329-
}
330-
delete packetUBXRXMQZSSL6;
331-
packetUBXRXMQZSSL6 = NULL; // Redundant?
332-
}
333-
334324
if (packetUBXRXMQZSSL6message != NULL)
335325
{
336326
if (packetUBXRXMQZSSL6message->callbackData != NULL)
337327
{
338-
delete packetUBXRXMQZSSL6message->callbackData;
328+
delete [] packetUBXRXMQZSSL6message->callbackData;
339329
}
340330
delete packetUBXRXMQZSSL6message;
341331
packetUBXRXMQZSSL6message = NULL; // Redundant?
@@ -1391,7 +1381,7 @@ bool SFE_UBLOX_GNSS::checkAutomatic(uint8_t Class, uint8_t ID)
13911381
result = true;
13921382
break;
13931383
case UBX_RXM_QZSSL6:
1394-
if ((packetUBXRXMQZSSL6 != NULL) || (packetUBXRXMQZSSL6message != NULL))
1384+
if (packetUBXRXMQZSSL6message != NULL)
13951385
result = true;
13961386
break;
13971387
case UBX_RXM_COR:
@@ -3861,50 +3851,26 @@ void SFE_UBLOX_GNSS::processUBXpacket(ubxPacket *msg)
38613851
// Note: length is variable with version 0x01
38623852
// Note: the field positions depend on the version
38633853
{
3864-
// Parse various byte fields into storage - but only if we have memory allocated for it.
3865-
// By default, new QZSSL6 data will always overwrite 'old' data (data which is valid but which has not yet been read by the callback).
3866-
// To prevent this, uncomment the line two lines below
3867-
if ((packetUBXRXMQZSSL6 != NULL) && (packetUBXRXMQZSSL6->callbackData != NULL)
3868-
//&& (packetUBXRXMQZSSL6->automaticFlags.flags.bits.callbackCopyValid == false) // <=== Uncomment this line to prevent new data from overwriting 'old'
3869-
)
3870-
{
3871-
packetUBXRXMQZSSL6->callbackData->version = extractByte(msg, 0);
3872-
packetUBXRXMQZSSL6->callbackData->svId = extractByte(msg, 1);
3873-
packetUBXRXMQZSSL6->callbackData->cno = extractInt(msg, 2);
3874-
packetUBXRXMQZSSL6->callbackData->timeTag = extractLong(msg, 4);
3875-
packetUBXRXMQZSSL6->callbackData->groupDelay = extractByte(msg, 8);
3876-
packetUBXRXMQZSSL6->callbackData->bitErrCorr = extractByte(msg, 9);
3877-
packetUBXRXMQZSSL6->callbackData->chInfo = extractInt(msg, 10);
3878-
packetUBXRXMQZSSL6->callbackData->reserved0[0] = extractByte(msg, 12);
3879-
packetUBXRXMQZSSL6->callbackData->reserved0[0] = extractByte(msg, 13);
3880-
for (uint16_t i = 0; (i < UBX_RXM_QZSSL6_DATALEN); i++)
3881-
{
3882-
packetUBXRXMQZSSL6->callbackData->msgBytes[i] = extractByte(msg, i + 14);
3883-
}
3884-
packetUBXRXMQZSSL6->automaticFlags.flags.bits.callbackCopyValid = true; // Mark the data as valid
3885-
}
3854+
// Full QZSSL6 message, including Class, ID and checksum
3855+
for (int ch = 0; ch < UBX_RXM_QZSSL6_NUM_CHANNELS; ch ++) {
3856+
if (0 == (packetUBXRXMQZSSL6message->automaticFlags.flags.bits.callbackCopyValid & (1<<ch))) {
3857+
3858+
packetUBXRXMQZSSL6message->callbackData[ch].sync1 = UBX_SYNCH_1;
3859+
packetUBXRXMQZSSL6message->callbackData[ch].sync2 = UBX_SYNCH_2;
3860+
packetUBXRXMQZSSL6message->callbackData[ch].cls = UBX_CLASS_RXM;
3861+
packetUBXRXMQZSSL6message->callbackData[ch].ID = UBX_RXM_QZSSL6;
3862+
packetUBXRXMQZSSL6message->callbackData[ch].lengthLSB = msg->len & 0xFF;
3863+
packetUBXRXMQZSSL6message->callbackData[ch].lengthMSB = msg->len >> 8;
38863864

3887-
// Full QZSSL6 message, including Class, ID and checksum
3888-
// By default, new QZSSL6 data will always overwrite 'old' data (data which is valid but which has not yet been read by the callback).
3889-
// To prevent this, uncomment the line two lines below
3890-
if ((packetUBXRXMQZSSL6message != NULL) && (packetUBXRXMQZSSL6message->callbackData != NULL)
3891-
//&& (packetUBXRXMQZSSL6message->automaticFlags.flags.bits.callbackCopyValid == false) // <=== Uncomment this line to prevent new data from overwriting 'old'
3892-
)
3893-
{
3894-
packetUBXRXMQZSSL6message->callbackData->sync1 = UBX_SYNCH_1;
3895-
packetUBXRXMQZSSL6message->callbackData->sync2 = UBX_SYNCH_2;
3896-
packetUBXRXMQZSSL6message->callbackData->cls = UBX_CLASS_RXM;
3897-
packetUBXRXMQZSSL6message->callbackData->ID = UBX_RXM_QZSSL6;
3898-
packetUBXRXMQZSSL6message->callbackData->lengthLSB = msg->len & 0xFF;
3899-
packetUBXRXMQZSSL6message->callbackData->lengthMSB = msg->len >> 8;
3900-
3901-
memcpy(packetUBXRXMQZSSL6message->callbackData->payload, msg->payload, msg->len);
3865+
memcpy(packetUBXRXMQZSSL6message->callbackData[ch].payload, msg->payload, msg->len);
39023866

3903-
packetUBXRXMQZSSL6message->callbackData->checksumA = msg->checksumA;
3904-
packetUBXRXMQZSSL6message->callbackData->checksumB = msg->checksumB;
3867+
packetUBXRXMQZSSL6message->callbackData[ch].checksumA = msg->checksumA;
3868+
packetUBXRXMQZSSL6message->callbackData[ch].checksumB = msg->checksumB;
39053869

3906-
packetUBXRXMQZSSL6message->automaticFlags.flags.bits.callbackCopyValid = true; // Mark the data as valid
3907-
}
3870+
packetUBXRXMQZSSL6message->automaticFlags.flags.bits.callbackCopyValid |= (1 << ch);
3871+
break; // abort when added
3872+
}
3873+
}
39083874
}
39093875
else if (msg->id == UBX_RXM_COR)
39103876
{
@@ -5521,26 +5487,17 @@ void SFE_UBLOX_GNSS::checkCallbacks(void)
55215487
packetUBXRXMPMPmessage->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
55225488
}
55235489

5524-
if ((packetUBXRXMQZSSL6 != NULL) // If RAM has been allocated for message storage
5525-
&& (packetUBXRXMQZSSL6->callbackData != NULL) // If RAM has been allocated for the copy of the data
5526-
&& (packetUBXRXMQZSSL6->callbackPointerPtr != NULL) // If the pointer to the callback has been defined
5527-
&& (packetUBXRXMQZSSL6->automaticFlags.flags.bits.callbackCopyValid == true)) // If the copy of the data is valid
5490+
if ((packetUBXRXMQZSSL6message != NULL) && // If RAM has been allocated for message storage
5491+
(packetUBXRXMQZSSL6message->callbackData != NULL) && // If RAM has been allocated for the copy of the data
5492+
(packetUBXRXMQZSSL6message->callbackPointerPtr != NULL)) // If the pointer to the callback has been defined
55285493
{
5529-
// if (_printDebug == true)
5530-
// _debugSerial->println(F("checkCallbacks: calling callbackPtr for RXM QZSSL6"));
5531-
packetUBXRXMQZSSL6->callbackPointerPtr(packetUBXRXMQZSSL6->callbackData); // Call the callback
5532-
packetUBXRXMQZSSL6->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
5533-
}
5534-
5535-
if ((packetUBXRXMQZSSL6message != NULL) // If RAM has been allocated for message storage
5536-
&& (packetUBXRXMQZSSL6message->callbackData != NULL) // If RAM has been allocated for the copy of the data
5537-
&& (packetUBXRXMQZSSL6message->callbackPointerPtr != NULL) // If the pointer to the callback has been defined
5538-
&& (packetUBXRXMQZSSL6message->automaticFlags.flags.bits.callbackCopyValid == true)) // If the copy of the data is valid
5539-
{
5540-
// if (_printDebug == true)
5541-
// _debugSerial->println(F("checkCallbacks: calling callbackPtr for RXM QZSSL6 message"));
5542-
packetUBXRXMQZSSL6message->callbackPointerPtr(packetUBXRXMQZSSL6message->callbackData); // Call the callback
5543-
packetUBXRXMQZSSL6message->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
5494+
for (int ch = 0; ch < UBX_RXM_QZSSL6_NUM_CHANNELS; ch ++) {
5495+
if (packetUBXRXMQZSSL6message->automaticFlags.flags.bits.callbackCopyValid & (1 << ch)) // If the copy of the data is valid
5496+
{
5497+
packetUBXRXMQZSSL6message->callbackPointerPtr( &packetUBXRXMQZSSL6message->callbackData[ch] ); // Call the callback
5498+
}
5499+
}
5500+
packetUBXRXMQZSSL6message->automaticFlags.flags.bits.callbackCopyValid = 0; // Mark the data as stale
55445501
}
55455502

55465503
if ((packetUBXRXMCOR != NULL) // If RAM has been allocated for message storage
@@ -12661,50 +12618,6 @@ bool SFE_UBLOX_GNSS::initPacketUBXRXMPMPmessage()
1266112618

1266212619
// ***** RXM QZSSL6 automatic support
1266312620

12664-
// Callback receives a pointer to the data, instead of _all_ the data. Much kinder on the stack!
12665-
bool SFE_UBLOX_GNSS::setRXMQZSSL6callbackPtr(void (*callbackPointer)(UBX_RXM_QZSSL6_data_t *))
12666-
{
12667-
if (packetUBXRXMQZSSL6 == NULL)
12668-
initPacketUBXRXMQZSSL6(); // Check that RAM has been allocated for the data
12669-
if (packetUBXRXMQZSSL6 == NULL) // Only attempt this if RAM allocation was successful
12670-
return false;
12671-
12672-
if (packetUBXRXMQZSSL6->callbackData == NULL) // Check if RAM has been allocated for the callback copy
12673-
{
12674-
packetUBXRXMQZSSL6->callbackData = new UBX_RXM_QZSSL6_data_t; // Allocate RAM for the main struct
12675-
}
12676-
12677-
if (packetUBXRXMQZSSL6->callbackData == NULL)
12678-
{
12679-
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
12680-
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
12681-
_debugSerial->println(F("setAutoRXMQZSSL6callbackPtr: RAM alloc failed!"));
12682-
#endif
12683-
return (false);
12684-
}
12685-
12686-
packetUBXRXMQZSSL6->callbackPointerPtr = callbackPointer;
12687-
return (true);
12688-
}
12689-
12690-
// PRIVATE: Allocate RAM for packetUBXRXMQZSSL6 and initialize it
12691-
bool SFE_UBLOX_GNSS::initPacketUBXRXMQZSSL6()
12692-
{
12693-
packetUBXRXMQZSSL6 = new UBX_RXM_QZSSL6_t; // Allocate RAM for the main struct
12694-
if (packetUBXRXMQZSSL6 == NULL)
12695-
{
12696-
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
12697-
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
12698-
_debugSerial->println(F("initPacketUBXRXMQZSSL6: RAM alloc failed!"));
12699-
#endif
12700-
return (false);
12701-
}
12702-
packetUBXRXMQZSSL6->automaticFlags.flags.all = 0;
12703-
packetUBXRXMQZSSL6->callbackPointerPtr = NULL;
12704-
packetUBXRXMQZSSL6->callbackData = NULL;
12705-
return (true);
12706-
}
12707-
1270812621
// Callback receives a pointer to the data, instead of _all_ the data. Much kinder on the stack!
1270912622
bool SFE_UBLOX_GNSS::setRXMQZSSL6messageCallbackPtr(void (*callbackPointer)(UBX_RXM_QZSSL6_message_data_t *))
1271012623
{
@@ -12715,7 +12628,7 @@ bool SFE_UBLOX_GNSS::setRXMQZSSL6messageCallbackPtr(void (*callbackPointer)(UBX_
1271512628

1271612629
if (packetUBXRXMQZSSL6message->callbackData == NULL) // Check if RAM has been allocated for the callback copy
1271712630
{
12718-
packetUBXRXMQZSSL6message->callbackData = new UBX_RXM_QZSSL6_message_data_t; // Allocate RAM for the main struct
12631+
packetUBXRXMQZSSL6message->callbackData = new UBX_RXM_QZSSL6_message_data_t[UBX_RXM_QZSSL6_NUM_CHANNELS]; // Allocate RAM for the main struct
1271912632
}
1272012633

1272112634
if (packetUBXRXMQZSSL6message->callbackData == NULL)

‎src/SparkFun_u-blox_GNSS_Arduino_Library.h‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1168,7 +1168,6 @@ class SFE_UBLOX_GNSS
11681168
// Note: on the NEO-D9C, the UBX-RXM-QZSSL6 messages are enabled by default on all ports.
11691169
// You can disable them by calling (e.g.) setVal8(UBLOX_CFG_MSGOUT_UBX_RXM_QZSSL6_I2C, 0)
11701170
// The NEO-D9C does not support UBX-CFG-MSG
1171-
bool setRXMQZSSL6callbackPtr(void (*callbackPointerPtr)(UBX_RXM_QZSSL6_data_t *)); // Callback receives a pointer to the data, instead of _all_ the data. Much kinder on the stack!
11721171
bool setRXMQZSSL6messageCallbackPtr(void (*callbackPointerPtr)(UBX_RXM_QZSSL6_message_data_t *)); // Use this if you want all of the QZSSL6 message (including sync chars, checksum, etc.) to push to a GNSS
11731172

11741173
bool setRXMCORcallbackPtr(void (*callbackPointerPtr)(UBX_RXM_COR_data_t *)); // RXM COR
@@ -1526,7 +1525,6 @@ class SFE_UBLOX_GNSS
15261525

15271526
UBX_RXM_PMP_t *packetUBXRXMPMP = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
15281527
UBX_RXM_PMP_message_t *packetUBXRXMPMPmessage = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
1529-
UBX_RXM_QZSSL6_t *packetUBXRXMQZSSL6 = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
15301528
UBX_RXM_QZSSL6_message_t *packetUBXRXMQZSSL6message = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
15311529
UBX_RXM_COR_t *packetUBXRXMCOR = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
15321530
UBX_RXM_SFRBX_t *packetUBXRXMSFRBX = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary

‎src/u-blox_structs.h‎

Lines changed: 15 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1600,7 +1600,8 @@ typedef struct
16001600
} UBX_RXM_PMP_message_t;
16011601

16021602
// UBX-RXM-QZSSL6 (0x02 0x73): QZSS L6 raw data (D9C modules)
1603-
#define UBX_RXM_QZSSL6_DATALEN 250
1603+
#define UBX_RXM_QZSSL6_NUM_CHANNELS 2
1604+
const uint16_t UBX_RXM_QZSSL6_DATALEN = 250;
16041605
const uint16_t UBX_RXM_QZSSL6_MAX_LEN = UBX_RXM_QZSSL6_DATALEN + 14;
16051606

16061607
typedef struct
@@ -1616,13 +1617,20 @@ typedef struct
16161617
uint8_t msgBytes[UBX_RXM_QZSSL6_DATALEN]; // Bytes in a QZSS L6 message
16171618
} UBX_RXM_QZSSL6_data_t;
16181619

1619-
// The QZSSL6 data can only be accessed via a callback. QZSSL6 cannot be polled.
1620-
typedef struct
1620+
struct ubxQZSSL6AutomaticFlags
16211621
{
1622-
ubxAutomaticFlags automaticFlags;
1623-
void (*callbackPointerPtr)(UBX_RXM_QZSSL6_data_t *);
1624-
UBX_RXM_QZSSL6_data_t *callbackData;
1625-
} UBX_RXM_QZSSL6_t;
1622+
union
1623+
{
1624+
uint8_t all;
1625+
struct
1626+
{
1627+
uint8_t automatic : 1; // Will this message be delivered and parsed "automatically" (without polling)
1628+
uint8_t implicitUpdate : 1; // Is the update triggered by accessing stale data (=true) or by a call to checkUblox (=false)
1629+
uint8_t addToFileBuffer : 1; // Should the raw UBX data be added to the file buffer?
1630+
uint8_t callbackCopyValid : UBX_RXM_QZSSL6_NUM_CHANNELS; // Is the copies of the data structs used by the callback valid/fresh?
1631+
} bits;
1632+
} flags;
1633+
};
16261634

16271635
// Define a struct to hold the entire QZSSL6 message so the whole thing can be pushed to a GNSS.
16281636
// Remember that the length of the payload could be variable (with version 1 messages).

0 commit comments

Comments
 (0)