From 1d7621d25e14d73151f6bc86f06059951633df3c Mon Sep 17 00:00:00 2001 From: PaulZC Date: Thu, 10 Feb 2022 15:42:53 +0000 Subject: [PATCH] Add setDynamicSPARTNKey --- keywords.txt | 3 + src/SparkFun_u-blox_GNSS_Arduino_Library.cpp | 134 +++++++++++++++++-- src/SparkFun_u-blox_GNSS_Arduino_Library.h | 18 ++- src/u-blox_config_keys.h | 18 +-- 4 files changed, 154 insertions(+), 19 deletions(-) diff --git a/keywords.txt b/keywords.txt index 7ee9bd2..bba5c7f 100644 --- a/keywords.txt +++ b/keywords.txt @@ -178,6 +178,9 @@ setAckAiding KEYWORD2 getAopCfg KEYWORD2 setAopCfg KEYWORD2 +setDynamicSPARTNKey KEYWORD2 +setDynamicSPARTNKeys KEYWORD2 + createKey KEYWORD2 getVal KEYWORD2 getVal8 KEYWORD2 diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp index 05da70d..2b6ea37 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.cpp @@ -436,14 +436,17 @@ void SFE_UBLOX_GNSS::end(void) //Allow the user to change packetCfgPayloadSize. Handy if you want to process big messages like RAWX //This can be called before .begin if required / desired -void SFE_UBLOX_GNSS::setPacketCfgPayloadSize(size_t payloadSize) +bool SFE_UBLOX_GNSS::setPacketCfgPayloadSize(size_t payloadSize) { + bool success = true; + if ((payloadSize == 0) && (payloadCfg != NULL)) { // Zero payloadSize? Dangerous! But we'll free the memory anyway... delete[] payloadCfg; // Created with new[] payloadCfg = NULL; // Redundant? packetCfg.payload = payloadCfg; + packetCfgPayloadSize = payloadSize; if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging _debugSerial->println(F("setPacketCfgPayloadSize: Zero payloadSize!")); } @@ -453,24 +456,37 @@ void SFE_UBLOX_GNSS::setPacketCfgPayloadSize(size_t payloadSize) payloadCfg = new uint8_t[payloadSize]; packetCfg.payload = payloadCfg; if (payloadCfg == NULL) + { + success = false; + packetCfgPayloadSize = 0; if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging _debugSerial->println(F("setPacketCfgPayloadSize: RAM alloc failed!")); + } + else + packetCfgPayloadSize = payloadSize; } else //Memory has already been allocated - so resize { uint8_t *newPayload = new uint8_t[payloadSize]; - for (size_t i = 0; (i < payloadSize) && (i < packetCfgPayloadSize); i++) // Copy as much existing data as we can - newPayload[i] = payloadCfg[i]; - delete[] payloadCfg; // Created with new[] - payloadCfg = newPayload; - packetCfg.payload = payloadCfg; - if (payloadCfg == NULL) + + if (newPayload == NULL) // Check if the alloc was successful + { + success = false; // Report failure. Don't change payloadCfg, packetCfg.payload or packetCfgPayloadSize if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging _debugSerial->println(F("setPacketCfgPayloadSize: RAM resize failed!")); + } + else + { + memcpy(newPayload, payloadCfg, payloadSize <= packetCfgPayloadSize ? payloadSize : packetCfgPayloadSize); // Copy as much existing data as we can + delete[] payloadCfg; // Free payloadCfg. Created with new[] + payloadCfg = newPayload; // Point to the newPayload + packetCfg.payload = payloadCfg; // Update the packet pointer + packetCfgPayloadSize = payloadSize; // Update the packet payload size + } } - packetCfgPayloadSize = payloadSize; + return (success); } //Initialize the I2C port @@ -7469,6 +7485,108 @@ bool SFE_UBLOX_GNSS::setAopCfg(uint8_t aopCfg, uint16_t aopOrbMaxErr, uint16_t m return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK } +//SPARTN dynamic keys +//"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) +{ + // 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 + } + } + + // Check if there is room for the key in packetCfg. Resize the buffer if not. + size_t payloadLength = (size_t)keyLengthBytes + 12; + if (packetCfgPayloadSize < payloadLength) + { + if (!setPacketCfgPayloadSize(payloadLength)) // Check if the resize was successful + { + return (false); + } + } + + // Copy the key etc. into packetCfg + packetCfg.cls = UBX_CLASS_RXM; + packetCfg.id = UBX_RXM_SPARTNKEY; + packetCfg.len = payloadLength; + packetCfg.startingSpot = 0; + + payloadCfg[0] = 0x01; // version + payloadCfg[1] = 0x01; // numKeys + payloadCfg[2] = 0x00; // reserved0 + payloadCfg[3] = 0x00; // reserved0 + payloadCfg[4] = 0x00; // reserved1 + payloadCfg[5] = keyLengthBytes; + payloadCfg[6] = validFromWno & 0xFF; // validFromWno little-endian + payloadCfg[7] = validFromWno >> 8; + payloadCfg[8] = validFromTow & 0xFF; // validFromTow little-endian + payloadCfg[9] = (validFromTow >> 8) & 0xFF; + payloadCfg[10] = (validFromTow >> 16) & 0xFF; + payloadCfg[11] = (validFromTow >> 24) & 0xFF; + + 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, 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 uint8_t *key1, + uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const uint8_t *key2, uint16_t maxWait) +{ + return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK +} + // CONFIGURATION INTERFACE (protocol v27 and above) //Form 32-bit key from group/id/size diff --git a/src/SparkFun_u-blox_GNSS_Arduino_Library.h b/src/SparkFun_u-blox_GNSS_Arduino_Library.h index e675cfe..b8f96cf 100644 --- a/src/SparkFun_u-blox_GNSS_Arduino_Library.h +++ b/src/SparkFun_u-blox_GNSS_Arduino_Library.h @@ -338,6 +338,8 @@ const uint8_t UBX_MON_PATCH = 0x27; //Output information about installed patches const uint8_t UBX_MON_RF = 0x38; //RF information const uint8_t UBX_MON_RXBUF = 0x07; //Receiver Buffer Status const uint8_t UBX_MON_RXR = 0x21; //Receiver Status Information +const uint8_t UBX_MON_SPAN = 0x31; //Signal characteristics +const uint8_t UBX_MON_SYS = 0x39; //Current system performance information const uint8_t UBX_MON_TXBUF = 0x08; //Transmitter Buffer Status. Used for query tx buffer size/state. const uint8_t UBX_MON_VER = 0x04; //Receiver/Software Version. Used for obtaining Protocol Version. @@ -352,6 +354,7 @@ const uint8_t UBX_NAV_HPPOSECEF = 0x13; //High Precision Position Solution in EC const uint8_t UBX_NAV_HPPOSLLH = 0x14; //High Precision Geodetic Position Solution. Used for obtaining lat/long/alt in high precision const uint8_t UBX_NAV_ODO = 0x09; //Odometer Solution const uint8_t UBX_NAV_ORB = 0x34; //GNSS Orbit Database Info +const uint8_t UBX_NAV_PL = 0x62; //Protection Level Information const uint8_t UBX_NAV_POSECEF = 0x01; //Position Solution in ECEF const uint8_t UBX_NAV_POSLLH = 0x02; //Geodetic Position Solution 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 //Class: RXM //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) +const uint8_t UBX_RXM_COR = 0x34; // Differential correction input status const uint8_t UBX_RXM_MEASX = 0x14; //Satellite Measurements for RRLP -const uint8_t UBX_RXM_PMP = 0x72; //PMP raw data (NEO-D9S) (two different versions) (packet size for version 0x01 is variable) +const uint8_t UBX_RXM_PMP = 0x72; //PMP raw data (NEO-D9S) (two different versions) (packet size for version 0x01 is variable) const uint8_t UBX_RXM_PMREQ = 0x41; //Requests a Power Management task (two different packet sizes) const uint8_t UBX_RXM_RAWX = 0x15; //Multi-GNSS Raw Measurement Data const uint8_t UBX_RXM_RLM = 0x59; //Galileo SAR Short-RLM report (two different packet sizes) const uint8_t UBX_RXM_RTCM = 0x32; //RTCM input status const uint8_t UBX_RXM_SFRBX = 0x13; //Broadcast Navigation Data Subframe const uint8_t UBX_RXM_SPARTN = 0x33; //SPARTN input status +const uint8_t UBX_RXM_SPARTNKEY = 0x36; //Poll/transfer dynamic SPARTN keys //Class: SEC //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 @@ public: #endif //New in v2.0: allow the payload size for packetCfg to be changed - void setPacketCfgPayloadSize(size_t payloadSize); // Set packetCfgPayloadSize + bool setPacketCfgPayloadSize(size_t payloadSize); // Set packetCfgPayloadSize //Begin communication with the GNSS. Advanced users can assume success if required. Useful if the port is already outputting messages at high navigation rate. //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 @@ public: uint8_t getAopCfg(uint16_t maxWait = defaultMaxWait); // Get the AssistNow Autonomous configuration (aopCfg) - returns 255 if the sendCommand fails bool setAopCfg(uint8_t aopCfg, uint16_t aopOrbMaxErr = 0, uint16_t maxWait = defaultMaxWait); // Set the aopCfg byte and the aopOrdMaxErr word + //SPARTN dynamic keys + //"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); + 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); + //General configuration (used only on protocol v27 and higher - ie, ZED-F9P) //It is probably safe to assume that users of the ZED-F9P will be using I2C / Qwiic. diff --git a/src/u-blox_config_keys.h b/src/u-blox_config_keys.h index 82093ab..9c273de 100644 --- a/src/u-blox_config_keys.h +++ b/src/u-blox_config_keys.h @@ -382,10 +382,10 @@ const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_UART1 = 0x2091038c; // Output rate const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_UART2 = 0x2091038d; // Output rate of the UBX-MON-SPAN message on port UART2 const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SPAN_USB = 0x2091038e; // Output rate of the UBX-MON-SPAN message on port USB const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_I2C = 0x2091069d; // Output rate of the UBX-MON-SYS message on port I2C -const uint32_t UBLOX_CFG_MSGOUT_UBXUBX_MON_SYS_SPI = 0x209106a1; // Output rate of the UBX-MON-SYS message on port SPI -const uint32_t UBLOX_CFG_MSGOUT_UBXUBX_MON_SYS_UART1 = 0x2091069e; // Output rate of the UBX-MON-SYS message on port UART1 -const uint32_t UBLOX_CFG_MSGOUT_UBXUBX_MON_SYS_UART2 = 0x2091069f; // Output rate of the UBX-MON-SYS message on port UART2 -const uint32_t UBLOX_CFG_MSGOUT_UBXUBX_MON_SYS_USB = 0x209106a0; // Output rate of the UBX-MON-SYS message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_SPI = 0x209106a1; // Output rate of the UBX-MON-SYS message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_UART1 = 0x2091069e; // Output rate of the UBX-MON-SYS message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_UART2 = 0x2091069f; // Output rate of the UBX-MON-SYS message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_SYS_USB = 0x209106a0; // Output rate of the UBX-MON-SYS message on port USB const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_I2C = 0x2091019b; // Output rate of the UBX-MON-TXBUF message on port I2C const uint32_t UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_SPI = 0x2091019f; // Output rate of the UBX-MON-TXBUF message on port SPI 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 const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_UART1 = 0x20910011; // Output rate of the UBX-NAV-ORB message on port UART1 const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_UART2 = 0x20910012; // Output rate of the UBX-NAV-ORB message on port UART2 const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_ORB_USB = 0x20910013; // Output rate of the UBX-NAV-ORB message on port USB -const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_I2C 0x20910415; // Output rate of the UBX-NAV-PL message on port I2C -const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_SPI 0x20910419; // Output rate of the UBX-NAV-PL message on port SPI -const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_UART1 0x20910416; // Output rate of the UBX-NAV-PL message on port UART1 -const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_UART2 0x20910417; // Output rate of the UBX-NAV-PL message on port UART2 -const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_USB 0x20910418; // Output rate of the UBX-NAV-PL message on port USB +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_I2C = 0x20910415; // Output rate of the UBX-NAV-PL message on port I2C +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_SPI = 0x20910419; // Output rate of the UBX-NAV-PL message on port SPI +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_UART1 = 0x20910416; // Output rate of the UBX-NAV-PL message on port UART1 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_UART2 = 0x20910417; // Output rate of the UBX-NAV-PL message on port UART2 +const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_PL_USB = 0x20910418; // Output rate of the UBX-NAV-PL message on port USB const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_I2C = 0x20910024; // Output rate of the UBX-NAV-POSECEF message on port I2C const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_SPI = 0x20910028; // Output rate of the UBX-NAV-POSECEF message on port SPI const uint32_t UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_UART1 = 0x20910025;// Output rate of the UBX-NAV-POSECEF message on port UART1