PlatformIO-Libs/SparkFunGNSS
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update Example19. Add setRXMPMPmessageCallbackPtr. Add const char versions of setDynamicSPARTNKey + setDynamicSPARTNKeys

Browse Source
This commit is contained in:
PaulZC
2022-02-14 12:09:13 +00:00
parent 213d29cafe
commit 1aa043cef5
7 changed files with 388 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/ZED-F9P/Example17_NTRIPClient_With_GGA_Callback/Example17_NTRIPClient_With_GGA_Callback.ino
+7 -13
View File
@@ -93,22 +93,17 @@ void pushGPGGA(NMEA_GGA_data_t *nmeaData)
// | | |
void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
{
long latitude = ubxDataStruct->lat; // Print the latitude
double latitude = ubxDataStruct->lat; // Print the latitude
Serial.print(F("Lat: "));
Serial.print(latitude / 10000000L);
Serial.print(F("."));
Serial.print(abs(latitude % 10000000L));
Serial.print(latitude / 10000000.0, 7);
long longitude = ubxDataStruct->lon; // Print the longitude
double longitude = ubxDataStruct->lon; // Print the longitude
Serial.print(F(" Long: "));
Serial.print(longitude / 10000000L);
Serial.print(F("."));
Serial.print(abs(longitude % 10000000L));
Serial.print(longitude / 10000000.0, 7);
long altitude = ubxDataStruct->hMSL; // Print the height above mean sea level
double altitude = ubxDataStruct->hMSL; // Print the height above mean sea level
Serial.print(F(" Height: "));
Serial.print(altitude);
Serial.print(F(" (mm)"));
Serial.print(altitude / 1000.0, 3);
uint8_t fixType = ubxDataStruct->fixType; // Print the fix type
Serial.print(F(" Fix: "));
@@ -159,9 +154,8 @@ void setup()
while (myGNSS.begin() == false) //Connect to the Ublox module using Wire port
{
Serial.println(F("u-blox GPS not detected at default I2C address. Please check wiring. Freezing."));
Serial.println(F("u-blox GPS not detected at default I2C address. Please check wiring."));
delay(2000);
//while (1);
}
Serial.println(F("u-blox module connected"));
examples/ZED-F9P/Example19_LBand_Corrections_with_NEO-D9S/Example19_LBand_Corrections_with_NEO-D9S.ino
+21 -27
View File
@@ -38,25 +38,24 @@ const uint32_t myLBandFreq = 1556290000; // Uncomment this line to use the US SP
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// Callback: pushRXMPMP will be called when new PMP data arrives
// See u-blox_structs.h for the full definition of UBX_RXM_PMP_data_t
// _____ You can use any name you like for the callback. Use the same name when you call setAutoRXMPMPcallbackPtr
// / _____ This _must_ be UBX_RXM_PMP_data_t
// See u-blox_structs.h for the full definition of UBX_RXM_PMP_message_data_t
// _____ You can use any name you like for the callback. Use the same name when you call setRXMPMPmessageCallbackPtr
// / _____ This _must_ be UBX_RXM_PMP_message_data_t
// | / _____ You can use any name you like for the struct
// | | /
// | | |
void pushRXMPMP(UBX_RXM_PMP_data_t *pmpData)
void pushRXMPMP(UBX_RXM_PMP_message_data_t *pmpData)
{
//Push the PMP data to the GNSS
Serial.print(F("New RXM-PMP data received. Message version is 0x0"));
Serial.print(pmpData->version, HEX);
Serial.print(F(". numBytesUserData is "));
size_t numDataBytes = 504; // Version 0x00 messages always contain 504 bytes of userData
if (pmpData->version == 0x01)
numDataBytes = pmpData->numBytesUserData;
Serial.print(numDataBytes);
Serial.println(F(". Pushing them to the GNSS..."));
//Extract the raw message payload length
uint16_t payloadLen = ((uint16_t)pmpData->lengthMSB << 8) | (uint16_t)pmpData->lengthLSB;
Serial.print(F("New RXM-PMP data received. Message payload length is "));
Serial.print(payloadLen);
Serial.println(F(" Bytes. Pushing it to the GNSS..."));
myGNSS.pushRawData(pmpData->userData, numDataBytes);
//Push the PMP data to the GNSS
//The payload length could be variable, so we need to push the header and payload, then checksum
myGNSS.pushRawData(&pmpData->sync1, (size_t)payloadLen + 6); // Push the sync chars, class, ID, length and payload
myGNSS.pushRawData(&pmpData->checksumA, (size_t)2); // Push the checksum bytes
}
//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
@@ -70,22 +69,17 @@ void pushRXMPMP(UBX_RXM_PMP_data_t *pmpData)
// | | |
void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
{
long latitude = ubxDataStruct->lat; // Print the latitude
double latitude = ubxDataStruct->lat; // Print the latitude
Serial.print(F("Lat: "));
Serial.print(latitude / 10000000L);
Serial.print(F("."));
Serial.print(abs(latitude % 10000000L));
Serial.print(latitude / 10000000.0, 7);
long longitude = ubxDataStruct->lon; // Print the longitude
double longitude = ubxDataStruct->lon; // Print the longitude
Serial.print(F(" Long: "));
Serial.print(longitude / 10000000L);
Serial.print(F("."));
Serial.print(abs(longitude % 10000000L));
Serial.print(longitude / 10000000.0, 7);
long altitude = ubxDataStruct->hMSL; // Print the height above mean sea level
double altitude = ubxDataStruct->hMSL; // Print the height above mean sea level
Serial.print(F(" Height: "));
Serial.print(altitude);
Serial.print(F(" (mm)"));
Serial.print(altitude / 1000.0, 3);
uint8_t fixType = ubxDataStruct->fixType; // Print the fix type
Serial.print(F(" Fix: "));
@@ -157,7 +151,7 @@ void setup()
//"When the receiver boots, the host should send 'current' and 'next' keys in one message." - Use setDynamicSPARTNKeys for this.
//"Every time the 'current' key is expired, 'next' takes its place."
//"Therefore the host should then retrieve the new 'next' key and send only that." - Use setDynamicSPARTNKey for this.
// The key can be provided in binary format or in ASCII Hex format, but in both cases keyLengthBytes _must_ represent the binary key length in bytes.
// The key can be provided in binary (uint8_t) format or in ASCII Hex (char) format, but in both cases keyLengthBytes _must_ represent the binary key length in bytes.
if (ok) ok = myGNSS.setDynamicSPARTNKeys(currentKeyLengthBytes, currentKeyGPSWeek, currentKeyGPSToW, currentDynamicKey,
nextKeyLengthBytes, nextKeyGPSWeek, nextKeyGPSToW, nextDynamicKey);
@@ -200,7 +194,7 @@ void setup()
myLBand.softwareResetGNSSOnly(); // Do a restart
myLBand.setAutoRXMPMPcallbackPtr(&pushRXMPMP); // Call pushRXMPMP when new PMP data arrives. Push it to the GNSS
myLBand.setRXMPMPmessageCallbackPtr(&pushRXMPMP); // Call pushRXMPMP when new PMP data arrives. Push it to the GNSS
}
examples/ZED-F9P/Example19_LBand_Corrections_with_NEO-D9S/secrets.h
+2 -2
View File
@@ -14,11 +14,11 @@
// The keys are given as: 32 hexadecimal digits = 128 bits = 16 Bytes
const uint8_t currentKeyLengthBytes = 16;
const uint8_t currentDynamicKey[] = "f742bd6b7248043177dd649141d8fb0b";
const char currentDynamicKey[] = "f742bd6b7248043177dd649141d8fb0b";
const uint16_t currentKeyGPSWeek = 2192;
const uint32_t currentKeyGPSToW = 518418;
const uint8_t nextKeyLengthBytes = 16;
const uint8_t nextDynamicKey[] = "8206........................29f4";
const char nextDynamicKey[] = "8206........................29f4";
const uint16_t nextKeyGPSWeek = 2196;
const uint32_t nextKeyGPSToW = 518418;
keywords.txt
+9 -1
View File
@@ -26,10 +26,15 @@ UBX_NAV_VELECEF_data_t KEYWORD1
UBX_NAV_VELNED_data_t KEYWORD1
UBX_NAV_HPPOSECEF_data_t KEYWORD1
UBX_NAV_HPPOSLLH_data_t KEYWORD1
UBX_NAV_PVAT_data_t KEYWORD1
UBX_NAV_CLOCK_data_t KEYWORD1
UBX_NAV_SAT_data_t KEYWORD1
UBX_NAV_RELPOSNED_data_t KEYWORD1
UBX_NAV_TIMELS_data_t KEYWORD1
UBX_NAV_AOPSTATUS_data_t KEYWORD1
UBX_RXM_PMP_data_t KEYWORD1
UBX_RXM_PMP_message_data_t KEYWORD1
UBX_RXM_SFRBX_data_t KEYWORD1
UBX_RXM_RAWX_data_t KEYWORD1
@@ -45,6 +50,8 @@ UBX_HNR_PVT_data_t KEYWORD1
UBX_HNR_ATT_data_t KEYWORD1
UBX_HNR_INS_data_t KEYWORD1
NMEA_GGA_data_t KEYWORD1
#######################################
# Methods and Functions (KEYWORD2)
#######################################
@@ -364,7 +371,8 @@ initPacketUBXAOPSTATUS KEYWORD2
flushAOPSTATUS KEYWORD2
logAOPSTATUS KEYWORD2
setAutoRXMPMPcallbackPtr KEYWORD2
setRXMPMPcallbackPtr KEYWORD2
setRXMPMPmessageCallbackPtr KEYWORD2
getRXMSFRBX KEYWORD2
setAutoRXMSFRBX KEYWORD2
src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+279 -201
View File
@@ -284,6 +284,16 @@ void SFE_UBLOX_GNSS::end(void)
packetUBXRXMPMP = NULL; // Redundant?
}
if (packetUBXRXMPMPmessage != NULL)
{
if (packetUBXRXMPMPmessage->callbackData != NULL)
{
delete packetUBXRXMPMPmessage->callbackData;
}
delete packetUBXRXMPMPmessage;
packetUBXRXMPMPmessage = NULL; // Redundant?
}
if (packetUBXRXMSFRBX != NULL)
{
if (packetUBXRXMSFRBX->callbackData != NULL)
@@ -1258,7 +1268,7 @@ bool SFE_UBLOX_GNSS::checkAutomatic(uint8_t Class, uint8_t ID)
result = true;
break;
case UBX_RXM_PMP:
if (packetUBXRXMPMP != NULL)
if ((packetUBXRXMPMP != NULL) || (packetUBXRXMPMPmessage != NULL))
result = true;
break;
}
@@ -3254,45 +3264,52 @@ void SFE_UBLOX_GNSS::processUBXpacket(ubxPacket *msg)
// Note: the field positions depend on the version
{
// Parse various byte fields into storage - but only if we have memory allocated for it
if (packetUBXRXMPMP != NULL)
if ((packetUBXRXMPMP != NULL) && (packetUBXRXMPMP->callbackData != NULL))
{
packetUBXRXMPMP->data.version = extractByte(msg, 0);
packetUBXRXMPMP->data.numBytesUserData = extractInt(msg, 2);
packetUBXRXMPMP->data.timeTag = extractLong(msg, 4);
packetUBXRXMPMP->data.uniqueWord[0] = extractLong(msg, 8);
packetUBXRXMPMP->data.uniqueWord[1] = extractLong(msg, 12);
packetUBXRXMPMP->data.serviceIdentifier = extractInt(msg, 16);
packetUBXRXMPMP->data.spare = extractByte(msg, 18);
packetUBXRXMPMP->data.uniqueWordBitErrors = extractByte(msg, 19);
packetUBXRXMPMP->callbackData->version = extractByte(msg, 0);
packetUBXRXMPMP->callbackData->numBytesUserData = extractInt(msg, 2);
packetUBXRXMPMP->callbackData->timeTag = extractLong(msg, 4);
packetUBXRXMPMP->callbackData->uniqueWord[0] = extractLong(msg, 8);
packetUBXRXMPMP->callbackData->uniqueWord[1] = extractLong(msg, 12);
packetUBXRXMPMP->callbackData->serviceIdentifier = extractInt(msg, 16);
packetUBXRXMPMP->callbackData->spare = extractByte(msg, 18);
packetUBXRXMPMP->callbackData->uniqueWordBitErrors = extractByte(msg, 19);
if (packetUBXRXMPMP->data.version == 0x00)
if (packetUBXRXMPMP->callbackData->version == 0x00)
{
packetUBXRXMPMP->data.fecBits = extractInt(msg, 524);
packetUBXRXMPMP->data.ebno = extractByte(msg, 526);
packetUBXRXMPMP->callbackData->fecBits = extractInt(msg, 524);
packetUBXRXMPMP->callbackData->ebno = extractByte(msg, 526);
}
else // if (packetUBXRXMPMP->data.version == 0x01)
{
packetUBXRXMPMP->data.fecBits = extractInt(msg, 20);
packetUBXRXMPMP->data.ebno = extractByte(msg, 22);
packetUBXRXMPMP->callbackData->fecBits = extractInt(msg, 20);
packetUBXRXMPMP->callbackData->ebno = extractByte(msg, 22);
}
uint16_t userDataStart = (packetUBXRXMPMP->data.version == 0x00) ? 20 : 24;
uint16_t userDataLength = (packetUBXRXMPMP->data.version == 0x00) ? 504 : (packetUBXRXMPMP->data.numBytesUserData);
uint16_t userDataStart = (packetUBXRXMPMP->callbackData->version == 0x00) ? 20 : 24;
uint16_t userDataLength = (packetUBXRXMPMP->callbackData->version == 0x00) ? 504 : (packetUBXRXMPMP->callbackData->numBytesUserData);
for (uint16_t i = 0; (i < userDataLength) && (i < 504); i++)
{
packetUBXRXMPMP->data.userData[i] = extractByte(msg, i + userDataStart);
packetUBXRXMPMP->callbackData->userData[i] = extractByte(msg, i + userDataStart);
}
// Mark all datums as fresh (not read before)
packetUBXRXMPMP->moduleQueried = true;
packetUBXRXMPMP->automaticFlags.flags.bits.callbackCopyValid = true; // Mark the data as valid
}
// Check if we need to copy the data for the callbacks
if ((packetUBXRXMPMP->callbackData != NULL) // If RAM has been allocated for the copy of the data
&& (packetUBXRXMPMP->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
// Full PMP message
if ((packetUBXRXMPMPmessage != NULL) && (packetUBXRXMPMPmessage->callbackData != NULL))
{
memcpy(&packetUBXRXMPMP->callbackData->version, &packetUBXRXMPMP->data.version, sizeof(UBX_RXM_PMP_data_t));
packetUBXRXMPMP->automaticFlags.flags.bits.callbackCopyValid = true;
}
packetUBXRXMPMPmessage->callbackData->sync1 = UBX_SYNCH_1;
packetUBXRXMPMPmessage->callbackData->sync2 = UBX_SYNCH_2;
packetUBXRXMPMPmessage->callbackData->cls = UBX_CLASS_RXM;
packetUBXRXMPMPmessage->callbackData->ID = UBX_RXM_PMP;
packetUBXRXMPMPmessage->callbackData->lengthLSB = msg->len & 0xFF;
packetUBXRXMPMPmessage->callbackData->lengthMSB = msg->len >> 8;
memcpy(packetUBXRXMPMPmessage->callbackData->payload, msg->payload, msg->len);
packetUBXRXMPMPmessage->callbackData->checksumA = msg->checksumA;
packetUBXRXMPMPmessage->callbackData->checksumB = msg->checksumB;
}
}
else if (msg->id == UBX_RXM_SFRBX)
@@ -4862,17 +4879,26 @@ void SFE_UBLOX_GNSS::checkCallbacks(void)
if ((packetUBXRXMPMP != NULL) // If RAM has been allocated for message storage
&& (packetUBXRXMPMP->callbackData != NULL) // If RAM has been allocated for the copy of the data
&& (packetUBXRXMPMP->callbackPointerPtr != NULL) // If the pointer to the callback has been defined
&& (packetUBXRXMPMP->automaticFlags.flags.bits.callbackCopyValid == true)) // If the copy of the data is valid
{
if (packetUBXRXMPMP->callbackPointerPtr != NULL) // If the pointer to the callback has been defined
{
// if (_printDebug == true)
// _debugSerial->println(F("checkCallbacks: calling callbackPtr for RXM PMP"));
packetUBXRXMPMP->callbackPointerPtr(packetUBXRXMPMP->callbackData); // Call the callback
}
packetUBXRXMPMP->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
}
if ((packetUBXRXMPMPmessage != NULL) // If RAM has been allocated for message storage
&& (packetUBXRXMPMPmessage->callbackData != NULL) // If RAM has been allocated for the copy of the data
&& (packetUBXRXMPMPmessage->callbackPointerPtr != NULL) // If the pointer to the callback has been defined
&& (packetUBXRXMPMPmessage->automaticFlags.flags.bits.callbackCopyValid == true)) // If the copy of the data is valid
{
// if (_printDebug == true)
// _debugSerial->println(F("checkCallbacks: calling callbackPtr for RXM PMP message"));
packetUBXRXMPMPmessage->callbackPointerPtr(packetUBXRXMPMPmessage->callbackData); // Call the callback
packetUBXRXMPMPmessage->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
}
if ((packetUBXRXMSFRBX != NULL) // If RAM has been allocated for message storage
&& (packetUBXRXMSFRBX->callbackData != NULL) // If RAM has been allocated for the copy of the data
&& (packetUBXRXMSFRBX->automaticFlags.flags.bits.callbackCopyValid == true)) // If the copy of the data is valid
@@ -7582,8 +7608,69 @@ bool SFE_UBLOX_GNSS::setAopCfg(uint8_t aopCfg, uint16_t aopOrbMaxErr, uint16_t m
//"When the receiver boots, the host should send 'current' and 'next' keys in one message." - Use setDynamicSPARTNKeys for this.
//"Every time the 'current' key is expired, 'next' takes its place."
//"Therefore the host should then retrieve the new 'next' key and send only that." - Use setDynamicSPARTNKey for this.
// The key can be provided in binary format or in ASCII Hex format, but in both cases keyLengthBytes _must_ represent the binary key length in bytes.
bool SFE_UBLOX_GNSS::setDynamicSPARTNKey(uint8_t keyLengthBytes, uint16_t validFromWno, uint32_t validFromTow, const uint8_t *key, uint16_t maxWait)
// The key can be provided in binary (uint8_t) format or in ASCII Hex (char) format, but in both cases keyLengthBytes _must_ represent the binary key length in bytes.
bool SFE_UBLOX_GNSS::setDynamicSPARTNKey(uint8_t keyLengthBytes, uint16_t validFromWno, uint32_t validFromTow, const char *key)
{
uint8_t *binaryKey = new uint8_t[keyLengthBytes]; // Allocate memory to store the binaryKey
if (binaryKey == NULL)
{
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
if (_printDebug == true)
_debugSerial->println(F("setDynamicSPARTNKey: binaryKey RAM allocation failed!"));
#endif
return (false);
}
bool ok = true;
// Convert the ASCII Hex const char to binary uint8_t
for (uint16_t i = 0; i < ((uint16_t)keyLengthBytes * 2); i += 2)
{
if ((key[i] >= '0') && (key[i] <= '9'))
{
binaryKey[i >> 1] = (key[i] - '0') << 4;
}
else if ((key[i] >= 'a') && (key[i] <= 'f'))
{
binaryKey[i >> 1] = (key[i] + 10 - 'a') << 4;
}
else if ((key[i] >= 'A') && (key[i] <= 'F'))
{
binaryKey[i >> 1] = (key[i] + 10 - 'A') << 4;
}
else
{
ok = false;
}
if ((key[i + 1] >= '0') && (key[i + 1] <= '9'))
{
binaryKey[i >> 1] |= key[i + 1] - '0';
}
else if ((key[i + 1] >= 'a') && (key[i + 1] <= 'f'))
{
binaryKey[i >> 1] |= key[i + 1] + 10 - 'a';
}
else if ((key[i + 1] >= 'A') && (key[i + 1] <= 'F'))
{
binaryKey[i >> 1] |= key[i + 1] + 10 - 'A';
}
else
{
ok = false;
}
}
if (ok)
ok = setDynamicSPARTNKey(keyLengthBytes, validFromWno, validFromTow, (const uint8_t *)binaryKey);
delete[] binaryKey; // Free the memory allocated for binaryKey
return (ok);
}
bool SFE_UBLOX_GNSS::setDynamicSPARTNKey(uint8_t keyLengthBytes, uint16_t validFromWno, uint32_t validFromTow, const uint8_t *key)
{
// Check if there is room for the key in packetCfg. Resize the buffer if not.
size_t payloadLength = (size_t)keyLengthBytes + 12;
@@ -7614,68 +7701,127 @@ bool SFE_UBLOX_GNSS::setDynamicSPARTNKey(uint8_t keyLengthBytes, uint16_t validF
payloadCfg[10] = (validFromTow >> 16) & 0xFF;
payloadCfg[11] = (validFromTow >> 24) & 0xFF;
// Check if all keyLengthBytes are ASCII Hex 0-9, a-f, A-F
bool isASCIIHex = true;
uint16_t i = 0;
while ((i < (uint16_t)keyLengthBytes) && (isASCIIHex == true))
{
if (((key[i] >= '0') && (key[i] <= '9')) || ((key[i] >= 'a') && (key[i] <= 'f')) || ((key[i] >= 'A') && (key[i] <= 'F')))
i++; // Keep checking if data is all ASCII Hex
else
isASCIIHex = false; // Data is binary
}
if (isASCIIHex) // Check the second half of the ASCII Hex key
{
while ((i < ((uint16_t)keyLengthBytes * 2) && (isASCIIHex == true)))
{
if (((key[i] >= '0') && (key[i] <= '9')) || ((key[i] >= 'a') && (key[i] <= 'f')) || ((key[i] >= 'A') && (key[i] <= 'F')))
i++; // Keep checking if data is all ASCII Hex
else
isASCIIHex = false; // Data is binary
}
}
if (isASCIIHex) // Convert ASCII Hex key to binary
{
for (i = 0; i < ((uint16_t)keyLengthBytes * 2); i += 2)
{
if ((key[i] >= '0') && (key[i] <= '9'))
{
payloadCfg[12 + (i >> 1)] = (key[i] - '0') << 4;
}
else if ((key[i] >= 'a') && (key[i] <= 'f'))
{
payloadCfg[12 + (i >> 1)] = (key[i] + 10 - 'a') << 4;
}
else // if ((key[i] >= 'A') && (key[i] <= 'F'))
{
payloadCfg[12 + (i >> 1)] = (key[i] + 10 - 'A') << 4;
}
if ((key[i + 1] >= '0') && (key[i + 1] <= '9'))
{
payloadCfg[12 + (i >> 1)] |= key[i + 1] - '0';
}
else if ((key[i + 1] >= 'a') && (key[i + 1] <= 'f'))
{
payloadCfg[12 + (i >> 1)] |= key[i + 1] + 10 - 'a';
}
else // if ((key[i + 1] >= 'A') && (key[i + 1] <= 'F'))
{
payloadCfg[12 + (i >> 1)] |= key[i + 1] + 10 - 'A';
}
}
}
else // Binary key
{
memcpy(&payloadCfg[12], key, keyLengthBytes);
return (sendCommand(&packetCfg, 0) == SFE_UBLOX_STATUS_SUCCESS); // UBX-RXM-SPARTNKEY is silent. It does not ACK (or NACK)
}
return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
bool SFE_UBLOX_GNSS::setDynamicSPARTNKeys(uint8_t keyLengthBytes1, uint16_t validFromWno1, uint32_t validFromTow1, const char *key1,
uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const char *key2)
{
uint8_t *binaryKey1 = new uint8_t[keyLengthBytes1]; // Allocate memory to store binaryKey1
if (binaryKey1 == NULL)
{
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
if (_printDebug == true)
_debugSerial->println(F("setDynamicSPARTNKeys: binaryKey1 RAM allocation failed!"));
#endif
return (false);
}
uint8_t *binaryKey2 = new uint8_t[keyLengthBytes2]; // Allocate memory to store binaryKey2
if (binaryKey2 == NULL)
{
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
if (_printDebug == true)
_debugSerial->println(F("setDynamicSPARTNKeys: binaryKey2 RAM allocation failed!"));
#endif
delete[] binaryKey1;
return (false);
}
bool ok = true;
// Convert the ASCII Hex const char to binary uint8_t
for (uint16_t i = 0; i < ((uint16_t)keyLengthBytes1 * 2); i += 2)
{
if ((key1[i] >= '0') && (key1[i] <= '9'))
{
binaryKey1[i >> 1] = (key1[i] - '0') << 4;
}
else if ((key1[i] >= 'a') && (key1[i] <= 'f'))
{
binaryKey1[i >> 1] = (key1[i] + 10 - 'a') << 4;
}
else if ((key1[i] >= 'A') && (key1[i] <= 'F'))
{
binaryKey1[i >> 1] = (key1[i] + 10 - 'A') << 4;
}
else
{
ok = false;
}
if ((key1[i + 1] >= '0') && (key1[i + 1] <= '9'))
{
binaryKey1[i >> 1] |= key1[i + 1] - '0';
}
else if ((key1[i + 1] >= 'a') && (key1[i + 1] <= 'f'))
{
binaryKey1[i >> 1] |= key1[i + 1] + 10 - 'a';
}
else if ((key1[i + 1] >= 'A') && (key1[i + 1] <= 'F'))
{
binaryKey1[i >> 1] |= key1[i + 1] + 10 - 'A';
}
else
{
ok = false;
}
}
// Convert the ASCII Hex const char to binary uint8_t
for (uint16_t i = 0; i < ((uint16_t)keyLengthBytes2 * 2); i += 2)
{
if ((key2[i] >= '0') && (key2[i] <= '9'))
{
binaryKey2[i >> 1] = (key2[i] - '0') << 4;
}
else if ((key2[i] >= 'a') && (key2[i] <= 'f'))
{
binaryKey2[i >> 1] = (key2[i] + 10 - 'a') << 4;
}
else if ((key2[i] >= 'A') && (key2[i] <= 'F'))
{
binaryKey2[i >> 1] = (key2[i] + 10 - 'A') << 4;
}
else
{
ok = false;
}
if ((key2[i + 1] >= '0') && (key2[i + 1] <= '9'))
{
binaryKey2[i >> 1] |= key2[i + 1] - '0';
}
else if ((key2[i + 1] >= 'a') && (key2[i + 1] <= 'f'))
{
binaryKey2[i >> 1] |= key2[i + 1] + 10 - 'a';
}
else if ((key2[i + 1] >= 'A') && (key2[i + 1] <= 'F'))
{
binaryKey2[i >> 1] |= key2[i + 1] + 10 - 'A';
}
else
{
ok = false;
}
}
if (ok)
ok = setDynamicSPARTNKeys(keyLengthBytes1, validFromWno1, validFromTow1, (const uint8_t *)binaryKey1,
keyLengthBytes2, validFromWno2, validFromTow2, (const uint8_t *)binaryKey2);
delete[] binaryKey1; // Free the memory allocated for binaryKey1
delete[] binaryKey2; // Free the memory allocated for binaryKey2
return (ok);
}
bool SFE_UBLOX_GNSS::setDynamicSPARTNKeys(uint8_t keyLengthBytes1, uint16_t validFromWno1, uint32_t validFromTow1, const uint8_t *key1,
uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const uint8_t *key2, uint16_t maxWait)
uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const uint8_t *key2)
{
// Check if there is room for the key in packetCfg. Resize the buffer if not.
size_t payloadLength = (size_t)keyLengthBytes1 + (size_t)keyLengthBytes2 + 20;
@@ -7714,121 +7860,10 @@ bool SFE_UBLOX_GNSS::setDynamicSPARTNKeys(uint8_t keyLengthBytes1, uint16_t vali
payloadCfg[18] = (validFromTow2 >> 16) & 0xFF;
payloadCfg[19] = (validFromTow2 >> 24) & 0xFF;
// Check if all keyLengthBytes are ASCII Hex 0-9, a-f, A-F
bool isASCIIHex = true;
uint16_t i = 0;
while ((i < (uint16_t)keyLengthBytes1) && (isASCIIHex == true))
{
if (((key1[i] >= '0') && (key1[i] <= '9')) || ((key1[i] >= 'a') && (key1[i] <= 'f')) || ((key1[i] >= 'A') && (key1[i] <= 'F')))
i++; // Keep checking if data is all ASCII Hex
else
isASCIIHex = false; // Data is binary
}
if (isASCIIHex) // Check the second half of the ASCII Hex key
{
while ((i < ((uint16_t)keyLengthBytes1 * 2) && (isASCIIHex == true)))
{
if (((key1[i] >= '0') && (key1[i] <= '9')) || ((key1[i] >= 'a') && (key1[i] <= 'f')) || ((key1[i] >= 'A') && (key1[i] <= 'F')))
i++; // Keep checking if data is all ASCII Hex
else
isASCIIHex = false; // Data is binary
}
}
if (isASCIIHex) // Convert ASCII Hex key to binary
{
for (i = 0; i < ((uint16_t)keyLengthBytes1 * 2); i += 2)
{
if ((key1[i] >= '0') && (key1[i] <= '9'))
{
payloadCfg[20 + (i >> 1)] = (key1[i] - '0') << 4;
}
else if ((key1[i] >= 'a') && (key1[i] <= 'f'))
{
payloadCfg[20 + (i >> 1)] = (key1[i] + 10 - 'a') << 4;
}
else // if ((key1[i] >= 'A') && (key1[i] <= 'F'))
{
payloadCfg[20 + (i >> 1)] = (key1[i] + 10 - 'A') << 4;
}
if ((key1[i + 1] >= '0') && (key1[i + 1] <= '9'))
{
payloadCfg[20 + (i >> 1)] |= key1[i + 1] - '0';
}
else if ((key1[i + 1] >= 'a') && (key1[i + 1] <= 'f'))
{
payloadCfg[20 + (i >> 1)] |= key1[i + 1] + 10 - 'a';
}
else // if ((key1[i + 1] >= 'A') && (key1[i + 1] <= 'F'))
{
payloadCfg[20 + (i >> 1)] |= key1[i + 1] + 10 - 'A';
}
}
}
else // Binary key
{
memcpy(&payloadCfg[20], key1, keyLengthBytes1);
}
// Check if all keyLengthBytes are ASCII Hex 0-9, a-f, A-F
isASCIIHex = true;
i = 0;
while ((i < (uint16_t)keyLengthBytes2) && (isASCIIHex == true))
{
if (((key2[i] >= '0') && (key2[i] <= '9')) || ((key2[i] >= 'a') && (key2[i] <= 'f')) || ((key2[i] >= 'A') && (key2[i] <= 'F')))
i++; // Keep checking if data is all ASCII Hex
else
isASCIIHex = false; // Data is binary
}
if (isASCIIHex) // Check the second half of the ASCII Hex key
{
while ((i < ((uint16_t)keyLengthBytes2 * 2) && (isASCIIHex == true)))
{
if (((key2[i] >= '0') && (key2[i] <= '9')) || ((key2[i] >= 'a') && (key2[i] <= 'f')) || ((key2[i] >= 'A') && (key2[i] <= 'F')))
i++; // Keep checking if data is all ASCII Hex
else
isASCIIHex = false; // Data is binary
}
}
if (isASCIIHex) // Convert ASCII Hex key to binary
{
for (i = 0; i < ((uint16_t)keyLengthBytes2 * 2); i += 2)
{
if ((key2[i] >= '0') && (key2[i] <= '9'))
{
payloadCfg[20 + keyLengthBytes1 + (i >> 1)] = (key2[i] - '0') << 4;
}
else if ((key2[i] >= 'a') && (key2[i] <= 'f'))
{
payloadCfg[20 + keyLengthBytes1 + (i >> 1)] = (key2[i] + 10 - 'a') << 4;
}
else // if ((key2[i] >= 'A') && (key2[i] <= 'F'))
{
payloadCfg[20 + keyLengthBytes1 + (i >> 1)] = (key2[i] + 10 - 'A') << 4;
}
if ((key2[i + 1] >= '0') && (key2[i + 1] <= '9'))
{
payloadCfg[20 + keyLengthBytes1 + (i >> 1)] |= key2[i + 1] - '0';
}
else if ((key2[i + 1] >= 'a') && (key2[i + 1] <= 'f'))
{
payloadCfg[20 + keyLengthBytes1 + (i >> 1)] |= key2[i + 1] + 10 - 'a';
}
else // if ((key2[i + 1] >= 'A') && (key2[i + 1] <= 'F'))
{
payloadCfg[20 + keyLengthBytes1 + (i >> 1)] |= key2[i + 1] + 10 - 'A';
}
}
}
else // Binary key
{
memcpy(&payloadCfg[20 + keyLengthBytes1], key2, keyLengthBytes2);
}
return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
return (sendCommand(&packetCfg, 0) == SFE_UBLOX_STATUS_SUCCESS); // UBX-RXM-SPARTNKEY is silent. It does not ACK (or NACK)
}
// CONFIGURATION INTERFACE (protocol v27 and above)
@@ -11460,7 +11495,7 @@ void SFE_UBLOX_GNSS::logAOPSTATUS(bool enabled)
// ***** RXM PMP automatic support
// Callback receives a pointer to the data, instead of _all_ the data. Much kinder on the stack!
bool SFE_UBLOX_GNSS::setAutoRXMPMPcallbackPtr(void (*callbackPointer)(UBX_RXM_PMP_data_t *))
bool SFE_UBLOX_GNSS::setRXMPMPcallbackPtr(void (*callbackPointer)(UBX_RXM_PMP_data_t *))
{
if (packetUBXRXMPMP == NULL)
initPacketUBXRXMPMP(); // Check that RAM has been allocated for the data
@@ -11500,7 +11535,50 @@ bool SFE_UBLOX_GNSS::initPacketUBXRXMPMP()
packetUBXRXMPMP->automaticFlags.flags.all = 0;
packetUBXRXMPMP->callbackPointerPtr = NULL;
packetUBXRXMPMP->callbackData = NULL;
packetUBXRXMPMP->moduleQueried = false;
return (true);
}
// Callback receives a pointer to the data, instead of _all_ the data. Much kinder on the stack!
bool SFE_UBLOX_GNSS::setRXMPMPmessageCallbackPtr(void (*callbackPointer)(UBX_RXM_PMP_message_data_t *))
{
if (packetUBXRXMPMPmessage == NULL)
initPacketUBXRXMPMPmessage(); // Check that RAM has been allocated for the data
if (packetUBXRXMPMPmessage == NULL) // Only attempt this if RAM allocation was successful
return false;
if (packetUBXRXMPMPmessage->callbackData == NULL) // Check if RAM has been allocated for the callback copy
{
packetUBXRXMPMPmessage->callbackData = new UBX_RXM_PMP_message_data_t; // Allocate RAM for the main struct
}
if (packetUBXRXMPMPmessage->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("setAutoRXMPMPmessagecallbackPtr: RAM alloc failed!"));
#endif
return (false);
}
packetUBXRXMPMPmessage->callbackPointerPtr = callbackPointer;
return (true);
}
// PRIVATE: Allocate RAM for packetUBXRXMPMPmessage and initialize it
bool SFE_UBLOX_GNSS::initPacketUBXRXMPMPmessage()
{
packetUBXRXMPMPmessage = new UBX_RXM_PMP_message_t; // Allocate RAM for the main struct
if (packetUBXRXMPMPmessage == NULL)
{
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
_debugSerial->println(F("initPacketUBXRXMPMPmessage: RAM alloc failed!"));
#endif
return (false);
}
packetUBXRXMPMPmessage->automaticFlags.flags.all = 0;
packetUBXRXMPMPmessage->callbackPointerPtr = NULL;
packetUBXRXMPMPmessage->callbackData = NULL;
return (true);
}
src/SparkFun_u-blox_GNSS_Arduino_Library.h
+10 -4
View File
@@ -901,10 +901,13 @@ public:
//"When the receiver boots, the host should send 'current' and 'next' keys in one message." - Use setDynamicSPARTNKeys for this.
//"Every time the 'current' key is expired, 'next' takes its place."
//"Therefore the host should then retrieve the new 'next' key and send only that." - Use setDynamicSPARTNKey for this.
// The key can be provided in binary format or in ASCII Hex format, but in both cases keyLengthBytes _must_ represent the binary key length in bytes.
bool setDynamicSPARTNKey(uint8_t keyLengthBytes, uint16_t validFromWno, uint32_t validFromTow, const uint8_t *key, uint16_t maxWait = defaultMaxWait);
// The key can be provided in binary (uint8_t) format or in ASCII Hex (char) format, but in both cases keyLengthBytes _must_ represent the binary key length in bytes.
bool setDynamicSPARTNKey(uint8_t keyLengthBytes, uint16_t validFromWno, uint32_t validFromTow, const char *key);
bool setDynamicSPARTNKey(uint8_t keyLengthBytes, uint16_t validFromWno, uint32_t validFromTow, const uint8_t *key);
bool setDynamicSPARTNKeys(uint8_t keyLengthBytes1, uint16_t validFromWno1, uint32_t validFromTow1, const char *key1,
uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const char *key2);
bool setDynamicSPARTNKeys(uint8_t keyLengthBytes1, uint16_t validFromWno1, uint32_t validFromTow1, const uint8_t *key1,
uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const uint8_t *key2, uint16_t maxWait = defaultMaxWait);
uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const uint8_t *key2);
// General configuration (used only on protocol v27 and higher - ie, ZED-F9P)
@@ -1110,7 +1113,8 @@ public:
// Note: on the NEO-D9S, the UBX-RXM-PMP messages are enabled by default on all ports.
// You can disable them by calling (e.g.) setVal8(UBLOX_CFG_MSGOUT_UBX_RXM_PMP_I2C, 0)
// The NEO-D9S does not support UBX-CFG-MSG
bool setAutoRXMPMPcallbackPtr(void (*callbackPointerPtr)(UBX_RXM_PMP_data_t *)); // Callback receives a pointer to the data, instead of _all_ the data. Much kinder on the stack!
bool setRXMPMPcallbackPtr(void (*callbackPointerPtr)(UBX_RXM_PMP_data_t *)); // Callback receives a pointer to the data, instead of _all_ the data. Much kinder on the stack!
bool setRXMPMPmessageCallbackPtr(void (*callbackPointerPtr)(UBX_RXM_PMP_message_data_t *)); // Use this if you want all of the PMP message (including sync chars, checksum, etc.) to push to a GNSS
bool getRXMSFRBX(uint16_t maxWait = defaultMaxWait); // RXM SFRBX
bool setAutoRXMSFRBX(bool enabled, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic RXM SFRBX reports at the navigation frequency
@@ -1443,6 +1447,7 @@ public:
UBX_NAV_AOPSTATUS_t *packetUBXNAVAOPSTATUS = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
UBX_RXM_PMP_t *packetUBXRXMPMP = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
UBX_RXM_PMP_message_t *packetUBXRXMPMPmessage = 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
@@ -1532,6 +1537,7 @@ private:
bool initPacketUBXNAVRELPOSNED(); // Allocate RAM for packetUBXNAVRELPOSNED and initialize it
bool initPacketUBXNAVAOPSTATUS(); // Allocate RAM for packetUBXNAVAOPSTATUS and initialize it
bool initPacketUBXRXMPMP(); // Allocate RAM for packetUBXRXMPMP and initialize it
bool initPacketUBXRXMPMPmessage(); // Allocate RAM for packetUBXRXMPMPRaw and initialize it
bool initPacketUBXRXMSFRBX(); // Allocate RAM for packetUBXRXMSFRBX and initialize it
bool initPacketUBXRXMRAWX(); // Allocate RAM for packetUBXRXMRAWX and initialize it
bool initPacketUBXCFGPRT(); // Allocate RAM for packetUBXCFGPRT and initialize it
src/u-blox_structs.h
+27 -4
View File
@@ -1486,7 +1486,8 @@ typedef struct
// UBX-RXM-PMP (0x02 0x72): PMP raw data (D9 modules)
// There are two versions of this message but, fortunately, both have a max len of 528
const uint16_t UBX_RXM_PMP_MAX_LEN = 528;
const uint16_t UBX_RXM_PMP_MAX_USER_DATA = 504;
const uint16_t UBX_RXM_PMP_MAX_LEN = UBX_RXM_PMP_MAX_USER_DATA + 24;
typedef struct
{
@@ -1504,19 +1505,41 @@ typedef struct
uint8_t ebno; // Energy per bit to noise power spectral density ratio : 2^-3 dB
uint8_t reserved1; // Reserved
uint8_t userData[504]; // Received user data: version 0x00 : starts at byte 20 ; version 0x01 : starts at byte 24
uint8_t userData[UBX_RXM_PMP_MAX_USER_DATA]; // Received user data: version 0x00 : starts at byte 20 ; version 0x01 : starts at byte 24
} UBX_RXM_PMP_data_t;
// The PMP data can only be accessed via a callback. PMP cannot be polled.
typedef struct
{
ubxAutomaticFlags automaticFlags;
UBX_RXM_PMP_data_t data;
bool moduleQueried;
void (*callbackPointerPtr)(UBX_RXM_PMP_data_t *);
UBX_RXM_PMP_data_t *callbackData;
} UBX_RXM_PMP_t;
// Define a struct to hold the entire PMP message so the whole thing can be pushed to a GNSS.
// Remember that the length of the payload could be variable (with version 1 messages).
typedef struct
{
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_PMP_MAX_LEN];
uint8_t checksumA;
uint8_t checksumB;
} UBX_RXM_PMP_message_data_t;
// The PMP data can only be accessed via a callback. PMP cannot be polled.
typedef struct
{
ubxAutomaticFlags automaticFlags;
void (*callbackPointerPtr)(UBX_RXM_PMP_message_data_t *);
UBX_RXM_PMP_message_data_t *callbackData;
} UBX_RXM_PMP_message_t;
// CFG-specific structs
// UBX-CFG-PRT (0x06 0x00): Port configuration