Skip to content

Commit 1d7621d

Browse files
PaulZCPaulZC
committed
Add setDynamicSPARTNKey
1 parent dba2cff commit 1d7621d

File tree

4 files changed

+154
-19
lines changed

4 files changed

+154
-19
lines changed

‎keywords.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -178,6 +178,9 @@ setAckAiding KEYWORD2
178178
getAopCfg KEYWORD2
179179
setAopCfg KEYWORD2
180180

181+
setDynamicSPARTNKey KEYWORD2
182+
setDynamicSPARTNKeys KEYWORD2
183+
181184
createKey KEYWORD2
182185
getVal KEYWORD2
183186
getVal8 KEYWORD2

‎src/SparkFun_u-blox_GNSS_Arduino_Library.cpp

Lines changed: 126 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -436,14 +436,17 @@ void SFE_UBLOX_GNSS::end(void)
436436

437437
//Allow the user to change packetCfgPayloadSize. Handy if you want to process big messages like RAWX
438438
//This can be called before .begin if required / desired
439-
void SFE_UBLOX_GNSS::setPacketCfgPayloadSize(size_t payloadSize)
439+
bool SFE_UBLOX_GNSS::setPacketCfgPayloadSize(size_t payloadSize)
440440
{
441+
bool success = true;
442+
441443
if ((payloadSize == 0) && (payloadCfg != NULL))
442444
{
443445
// Zero payloadSize? Dangerous! But we'll free the memory anyway...
444446
delete[] payloadCfg; // Created with new[]
445447
payloadCfg = NULL; // Redundant?
446448
packetCfg.payload = payloadCfg;
449+
packetCfgPayloadSize = payloadSize;
447450
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
448451
_debugSerial->println(F("setPacketCfgPayloadSize: Zero payloadSize!"));
449452
}
@@ -453,24 +456,37 @@ void SFE_UBLOX_GNSS::setPacketCfgPayloadSize(size_t payloadSize)
453456
payloadCfg = new uint8_t[payloadSize];
454457
packetCfg.payload = payloadCfg;
455458
if (payloadCfg == NULL)
459+
{
460+
success = false;
461+
packetCfgPayloadSize = 0;
456462
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
457463
_debugSerial->println(F("setPacketCfgPayloadSize: RAM alloc failed!"));
464+
}
465+
else
466+
packetCfgPayloadSize = payloadSize;
458467
}
459468

460469
else //Memory has already been allocated - so resize
461470
{
462471
uint8_t *newPayload = new uint8_t[payloadSize];
463-
for (size_t i = 0; (i < payloadSize) && (i < packetCfgPayloadSize); i++) // Copy as much existing data as we can
464-
newPayload[i] = payloadCfg[i];
465-
delete[] payloadCfg; // Created with new[]
466-
payloadCfg = newPayload;
467-
packetCfg.payload = payloadCfg;
468-
if (payloadCfg == NULL)
472+
473+
if (newPayload == NULL) // Check if the alloc was successful
474+
{
475+
success = false; // Report failure. Don't change payloadCfg, packetCfg.payload or packetCfgPayloadSize
469476
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
470477
_debugSerial->println(F("setPacketCfgPayloadSize: RAM resize failed!"));
478+
}
479+
else
480+
{
481+
memcpy(newPayload, payloadCfg, payloadSize <= packetCfgPayloadSize ? payloadSize : packetCfgPayloadSize); // Copy as much existing data as we can
482+
delete[] payloadCfg; // Free payloadCfg. Created with new[]
483+
payloadCfg = newPayload; // Point to the newPayload
484+
packetCfg.payload = payloadCfg; // Update the packet pointer
485+
packetCfgPayloadSize = payloadSize; // Update the packet payload size
486+
}
471487
}
472488

473-
packetCfgPayloadSize = payloadSize;
489+
return (success);
474490
}
475491

476492
//Initialize the I2C port
@@ -7469,6 +7485,108 @@ bool SFE_UBLOX_GNSS::setAopCfg(uint8_t aopCfg, uint16_t aopOrbMaxErr, uint16_t m
74697485
return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
74707486
}
74717487

7488+
//SPARTN dynamic keys
7489+
//"When the receiver boots, the host should send 'current' and 'next' keys in one message." - Use setDynamicSPARTNKeys for this.
7490+
//"Every time the 'current' key is expired, 'next' takes its place."
7491+
//"Therefore the host should then retrieve the new 'next' key and send only that." - Use setDynamicSPARTNKey for this.
7492+
//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.
7493+
bool SFE_UBLOX_GNSS::setDynamicSPARTNKey(uint8_t keyLengthBytes, uint16_t validFromWno, uint32_t validFromTow, const uint8_t *key, uint16_t maxWait)
7494+
{
7495+
// Check if all keyLengthBytes are ASCII Hex 0-9, a-f, A-F
7496+
bool isASCIIHex = true;
7497+
uint16_t i = 0;
7498+
while((i < (uint16_t)keyLengthBytes) && (isASCIIHex == true))
7499+
{
7500+
if (((key[i] >= '0') && (key[i] <= '9')) || ((key[i] >= 'a') && (key[i] <= 'f')) || ((key[i] >= 'A') && (key[i] <= 'F')))
7501+
i++; // Keep checking if data is all ASCII Hex
7502+
else
7503+
isASCIIHex = false; // Data is binary
7504+
}
7505+
if (isASCIIHex) // Check the second half of the ASCII Hex key
7506+
{
7507+
while((i < ((uint16_t)keyLengthBytes * 2) && (isASCIIHex == true)))
7508+
{
7509+
if (((key[i] >= '0') && (key[i] <= '9')) || ((key[i] >= 'a') && (key[i] <= 'f')) || ((key[i] >= 'A') && (key[i] <= 'F')))
7510+
i++; // Keep checking if data is all ASCII Hex
7511+
else
7512+
isASCIIHex = false; // Data is binary
7513+
}
7514+
}
7515+
7516+
// Check if there is room for the key in packetCfg. Resize the buffer if not.
7517+
size_t payloadLength = (size_t)keyLengthBytes + 12;
7518+
if (packetCfgPayloadSize < payloadLength)
7519+
{
7520+
if (!setPacketCfgPayloadSize(payloadLength)) // Check if the resize was successful
7521+
{
7522+
return (false);
7523+
}
7524+
}
7525+
7526+
// Copy the key etc. into packetCfg
7527+
packetCfg.cls = UBX_CLASS_RXM;
7528+
packetCfg.id = UBX_RXM_SPARTNKEY;
7529+
packetCfg.len = payloadLength;
7530+
packetCfg.startingSpot = 0;
7531+
7532+
payloadCfg[0] = 0x01; // version
7533+
payloadCfg[1] = 0x01; // numKeys
7534+
payloadCfg[2] = 0x00; // reserved0
7535+
payloadCfg[3] = 0x00; // reserved0
7536+
payloadCfg[4] = 0x00; // reserved1
7537+
payloadCfg[5] = keyLengthBytes;
7538+
payloadCfg[6] = validFromWno & 0xFF; // validFromWno little-endian
7539+
payloadCfg[7] = validFromWno >> 8;
7540+
payloadCfg[8] = validFromTow & 0xFF; // validFromTow little-endian
7541+
payloadCfg[9] = (validFromTow >> 8) & 0xFF;
7542+
payloadCfg[10] = (validFromTow >> 16) & 0xFF;
7543+
payloadCfg[11] = (validFromTow >> 24) & 0xFF;
7544+
7545+
if (isASCIIHex) // Convert ASCII Hex key to binary
7546+
{
7547+
for(i = 0; i < ((uint16_t)keyLengthBytes * 2); i += 2)
7548+
{
7549+
if ((key[i] >= '0') && (key[i] <= '9'))
7550+
{
7551+
payloadCfg[12 + (i >> 1)] = (key[i] - '0') << 4;
7552+
}
7553+
else if ((key[i] >= 'a') && (key[i] <= 'f'))
7554+
{
7555+
payloadCfg[12 + (i >> 1)] = (key[i] + 10 - 'a') << 4;
7556+
}
7557+
else // if ((key[i] >= 'A') && (key[i] <= 'F'))
7558+
{
7559+
payloadCfg[12 + (i >> 1)] = (key[i] + 10 - 'A') << 4;
7560+
}
7561+
7562+
if ((key[i + 1] >= '0') && (key[i + 1] <= '9'))
7563+
{
7564+
payloadCfg[12 + (i >> 1)] |= key[i + 1] - '0';
7565+
}
7566+
else if ((key[i + 1] >= 'a') && (key[i + 1] <= 'f'))
7567+
{
7568+
payloadCfg[12 + (i >> 1)] |= key[i + 1] + 10 - 'a';
7569+
}
7570+
else // if ((key[i + 1] >= 'A') && (key[i + 1] <= 'F'))
7571+
{
7572+
payloadCfg[12 + (i >> 1)] |= key[i + 1] + 10 - 'A';
7573+
}
7574+
}
7575+
}
7576+
else // Binary key
7577+
{
7578+
memcpy(&payloadCfg[12], key, keyLengthBytes);
7579+
}
7580+
7581+
return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
7582+
}
7583+
7584+
bool SFE_UBLOX_GNSS::setDynamicSPARTNKeys(uint8_t keyLengthBytes1, uint16_t validFromWno1, uint32_t validFromTow1, const uint8_t *key1,
7585+
uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const uint8_t *key2, uint16_t maxWait)
7586+
{
7587+
return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
7588+
}
7589+
74727590
// CONFIGURATION INTERFACE (protocol v27 and above)
74737591

74747592
//Form 32-bit key from group/id/size

‎src/SparkFun_u-blox_GNSS_Arduino_Library.h

Lines changed: 16 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -338,6 +338,8 @@ const uint8_t UBX_MON_PATCH = 0x27; //Output information about installed patches
338338
const uint8_t UBX_MON_RF = 0x38; //RF information
339339
const uint8_t UBX_MON_RXBUF = 0x07; //Receiver Buffer Status
340340
const uint8_t UBX_MON_RXR = 0x21; //Receiver Status Information
341+
const uint8_t UBX_MON_SPAN = 0x31; //Signal characteristics
342+
const uint8_t UBX_MON_SYS = 0x39; //Current system performance information
341343
const uint8_t UBX_MON_TXBUF = 0x08; //Transmitter Buffer Status. Used for query tx buffer size/state.
342344
const uint8_t UBX_MON_VER = 0x04; //Receiver/Software Version. Used for obtaining Protocol Version.
343345

@@ -352,6 +354,7 @@ const uint8_t UBX_NAV_HPPOSECEF = 0x13; //High Precision Position Solution in EC
352354
const uint8_t UBX_NAV_HPPOSLLH = 0x14; //High Precision Geodetic Position Solution. Used for obtaining lat/long/alt in high precision
353355
const uint8_t UBX_NAV_ODO = 0x09; //Odometer Solution
354356
const uint8_t UBX_NAV_ORB = 0x34; //GNSS Orbit Database Info
357+
const uint8_t UBX_NAV_PL = 0x62; //Protection Level Information
355358
const uint8_t UBX_NAV_POSECEF = 0x01; //Position Solution in ECEF
356359
const uint8_t UBX_NAV_POSLLH = 0x02; //Geodetic Position Solution
357360
const uint8_t UBX_NAV_PVT = 0x07; //All the things! Position, velocity, time, PDOP, height, h/v accuracies, number of satellites. Navigation Position Velocity Time Solution.
@@ -374,14 +377,16 @@ const uint8_t UBX_NAV_AOPSTATUS = 0x60; //AssistNow Autonomous status
374377

375378
//Class: RXM
376379
//The following are used to configure the RXM UBX messages (receiver manager messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 36)
380+
const uint8_t UBX_RXM_COR = 0x34; // Differential correction input status
377381
const uint8_t UBX_RXM_MEASX = 0x14; //Satellite Measurements for RRLP
378-
const uint8_t UBX_RXM_PMP = 0x72; //PMP raw data (NEO-D9S) (two different versions) (packet size for version 0x01 is variable)
382+
const uint8_t UBX_RXM_PMP = 0x72; //PMP raw data (NEO-D9S) (two different versions) (packet size for version 0x01 is variable)
379383
const uint8_t UBX_RXM_PMREQ = 0x41; //Requests a Power Management task (two different packet sizes)
380384
const uint8_t UBX_RXM_RAWX = 0x15; //Multi-GNSS Raw Measurement Data
381385
const uint8_t UBX_RXM_RLM = 0x59; //Galileo SAR Short-RLM report (two different packet sizes)
382386
const uint8_t UBX_RXM_RTCM = 0x32; //RTCM input status
383387
const uint8_t UBX_RXM_SFRBX = 0x13; //Broadcast Navigation Data Subframe
384388
const uint8_t UBX_RXM_SPARTN = 0x33; //SPARTN input status
389+
const uint8_t UBX_RXM_SPARTNKEY = 0x36; //Poll/transfer dynamic SPARTN keys
385390

386391
//Class: SEC
387392
//The following are used to configure the SEC UBX messages (security feature messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 36)
@@ -621,7 +626,7 @@ class SFE_UBLOX_GNSS
621626
#endif
622627

623628
//New in v2.0: allow the payload size for packetCfg to be changed
624-
void setPacketCfgPayloadSize(size_t payloadSize); // Set packetCfgPayloadSize
629+
bool setPacketCfgPayloadSize(size_t payloadSize); // Set packetCfgPayloadSize
625630

626631
//Begin communication with the GNSS. Advanced users can assume success if required. Useful if the port is already outputting messages at high navigation rate.
627632
//Begin will then return true if "signs of life" have been seen: reception of _any_ valid UBX packet or _any_ valid NMEA header.
@@ -892,6 +897,15 @@ class SFE_UBLOX_GNSS
892897
uint8_t getAopCfg(uint16_t maxWait = defaultMaxWait); // Get the AssistNow Autonomous configuration (aopCfg) - returns 255 if the sendCommand fails
893898
bool setAopCfg(uint8_t aopCfg, uint16_t aopOrbMaxErr = 0, uint16_t maxWait = defaultMaxWait); // Set the aopCfg byte and the aopOrdMaxErr word
894899

900+
//SPARTN dynamic keys
901+
//"When the receiver boots, the host should send 'current' and 'next' keys in one message." - Use setDynamicSPARTNKeys for this.
902+
//"Every time the 'current' key is expired, 'next' takes its place."
903+
//"Therefore the host should then retrieve the new 'next' key and send only that." - Use setDynamicSPARTNKey for this.
904+
//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.
905+
bool setDynamicSPARTNKey(uint8_t keyLengthBytes, uint16_t validFromWno, uint32_t validFromTow, const uint8_t *key, uint16_t maxWait = defaultMaxWait);
906+
bool setDynamicSPARTNKeys(uint8_t keyLengthBytes1, uint16_t validFromWno1, uint32_t validFromTow1, const uint8_t *key1,
907+
uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const uint8_t *key2, uint16_t maxWait = defaultMaxWait);
908+
895909
//General configuration (used only on protocol v27 and higher - ie, ZED-F9P)
896910

897911
//It is probably safe to assume that users of the ZED-F9P will be using I2C / Qwiic.

‎src/u-blox_config_keys.h

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -382,10 +382,10 @@ const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_UART1 = 0x2091038c; // Output rate
382382
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_UART2 = 0x2091038d; // Output rate of the UBX-MON-SPAN message on port UART2
383383
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_USB = 0x2091038e; // Output rate of the UBX-MON-SPAN message on port USB
384384
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_I2C = 0x2091069d; // Output rate of the UBX-MON-SYS message on port I2C
385-
const uint32_t UBLOX_CFG_MSGOUT_UBXUBX_MON_SYS_SPI = 0x209106a1; // Output rate of the UBX-MON-SYS message on port SPI
386-
const uint32_t UBLOX_CFG_MSGOUT_UBXUBX_MON_SYS_UART1 = 0x2091069e; // Output rate of the UBX-MON-SYS message on port UART1
387-
const uint32_t UBLOX_CFG_MSGOUT_UBXUBX_MON_SYS_UART2 = 0x2091069f; // Output rate of the UBX-MON-SYS message on port UART2
388-
const uint32_t UBLOX_CFG_MSGOUT_UBXUBX_MON_SYS_USB = 0x209106a0; // Output rate of the UBX-MON-SYS message on port USB
385+
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_SPI = 0x209106a1; // Output rate of the UBX-MON-SYS message on port SPI
386+
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_UART1 = 0x2091069e; // Output rate of the UBX-MON-SYS message on port UART1
387+
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_UART2 = 0x2091069f; // Output rate of the UBX-MON-SYS message on port UART2
388+
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_USB = 0x209106a0; // Output rate of the UBX-MON-SYS message on port USB
389389
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_I2C = 0x2091019b; // Output rate of the UBX-MON-TXBUF message on port I2C
390390
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_SPI = 0x2091019f; // Output rate of the UBX-MON-TXBUF message on port SPI
391391
const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_UART1 = 0x2091019c; // Output rate of the UBX-MON-TXBUF message on port UART1
@@ -436,11 +436,11 @@ const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_SPI = 0x20910014; // Output rate
436436
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_UART1 = 0x20910011; // Output rate of the UBX-NAV-ORB message on port UART1
437437
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_UART2 = 0x20910012; // Output rate of the UBX-NAV-ORB message on port UART2
438438
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_USB = 0x20910013; // Output rate of the UBX-NAV-ORB message on port USB
439-
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_I2C 0x20910415; // Output rate of the UBX-NAV-PL message on port I2C
440-
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_SPI 0x20910419; // Output rate of the UBX-NAV-PL message on port SPI
441-
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_UART1 0x20910416; // Output rate of the UBX-NAV-PL message on port UART1
442-
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_UART2 0x20910417; // Output rate of the UBX-NAV-PL message on port UART2
443-
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_USB 0x20910418; // Output rate of the UBX-NAV-PL message on port USB
439+
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_I2C = 0x20910415; // Output rate of the UBX-NAV-PL message on port I2C
440+
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_SPI = 0x20910419; // Output rate of the UBX-NAV-PL message on port SPI
441+
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_UART1 = 0x20910416; // Output rate of the UBX-NAV-PL message on port UART1
442+
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_UART2 = 0x20910417; // Output rate of the UBX-NAV-PL message on port UART2
443+
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_USB = 0x20910418; // Output rate of the UBX-NAV-PL message on port USB
444444
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_I2C = 0x20910024; // Output rate of the UBX-NAV-POSECEF message on port I2C
445445
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_SPI = 0x20910028; // Output rate of the UBX-NAV-POSECEF message on port SPI
446446
const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_UART1 = 0x20910025;// Output rate of the UBX-NAV-POSECEF message on port UART1

0 commit comments

Comments
 (0)