Skip to content

Commit 10dbb36

Browse files
PaulZCPaulZC
committed
Move parseSPARTN into the library
1 parent 005b565 commit 10dbb36

File tree

6 files changed

+551
-1061
lines changed

6 files changed

+551
-1061
lines changed

‎examples/NEO-D9S_and_NEO-D9C/Example8_LBand_SPARTN_Parsing/Example8_LBand_SPARTN_Parsing.ino‎

Lines changed: 1 addition & 532 deletions
Large diffs are not rendered by default.

‎examples/NEO-D9S_and_NEO-D9C/Example9_LBand_SPARTN_over_Serial/Example9_LBand_SPARTN_over_Serial.ino‎

Lines changed: 1 addition & 529 deletions
Large diffs are not rendered by default.

‎keywords.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -244,6 +244,7 @@ setAopCfg KEYWORD2
244244

245245
setDynamicSPARTNKey KEYWORD2
246246
setDynamicSPARTNKeys KEYWORD2
247+
parseSPARTN KEYWORD2
247248

248249
getUniqueChipId KEYWORD2
249250
getUniqueChipIdStr KEYWORD2

‎src/u-blox_GNSS.cpp‎

Lines changed: 359 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9015,6 +9015,365 @@ bool DevUBLOXGNSS::setDynamicSPARTNKeys(uint8_t keyLengthBytes1, uint16_t validF
90159015
return (sendCommand(&packetCfg, 0) == SFE_UBLOX_STATUS_SUCCESS); // UBX-RXM-SPARTNKEY is silent. It does not ACK (or NACK)
90169016
}
90179017

9018+
// Support for SPARTN parsing
9019+
// Mostly stolen from https://github.com/u-blox/ubxlib/blob/master/common/spartn/src/u_spartn_crc.c
9020+
9021+
uint8_t DevUBLOXGNSS::uSpartnCrc4(const uint8_t *pU8Msg, size_t size)
9022+
{
9023+
// Initialize local variables
9024+
uint8_t u8TableRemainder;
9025+
uint8_t u8Remainder = 0; // Initial remainder
9026+
9027+
// Compute the CRC value
9028+
// Divide each byte of the message by the corresponding polynomial
9029+
for (size_t x = 0; x < size; x++) {
9030+
u8TableRemainder = pU8Msg[x] ^ u8Remainder;
9031+
u8Remainder = sfe_ublox_u8Crc4Table[u8TableRemainder];
9032+
}
9033+
9034+
return u8Remainder;
9035+
}
9036+
9037+
uint8_t DevUBLOXGNSS::uSpartnCrc8(const uint8_t *pU8Msg, size_t size)
9038+
{
9039+
// Initialize local variables
9040+
uint8_t u8TableRemainder;
9041+
uint8_t u8Remainder = 0; // Initial remainder
9042+
9043+
// Compute the CRC value
9044+
// Divide each byte of the message by the corresponding polynomial
9045+
for (size_t x = 0; x < size; x++) {
9046+
u8TableRemainder = pU8Msg[x] ^ u8Remainder;
9047+
u8Remainder = sfe_ublox_u8Crc8Table[u8TableRemainder];
9048+
}
9049+
9050+
return u8Remainder;
9051+
}
9052+
9053+
uint16_t DevUBLOXGNSS::uSpartnCrc16(const uint8_t *pU8Msg, size_t size)
9054+
{
9055+
// Initialize local variables
9056+
uint16_t u16TableRemainder;
9057+
uint16_t u16Remainder = 0; // Initial remainder
9058+
uint8_t u8NumBitsInCrc = (8 * sizeof(uint16_t));
9059+
9060+
// Compute the CRC value
9061+
// Divide each byte of the message by the corresponding polynomial
9062+
for (size_t x = 0; x < size; x++) {
9063+
u16TableRemainder = pU8Msg[x] ^ (u16Remainder >> (u8NumBitsInCrc - 8));
9064+
u16Remainder = sfe_ublox_u16Crc16Table[u16TableRemainder] ^ (u16Remainder << 8);
9065+
}
9066+
9067+
return u16Remainder;
9068+
}
9069+
9070+
uint32_t DevUBLOXGNSS::uSpartnCrc24(const uint8_t *pU8Msg, size_t size)
9071+
{
9072+
// Initialize local variables
9073+
uint32_t u32TableRemainder;
9074+
uint32_t u32Remainder = 0; // Initial remainder
9075+
uint8_t u8NumBitsInCrc = (8 * sizeof(uint8_t) * 3);
9076+
9077+
// Compute the CRC value
9078+
// Divide each byte of the message by the corresponding polynomial
9079+
for (size_t x = 0; x < size; x++) {
9080+
u32TableRemainder = pU8Msg[x] ^ (u32Remainder >> (u8NumBitsInCrc - 8));
9081+
u32Remainder = sfe_ublox_u32Crc24Table[u32TableRemainder] ^ (u32Remainder << 8);
9082+
u32Remainder = u32Remainder & 0x00FFFFFF; // Only interested in 24 bits
9083+
}
9084+
9085+
return u32Remainder;
9086+
}
9087+
9088+
uint32_t DevUBLOXGNSS::uSpartnCrc32(const uint8_t *pU8Msg, size_t size)
9089+
{
9090+
// Initialize local variables
9091+
uint32_t u32TableRemainder;
9092+
uint32_t u32Remainder = 0xFFFFFFFFU; // Initial remainder
9093+
uint8_t u8NumBitsInCrc = (8 * sizeof(uint32_t));
9094+
uint32_t u32FinalXORValue = 0xFFFFFFFFU;
9095+
9096+
// Compute the CRC value
9097+
// Divide each byte of the message by the corresponding polynomial
9098+
for (size_t x = 0; x < size; x++) {
9099+
u32TableRemainder = pU8Msg[x] ^ (u32Remainder >> (u8NumBitsInCrc - 8));
9100+
u32Remainder = sfe_ublox_u32Crc32Table[u32TableRemainder] ^ (u32Remainder << 8);
9101+
}
9102+
9103+
u32Remainder = u32Remainder ^ u32FinalXORValue;
9104+
9105+
return u32Remainder;
9106+
}
9107+
9108+
// Parse SPARTN data
9109+
uint8_t * DevUBLOXGNSS::parseSPARTN(uint8_t incoming, bool &valid, uint16_t &len)
9110+
{
9111+
typedef enum {
9112+
waitingFor73,
9113+
TF002_TF006,
9114+
TF007,
9115+
TF009,
9116+
TF016,
9117+
TF017,
9118+
TF018
9119+
} parseStates;
9120+
static parseStates parseState = waitingFor73;
9121+
9122+
static uint8_t spartn[1100];
9123+
9124+
static uint16_t frameCount;
9125+
static uint8_t messageType;
9126+
static uint16_t payloadLength;
9127+
static uint16_t EAF;
9128+
static uint8_t crcType;
9129+
static uint16_t crcBytes;
9130+
static uint8_t frameCRC;
9131+
static uint8_t messageSubtype;
9132+
static uint16_t timeTagType;
9133+
static uint16_t authenticationIndicator;
9134+
static uint16_t embeddedApplicationLengthBytes;
9135+
static uint16_t TF007toTF016;
9136+
9137+
valid = false;
9138+
9139+
switch(parseState)
9140+
{
9141+
case waitingFor73:
9142+
if (incoming == 0x73)
9143+
{
9144+
parseState = TF002_TF006;
9145+
frameCount = 0;
9146+
spartn[0] = incoming;
9147+
}
9148+
break;
9149+
case TF002_TF006:
9150+
spartn[1 + frameCount] = incoming;
9151+
if (frameCount == 0)
9152+
{
9153+
messageType = incoming >> 1;
9154+
payloadLength = incoming & 0x01;
9155+
}
9156+
if (frameCount == 1)
9157+
{
9158+
payloadLength <<= 8;
9159+
payloadLength |= incoming;
9160+
}
9161+
if (frameCount == 2)
9162+
{
9163+
payloadLength <<= 1;
9164+
payloadLength |= incoming >> 7;
9165+
EAF = (incoming >> 6) & 0x01;
9166+
crcType = (incoming >> 4) & 0x03;
9167+
switch (crcType)
9168+
{
9169+
case 0:
9170+
crcBytes = 1;
9171+
break;
9172+
case 1:
9173+
crcBytes = 2;
9174+
break;
9175+
case 2:
9176+
crcBytes = 3;
9177+
break;
9178+
default:
9179+
crcBytes = 4;
9180+
break;
9181+
}
9182+
frameCRC = incoming & 0x0F;
9183+
spartn[3] = spartn[3] & 0xF0; // Zero the 4 LSBs before calculating the CRC
9184+
if (uSpartnCrc4(&spartn[1], 3) == frameCRC)
9185+
{
9186+
spartn[3] = incoming; // Restore TF005 and TF006 now we know the data is valid
9187+
parseState = TF007;
9188+
//Serial.println("Header CRC is valid");
9189+
//Serial.printf("payloadLength %d EAF %d crcType %d\n", payloadLength, EAF, crcType);
9190+
}
9191+
else
9192+
{
9193+
parseState = waitingFor73;
9194+
//Serial.println("Header CRC is INVALID");
9195+
}
9196+
}
9197+
frameCount++;
9198+
break;
9199+
case TF007:
9200+
spartn[4] = incoming;
9201+
messageSubtype = incoming >> 4;
9202+
timeTagType = (incoming >> 3) & 0x01;
9203+
//Serial.printf("timeTagType %d\n", timeTagType);
9204+
if (timeTagType == 0)
9205+
TF007toTF016 = 4;
9206+
else
9207+
TF007toTF016 = 6;
9208+
if (EAF > 0)
9209+
TF007toTF016 += 2;
9210+
parseState = TF009;
9211+
frameCount = 1;
9212+
break;
9213+
case TF009:
9214+
spartn[4 + frameCount] = incoming;
9215+
frameCount++;
9216+
if (frameCount == TF007toTF016)
9217+
{
9218+
if (EAF == 0)
9219+
{
9220+
authenticationIndicator = 0;
9221+
embeddedApplicationLengthBytes = 0;
9222+
}
9223+
else
9224+
{
9225+
authenticationIndicator = (incoming >> 3) & 0x07;
9226+
//Serial.printf("authenticationIndicator %d\n", authenticationIndicator);
9227+
if (authenticationIndicator <= 1)
9228+
embeddedApplicationLengthBytes = 0;
9229+
else
9230+
{
9231+
switch(incoming & 0x07)
9232+
{
9233+
case 0:
9234+
embeddedApplicationLengthBytes = 8; // 64 bits
9235+
break;
9236+
case 1:
9237+
embeddedApplicationLengthBytes = 12; // 96 bits
9238+
break;
9239+
case 2:
9240+
embeddedApplicationLengthBytes = 16; // 128 bits
9241+
break;
9242+
case 3:
9243+
embeddedApplicationLengthBytes = 32; // 256 bits
9244+
break;
9245+
default:
9246+
embeddedApplicationLengthBytes = 64; // 512 / TBD bits
9247+
break;
9248+
}
9249+
}
9250+
//Serial.printf("embeddedApplicationLengthBytes %d\n", embeddedApplicationLengthBytes);
9251+
}
9252+
parseState = TF016;
9253+
frameCount = 0;
9254+
}
9255+
break;
9256+
case TF016:
9257+
spartn[4 + TF007toTF016 + frameCount] = incoming;
9258+
frameCount++;
9259+
if (frameCount == payloadLength)
9260+
{
9261+
if (embeddedApplicationLengthBytes > 0)
9262+
{
9263+
parseState = TF017;
9264+
frameCount = 0;
9265+
}
9266+
else
9267+
{
9268+
parseState = TF018;
9269+
frameCount = 0;
9270+
}
9271+
}
9272+
break;
9273+
case TF017:
9274+
spartn[4 + TF007toTF016 + payloadLength + frameCount] = incoming;
9275+
frameCount++;
9276+
if (frameCount == embeddedApplicationLengthBytes)
9277+
{
9278+
parseState = TF018;
9279+
frameCount = 0;
9280+
}
9281+
break;
9282+
case TF018:
9283+
spartn[4 + TF007toTF016 + payloadLength + embeddedApplicationLengthBytes + frameCount] = incoming;
9284+
frameCount++;
9285+
if (frameCount == crcBytes)
9286+
{
9287+
parseState = waitingFor73;
9288+
uint16_t numBytes = 4 + TF007toTF016 + payloadLength + embeddedApplicationLengthBytes;
9289+
//Serial.printf("numBytes %d\n", numBytes);
9290+
uint8_t *ptr = &spartn[numBytes];
9291+
switch (crcType)
9292+
{
9293+
case 0:
9294+
{
9295+
uint8_t expected = *ptr;
9296+
if (uSpartnCrc8(&spartn[1], numBytes - 1) == expected) // Don't include the preamble in the CRC
9297+
{
9298+
valid = true;
9299+
len = numBytes + 1;
9300+
//Serial.println("SPARTN CRC-8 is valid");
9301+
}
9302+
else
9303+
{
9304+
//Serial.println("SPARTN CRC-8 is INVALID");
9305+
}
9306+
}
9307+
break;
9308+
case 1:
9309+
{
9310+
uint16_t expected = *ptr++;
9311+
expected <<= 8;
9312+
expected |= *ptr;
9313+
if (uSpartnCrc16(&spartn[1], numBytes - 1) == expected) // Don't include the preamble in the CRC
9314+
{
9315+
valid = true;
9316+
len = numBytes + 2;
9317+
//Serial.println("SPARTN CRC-16 is valid");
9318+
}
9319+
else
9320+
{
9321+
//Serial.println("SPARTN CRC-16 is INVALID");
9322+
}
9323+
}
9324+
break;
9325+
case 2:
9326+
{
9327+
uint32_t expected = *ptr++;
9328+
expected <<= 8;
9329+
expected |= *ptr++;
9330+
expected <<= 8;
9331+
expected |= *ptr;
9332+
uint32_t crc = uSpartnCrc24(&spartn[1], numBytes - 1); // Don't include the preamble in the CRC
9333+
if (crc == expected)
9334+
{
9335+
valid = true;
9336+
len = numBytes + 3;
9337+
//Serial.println("SPARTN CRC-24 is valid");
9338+
}
9339+
else
9340+
{
9341+
//Serial.printf("SPARTN CRC-24 is INVALID: 0x%06X vs 0x%06X\n", expected, crc);
9342+
}
9343+
}
9344+
break;
9345+
default:
9346+
{
9347+
uint32_t expected = *ptr++;
9348+
expected <<= 8;
9349+
expected |= *ptr++;
9350+
expected <<= 8;
9351+
expected |= *ptr++;
9352+
expected <<= 8;
9353+
expected |= *ptr;
9354+
if (uSpartnCrc32(&spartn[1], numBytes - 1) == expected)
9355+
{
9356+
valid = true;
9357+
len = numBytes + 4;
9358+
//Serial.println("SPARTN CRC-32 is valid");
9359+
}
9360+
else
9361+
{
9362+
//Serial.println("SPARTN CRC-32 is INVALID");
9363+
}
9364+
}
9365+
break;
9366+
}
9367+
}
9368+
break;
9369+
}
9370+
9371+
(void)messageType; // Avoid pesky compiler warnings-as-errors
9372+
(void)messageSubtype;
9373+
9374+
return &spartn[0];
9375+
}
9376+
90189377
// Get the unique chip ID using UBX-SEC-UNIQID
90199378
// The ID is five bytes on the F9 and M9 (version 1) but six bytes on the M10 (version 2)
90209379
bool DevUBLOXGNSS::getUniqueChipId(UBX_SEC_UNIQID_data_t *data, uint16_t maxWait)

‎src/u-blox_GNSS.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -464,6 +464,14 @@ class DevUBLOXGNSS
464464
bool setDynamicSPARTNKeys(uint8_t keyLengthBytes1, uint16_t validFromWno1, uint32_t validFromTow1, const uint8_t *key1,
465465
uint8_t keyLengthBytes2, uint16_t validFromWno2, uint32_t validFromTow2, const uint8_t *key2);
466466

467+
// Support for SPARTN parsing
468+
uint8_t uSpartnCrc4(const uint8_t *pU8Msg, size_t size);
469+
uint8_t uSpartnCrc8(const uint8_t *pU8Msg, size_t size);
470+
uint16_t uSpartnCrc16(const uint8_t *pU8Msg, size_t size);
471+
uint32_t uSpartnCrc24(const uint8_t *pU8Msg, size_t size);
472+
uint32_t uSpartnCrc32(const uint8_t *pU8Msg, size_t size);
473+
uint8_t * parseSPARTN(uint8_t incoming, bool &valid, uint16_t &len);
474+
467475
// Get unique chip ID - UBX-SEC-UNIQID
468476
bool getUniqueChipId(UBX_SEC_UNIQID_data_t *data = nullptr, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait); // Get the unique chip ID using UBX_SEC_UNIQID
469477
const char *getUniqueChipIdStr(UBX_SEC_UNIQID_data_t *data = nullptr, uint16_t maxWait = kUBLOXGNSSDefaultMaxWait); // Get the unique chip ID using UBX_SEC_UNIQID

0 commit comments

Comments
 (0)