diff --git a/Firmware/RTK_Surveyor/GpsMessageParser.h b/Firmware/RTK_Surveyor/GpsMessageParser.h new file mode 100644 index 000000000..50a32c8ef --- /dev/null +++ b/Firmware/RTK_Surveyor/GpsMessageParser.h @@ -0,0 +1,146 @@ +/*------------------------------------------------------------------------------ +GpsMessageParser.h + + Constant and routine declarations for the GPS message parser. +------------------------------------------------------------------------------*/ + +#ifndef __GPS_MESSAGE_PARSER_H__ +#define __GPS_MESSAGE_PARSER_H__ + +#include + +#include "crc24q.h" // 24-bit CRC-24Q cyclic redundancy checksum for RTCM parsing + +//---------------------------------------- +// Constants +//---------------------------------------- + +#define PARSE_BUFFER_LENGTH 3000 // Some USB RAWX messages can be > 2k + +enum +{ + SENTENCE_TYPE_NONE = 0, + + // Add new sentence types below in alphabetical order + SENTENCE_TYPE_NMEA, + SENTENCE_TYPE_RTCM, + SENTENCE_TYPE_UBX, +}; + +//---------------------------------------- +// Types +//---------------------------------------- + +typedef struct _PARSE_STATE *P_PARSE_STATE; + +// Parse routine +typedef uint8_t (*PARSE_ROUTINE)(P_PARSE_STATE parse, // Parser state + uint8_t data); // Incoming data byte + +// End of message callback routine +typedef void (*PARSE_EOM_CALLBACK)(P_PARSE_STATE parse, // Parser state + uint8_t type); // Message type + +typedef struct _PARSE_STATE +{ + PARSE_ROUTINE state; // Parser state routine + PARSE_EOM_CALLBACK eomCallback; // End of message callback routine + const char *parserName; // Name of parser + uint32_t crc; // RTCM computed CRC + uint32_t rtcmCrc; // Computed CRC value for the RTCM message + uint32_t invalidRtcmCrcs; // Number of bad RTCM CRCs detected + uint16_t bytesRemaining; // Bytes remaining in RTCM CRC calculation + uint16_t length; // Message length including line termination + uint16_t maxLength; // Maximum message length including line termination + uint16_t message; // RTCM message number + uint16_t nmeaLength; // Length of the NMEA message without line termination + uint8_t buffer[PARSE_BUFFER_LENGTH]; // Buffer containing the message + uint8_t nmeaMessageName[16]; // Message name + uint8_t nmeaMessageNameLength; // Length of the message name + uint8_t ck_a; // U-blox checksum byte 1 + uint8_t ck_b; // U-blox checksum byte 2 + bool computeCrc; // Compute the CRC when true +} PARSE_STATE; + +//---------------------------------------- +// Macros +//---------------------------------------- + +#ifdef PARSE_NMEA_MESSAGES +#define NMEA_PREAMBLE nmeaPreamble, +#else +#define NMEA_PREAMBLE +#endif // PARSE_NMEA_MESSAGES + +#ifdef PARSE_RTCM_MESSAGES +#define RTCM_PREAMBLE rtcmPreamble, +#else +#define RTCM_PREAMBLE +#endif // PARSE_RTCM_MESSAGES + +#ifdef PARSE_UBLOX_MESSAGES +#define UBLOX_PREAMBLE ubloxPreamble, +#else +#define UBLOX_PREAMBLE +#endif // PARSE_UBLOX_MESSAGES + +#define GPS_PARSE_TABLE \ +PARSE_ROUTINE const gpsParseTable[] = \ +{ \ + NMEA_PREAMBLE \ + RTCM_PREAMBLE \ + UBLOX_PREAMBLE \ +}; \ + \ +const int gpsParseTableEntries = sizeof(gpsParseTable) / sizeof(gpsParseTable[0]); + +//---------------------------------------- +// External values +//---------------------------------------- + +extern PARSE_ROUTINE const gpsParseTable[]; +extern const int gpsParseTableEntries; + +//---------------------------------------- +// External routines +//---------------------------------------- + +// Main parser routine +uint8_t gpsMessageParserFirstByte(PARSE_STATE *parse, uint8_t data); + +// NMEA parse routines +uint8_t nmeaPreamble(PARSE_STATE *parse, uint8_t data); +uint8_t nmeaFindFirstComma(PARSE_STATE *parse, uint8_t data); +uint8_t nmeaFindAsterisk(PARSE_STATE *parse, uint8_t data); +uint8_t nmeaChecksumByte1(PARSE_STATE *parse, uint8_t data); +uint8_t nmeaChecksumByte2(PARSE_STATE *parse, uint8_t data); +uint8_t nmeaLineTermination(PARSE_STATE *parse, uint8_t data); + +// RTCM parse routines +uint8_t rtcmPreamble(PARSE_STATE *parse, uint8_t data); +uint8_t rtcmReadLength1(PARSE_STATE *parse, uint8_t data); +uint8_t rtcmReadLength2(PARSE_STATE *parse, uint8_t data); +uint8_t rtcmReadMessage1(PARSE_STATE *parse, uint8_t data); +uint8_t rtcmReadMessage2(PARSE_STATE *parse, uint8_t data); +uint8_t rtcmReadData(PARSE_STATE *parse, uint8_t data); +uint8_t rtcmReadCrc(PARSE_STATE *parse, uint8_t data); + +// u-blox parse routines +uint8_t ubloxPreamble(PARSE_STATE *parse, uint8_t data); +uint8_t ubloxSync2(PARSE_STATE *parse, uint8_t data); +uint8_t ubloxClass(PARSE_STATE *parse, uint8_t data); +uint8_t ubloxId(PARSE_STATE *parse, uint8_t data); +uint8_t ubloxLength1(PARSE_STATE *parse, uint8_t data); +uint8_t ubloxLength2(PARSE_STATE *parse, uint8_t data); +uint8_t ubloxPayload(PARSE_STATE *parse, uint8_t data); +uint8_t ubloxCkA(PARSE_STATE *parse, uint8_t data); +uint8_t ubloxCkB(PARSE_STATE *parse, uint8_t data); + +// External print routines +void printNmeaChecksumError(PARSE_STATE *parse); +void printRtcmChecksumError(PARSE_STATE *parse); +void printRtcmMaxLength(PARSE_STATE *parse); +void printUbloxChecksumError(PARSE_STATE *parse); +void printUbloxInvalidData(PARSE_STATE *parse); + +#endif // __GPS_MESSAGE_PARSER_H__ diff --git a/Firmware/RTK_Surveyor/GpsMessageParser.ino b/Firmware/RTK_Surveyor/GpsMessageParser.ino new file mode 100644 index 000000000..74d5d643c --- /dev/null +++ b/Firmware/RTK_Surveyor/GpsMessageParser.ino @@ -0,0 +1,27 @@ +/*------------------------------------------------------------------------------ +GpsMessageParser.ino + + Parse messages from GPS radios +------------------------------------------------------------------------------*/ + +// Wait for the first byte in the GPS message +uint8_t gpsMessageParserFirstByte(PARSE_STATE *parse, uint8_t data) +{ + int index; + PARSE_ROUTINE parseRoutine; + uint8_t sentenceType; + + // Walk through the parse table + for (index = 0; index < gpsParseTableEntries; index++) + { + parseRoutine = gpsParseTable[index]; + sentenceType = parseRoutine(parse, data); + if (sentenceType) + return sentenceType; + } + + // preamble byte not found + parse->length = 0; + parse->state = gpsMessageParserFirstByte; + return SENTENCE_TYPE_NONE; +} diff --git a/Firmware/RTK_Surveyor/Parse_NMEA.ino b/Firmware/RTK_Surveyor/Parse_NMEA.ino new file mode 100644 index 000000000..a05dd0c4d --- /dev/null +++ b/Firmware/RTK_Surveyor/Parse_NMEA.ino @@ -0,0 +1,116 @@ +/*------------------------------------------------------------------------------ +Parse_NMEA.ino + + NMEA message parsing support routines +------------------------------------------------------------------------------*/ + +// +// NMEA Message +// +// +----------+---------+--------+---------+----------+----------+ +// | Preamble | Name | Comma | Data | Asterisk | Checksum | +// | 8 bits | n bytes | 8 bits | n bytes | 8 bits | 2 bytes | +// | $ | | , | | | | +// +----------+---------+--------+---------+----------+----------+ +// | | +// |<-------- Checksum -------->| +// + +// Check for the preamble +uint8_t nmeaPreamble(PARSE_STATE *parse, uint8_t data) +{ + if (data == '$') + { + parse->crc = 0; + parse->computeCrc = false; + parse->nmeaMessageNameLength = 0; + parse->state = nmeaFindFirstComma; + return SENTENCE_TYPE_NMEA; + } + return SENTENCE_TYPE_NONE; +} + +// Read the message name +uint8_t nmeaFindFirstComma(PARSE_STATE *parse, uint8_t data) +{ + parse->crc ^= data; + if ((data != ',') || (parse->nmeaMessageNameLength == 0)) + { + if ((data < 'A') || (data > 'Z')) + { + parse->length = 0; + parse->buffer[parse->length++] = data; + return gpsMessageParserFirstByte(parse, data); + } + + // Save the message name + parse->nmeaMessageName[parse->nmeaMessageNameLength++] = data; + } + else + { + // Zero terminate the message name + parse->nmeaMessageName[parse->nmeaMessageNameLength++] = 0; + parse->state = nmeaFindAsterisk; + } + return SENTENCE_TYPE_NMEA; +} + +// Read the message data +uint8_t nmeaFindAsterisk(PARSE_STATE *parse, uint8_t data) +{ + if (data != '*') + parse->crc ^= data; + else + parse->state = nmeaChecksumByte1; + return SENTENCE_TYPE_NMEA; +} + +// Read the first checksum byte +uint8_t nmeaChecksumByte1(PARSE_STATE *parse, uint8_t data) +{ + parse->state = nmeaChecksumByte2; + return SENTENCE_TYPE_NMEA; +} + +// Read the second checksum byte +uint8_t nmeaChecksumByte2(PARSE_STATE *parse, uint8_t data) +{ + parse->nmeaLength = parse->length; + parse->state = nmeaLineTermination; + return SENTENCE_TYPE_NMEA; +} + +// Read the line termination +uint8_t nmeaLineTermination(PARSE_STATE *parse, uint8_t data) +{ + int checksum; + + // Process the line termination + if ((data != '\r') && (data != '\n')) + { + // Don't include this character in the buffer + parse->length--; + + // Convert the checksum characters into binary + checksum = AsciiToNibble(parse->buffer[parse->nmeaLength - 2]) << 4; + checksum |= AsciiToNibble(parse->buffer[parse->nmeaLength - 1]); + + // Validate the checksum + if (checksum == parse->crc) + parse->crc = 0; + if (settings.enablePrintBadMessages && parse->crc && (!inMainMenu)) + printNmeaChecksumError(parse); + + // Process this message if CRC is valid + if (parse->crc == 0) + parse->eomCallback(parse, SENTENCE_TYPE_NMEA); + else + failedParserMessages_NMEA++; + + // Add this character to the beginning of the buffer + parse->length = 0; + parse->buffer[parse->length++] = data; + return gpsMessageParserFirstByte(parse, data); + } + return SENTENCE_TYPE_NMEA; +} diff --git a/Firmware/RTK_Surveyor/Parse_RTCM.ino b/Firmware/RTK_Surveyor/Parse_RTCM.ino new file mode 100644 index 000000000..e3402c1dd --- /dev/null +++ b/Firmware/RTK_Surveyor/Parse_RTCM.ino @@ -0,0 +1,134 @@ +/*------------------------------------------------------------------------------ +Parse_RTCM.ino + + RTCM message parsing support routines +------------------------------------------------------------------------------*/ + +// +// RTCM Standard 10403.2 - Chapter 4, Transport Layer +// +// |<------------- 3 bytes ------------>|<----- length ----->|<- 3 bytes ->| +// | | | | +// +----------+--------+----------------+---------+----------+-------------+ +// | Preamble | Fill | Message Length | Message | Fill | CRC-24Q | +// | 8 bits | 6 bits | 10 bits | n-bits | 0-7 bits | 24 bits | +// | 0xd3 | 000000 | (in bytes) | | zeros | | +// +----------+--------+----------------+---------+----------+-------------+ +// | | +// |<------------------------ CRC -------------------------->| +// + +// Check for the preamble +uint8_t rtcmPreamble(PARSE_STATE *parse, uint8_t data) +{ + if (data == 0xd3) + { + // Start the CRC with this byte + parse->crc = 0; + parse->crc = COMPUTE_CRC24Q(parse, data); + parse->computeCrc = true; + + // Get the message length + parse->state = rtcmReadLength1; + return SENTENCE_TYPE_RTCM; + } + return SENTENCE_TYPE_NONE; +} + +// Read the upper two bits of the length +uint8_t rtcmReadLength1(PARSE_STATE *parse, uint8_t data) +{ + // Verify the length byte - check the 6 MS bits are all zero + if (data & (~3)) + { + // Invalid length, place this byte at the beginning of the buffer + parse->length = 0; + parse->buffer[parse->length++] = data; + parse->computeCrc = false; + + // Start searching for a preamble byte + return gpsMessageParserFirstByte(parse, data); + } + + // Save the upper 2 bits of the length + parse->bytesRemaining = data << 8; + parse->state = rtcmReadLength2; + return SENTENCE_TYPE_RTCM; +} + +// Read the lower 8 bits of the length +uint8_t rtcmReadLength2(PARSE_STATE *parse, uint8_t data) +{ + parse->bytesRemaining |= data; + parse->state = rtcmReadMessage1; + return SENTENCE_TYPE_RTCM; +} + +// Read the upper 8 bits of the message number +uint8_t rtcmReadMessage1(PARSE_STATE *parse, uint8_t data) +{ + parse->message = data << 4; + parse->bytesRemaining -= 1; + parse->state = rtcmReadMessage2; + return SENTENCE_TYPE_RTCM; +} + +// Read the lower 4 bits of the message number +uint8_t rtcmReadMessage2(PARSE_STATE *parse, uint8_t data) +{ + parse->message |= data >> 4; + parse->bytesRemaining -= 1; + parse->state = rtcmReadData; + return SENTENCE_TYPE_RTCM; +} + +// Read the rest of the message +uint8_t rtcmReadData(PARSE_STATE *parse, uint8_t data) +{ + // Account for this data byte + parse->bytesRemaining -= 1; + + // Wait until all the data is received + if (parse->bytesRemaining <= 0) + { + parse->rtcmCrc = parse->crc & 0x00ffffff; + parse->bytesRemaining = 3; + parse->state = rtcmReadCrc; + } + return SENTENCE_TYPE_RTCM; +} + +// Read the CRC +uint8_t rtcmReadCrc(PARSE_STATE *parse, uint8_t data) +{ + // Account for this data byte + parse->bytesRemaining -= 1; + + // Wait until all the data is received + if (parse->bytesRemaining > 0) + return SENTENCE_TYPE_RTCM; + + // Update the maximum message length + if (parse->length > parse->maxLength) + { + parse->maxLength = parse->length; + printRtcmMaxLength(parse); + } + + // Display the RTCM messages with bad CRC + parse->crc &= 0x00ffffff; + if (settings.enablePrintBadMessages && parse->crc && (!inMainMenu)) + printRtcmChecksumError(parse); + + // Process the message if CRC is valid + if (parse->crc == 0) + parse->eomCallback(parse, SENTENCE_TYPE_RTCM); + else + failedParserMessages_RTCM++; + + // Search for another preamble byte + parse->length = 0; + parse->computeCrc = false; + parse->state = gpsMessageParserFirstByte; + return SENTENCE_TYPE_NONE; +} diff --git a/Firmware/RTK_Surveyor/Parse_UBLOX.ino b/Firmware/RTK_Surveyor/Parse_UBLOX.ino new file mode 100644 index 000000000..3d613acd8 --- /dev/null +++ b/Firmware/RTK_Surveyor/Parse_UBLOX.ino @@ -0,0 +1,155 @@ +/*------------------------------------------------------------------------------ +Parse_UBLOX.ino + + u-blox message parsing support routines +------------------------------------------------------------------------------*/ + +// +// U-BLOX Message +// +// |<-- Preamble --->| +// | | +// +--------+--------+---------+--------+---------+---------+--------+--------+ +// | SYNC | SYNC | Class | ID | Length | Payload | CK_A | CK_B | +// | 8 bits | 8 bits | 8 bits | 8 bits | 2 bytes | n bytes | 8 bits | 8 bits | +// | 0xb5 | 0x62 | | | | | | | +// +--------+--------+---------+--------+---------+---------+--------+--------+ +// | | +// |<------------- Checksum ------------->| +// +// 8-Bit Fletcher Algorithm, which is used in the TCP standard (RFC 1145) +// http://www.ietf.org/rfc/rfc1145.txt +// Checksum calculation +// Initialization: CK_A = CK_B = 0 +// CK_A += data +// CK_B += CK_A +// + +// Check for the preamble +uint8_t ubloxPreamble(PARSE_STATE *parse, uint8_t data) +{ + if (data == 0xb5) + { + parse->state = ubloxSync2; + return SENTENCE_TYPE_UBX; + } + return SENTENCE_TYPE_NONE; +} + +// Read the second sync byte +uint8_t ubloxSync2(PARSE_STATE *parse, uint8_t data) +{ + // Verify the sync 2 byte + if (data != 0x62) + { + // Display the invalid data + if (settings.enablePrintBadMessages && (!inMainMenu)) + printUbloxInvalidData(parse); + + // Invalid sync 2 byte, place this byte at the beginning of the buffer + parse->length = 0; + parse->buffer[parse->length++] = data; + + // Start searching for a preamble byte + return gpsMessageParserFirstByte(parse, data); + } + + parse->state = ubloxClass; + return SENTENCE_TYPE_UBX; +} + +// Read the class byte +uint8_t ubloxClass(PARSE_STATE *parse, uint8_t data) +{ + // Start the checksum calculation + parse->ck_a = data; + parse->ck_b = data; + + // Save the class as the upper 8-bits of the message + parse->message = ((uint16_t)data) << 8; + parse->state = ubloxId; + return SENTENCE_TYPE_UBX; +} + +// Read the ID byte +uint8_t ubloxId(PARSE_STATE *parse, uint8_t data) +{ + // Calculate the checksum + parse->ck_a += data; + parse->ck_b += parse->ck_a; + + // Save the ID as the lower 8-bits of the message + parse->message |= data; + parse->state = ubloxLength1; + return SENTENCE_TYPE_UBX; +} + +// Read the first length byte +uint8_t ubloxLength1(PARSE_STATE *parse, uint8_t data) +{ + // Calculate the checksum + parse->ck_a += data; + parse->ck_b += parse->ck_a; + + // Save the first length byte + parse->bytesRemaining = data; + parse->state = ubloxLength2; + return SENTENCE_TYPE_UBX; +} + +// Read the second length byte +uint8_t ubloxLength2(PARSE_STATE *parse, uint8_t data) +{ + // Calculate the checksum + parse->ck_a += data; + parse->ck_b += parse->ck_a; + + // Save the second length byte + parse->bytesRemaining |= ((uint16_t)data) << 8; + parse->state = ubloxPayload; + return SENTENCE_TYPE_UBX; +} + +// Read the payload +uint8_t ubloxPayload(PARSE_STATE *parse, uint8_t data) +{ + // Compute the checksum over the payload + if (parse->bytesRemaining--) + { + // Calculate the checksum + parse->ck_a += data; + parse->ck_b += parse->ck_a; + return SENTENCE_TYPE_UBX; + } + return ubloxCkA(parse, data); +} + +// Read the CK_A byte +uint8_t ubloxCkA(PARSE_STATE *parse, uint8_t data) +{ + parse->state = ubloxCkB; + return SENTENCE_TYPE_UBX; +} + +// Read the CK_B byte +uint8_t ubloxCkB(PARSE_STATE *parse, uint8_t data) +{ + bool badChecksum; + + // Validate the checksum + badChecksum = + ((parse->buffer[parse->length - 2] != parse->ck_a) || (parse->buffer[parse->length - 1] != parse->ck_b)); + if (settings.enablePrintBadMessages && badChecksum && (!inMainMenu)) + printUbloxChecksumError(parse); + + // Process this message if checksum is valid + if (badChecksum == false) + parse->eomCallback(parse, SENTENCE_TYPE_UBX); + else + failedParserMessages_UBX++; + + // Search for the next preamble byte + parse->length = 0; + parse->state = gpsMessageParserFirstByte; + return SENTENCE_TYPE_NONE; +} diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino index d7a390d97..52867e4d6 100644 --- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino +++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino @@ -60,7 +60,6 @@ // the minor firmware version #define RTK_IDENTIFIER (FIRMWARE_VERSION_MAJOR * 0x10 + FIRMWARE_VERSION_MINOR) -#include "crc24q.h" //24-bit CRC-24Q cyclic redundancy checksum for RTCM parsing #include "settings.h" #define MAX_CPU_CORES 2 @@ -259,11 +258,6 @@ int wifiOriginalMaxConnectionAttempts = wifiMaxConnectionAttempts; // Modified d //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= #include //http://librarymanager/All#SparkFun_u-blox_GNSS_v3 v3.0.5 -#define SENTENCE_TYPE_NMEA DevUBLOXGNSS::SFE_UBLOX_SENTENCE_TYPE_NMEA -#define SENTENCE_TYPE_NONE DevUBLOXGNSS::SFE_UBLOX_SENTENCE_TYPE_NONE -#define SENTENCE_TYPE_RTCM DevUBLOXGNSS::SFE_UBLOX_SENTENCE_TYPE_RTCM -#define SENTENCE_TYPE_UBX DevUBLOXGNSS::SFE_UBLOX_SENTENCE_TYPE_UBX - char zedFirmwareVersion[20]; // The string looks like 'HPG 1.12'. Output to system status menu and settings file. char neoFirmwareVersion[20]; // Output to system status menu. uint8_t zedFirmwareVersionInt = 0; // Controls which features (constellations) can be configured (v1.12 doesn't support @@ -383,6 +377,21 @@ bool lBandCommunicationEnabled = false; unsigned long rtcmLastPacketReceived = 0; //Monitors the last time we received RTCM. Proctors PMP vs RTCM prioritization. //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= +// GPS parse table +//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= +// Define the parsers that get included +#define PARSE_NMEA_MESSAGES +#define PARSE_RTCM_MESSAGES +#define PARSE_UBLOX_MESSAGES + +// Build the GPS_PARSE_TABLE macro +#include "GpsMessageParser.h" // Include the parser + +// Create the GPS message parse table instance +GPS_PARSE_TABLE; + +//=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= + // Battery fuel gauge and PWM LEDs //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= #include //Click here to get the library: http://librarymanager/All#SparkFun_MAX1704x_Fuel_Gauge_Arduino_Library diff --git a/Firmware/RTK_Surveyor/Tasks.ino b/Firmware/RTK_Surveyor/Tasks.ino index e3ab05ebe..7bae303c3 100644 --- a/Firmware/RTK_Surveyor/Tasks.ino +++ b/Firmware/RTK_Surveyor/Tasks.ino @@ -16,7 +16,7 @@ Tasks.ino '------->+<-------' | | gnssReadTask - | waitForPreamble + | gpsMessageParserFirstByte | ... | processUart1Message | @@ -296,7 +296,7 @@ void feedWdt() // time. void gnssReadTask(void *e) { - static PARSE_STATE parse = {waitForPreamble, processUart1Message, "Log"}; + static PARSE_STATE parse = {gpsMessageParserFirstByte, processUart1Message, "Log"}; uint8_t incomingData = 0; diff --git a/Firmware/RTK_Surveyor/settings.h b/Firmware/RTK_Surveyor/settings.h index 5e0ce15bc..315b8a1e4 100644 --- a/Firmware/RTK_Surveyor/settings.h +++ b/Firmware/RTK_Surveyor/settings.h @@ -285,39 +285,6 @@ typedef struct WiFiNetwork typedef uint16_t RING_BUFFER_OFFSET; -typedef struct _PARSE_STATE *P_PARSE_STATE; - -// Parse routine -typedef uint8_t (*PARSE_ROUTINE)(P_PARSE_STATE parse, // Parser state - uint8_t data); // Incoming data byte - -// End of message callback routine -typedef void (*EOM_CALLBACK)(P_PARSE_STATE parse, // Parser state - uint8_t type); // Message type - -#define PARSE_BUFFER_LENGTH 3000 // Some USB RAWX messages can be > 2k - -typedef struct _PARSE_STATE -{ - PARSE_ROUTINE state; // Parser state routine - EOM_CALLBACK eomCallback; // End of message callback routine - const char *parserName; // Name of parser - uint32_t crc; // RTCM computed CRC - uint32_t rtcmCrc; // Computed CRC value for the RTCM message - uint32_t invalidRtcmCrcs; // Number of bad RTCM CRCs detected - uint16_t bytesRemaining; // Bytes remaining in RTCM CRC calculation - uint16_t length; // Message length including line termination - uint16_t maxLength; // Maximum message length including line termination - uint16_t message; // RTCM message number - uint16_t nmeaLength; // Length of the NMEA message without line termination - uint8_t buffer[PARSE_BUFFER_LENGTH]; // Buffer containing the message - uint8_t nmeaMessageName[16]; // Message name - uint8_t nmeaMessageNameLength; // Length of the message name - uint8_t ck_a; // U-blox checksum byte 1 - uint8_t ck_b; // U-blox checksum byte 2 - bool computeCrc; // Compute the CRC when true -} PARSE_STATE; - typedef enum { ETH_NOT_STARTED = 0, diff --git a/Firmware/RTK_Surveyor/support.ino b/Firmware/RTK_Surveyor/support.ino index f0af90095..da9bd68c3 100644 --- a/Firmware/RTK_Surveyor/support.ino +++ b/Firmware/RTK_Surveyor/support.ino @@ -721,409 +721,6 @@ void dumpBuffer(uint8_t *buffer, uint16_t length) } } -// Read the line termination -uint8_t nmeaLineTermination(PARSE_STATE *parse, uint8_t data) -{ - int checksum; - - // Process the line termination - if ((data != '\r') && (data != '\n')) - { - // Don't include this character in the buffer - parse->length--; - - // Convert the checksum characters into binary - checksum = AsciiToNibble(parse->buffer[parse->nmeaLength - 2]) << 4; - checksum |= AsciiToNibble(parse->buffer[parse->nmeaLength - 1]); - - // Validate the checksum - if (checksum == parse->crc) - parse->crc = 0; - if (settings.enablePrintBadMessages && parse->crc && (!inMainMenu)) - { - printTimeStamp(); - systemPrintf(" %s NMEA %s, %2d bytes, bad checksum, expecting 0x%c%c, computed: 0x%02x\r\n", - parse->parserName, parse->nmeaMessageName, parse->length, parse->buffer[parse->nmeaLength - 2], - parse->buffer[parse->nmeaLength - 1], parse->crc); - } - - // Process this message if CRC is valid - if (parse->crc == 0) - parse->eomCallback(parse, SENTENCE_TYPE_NMEA); - else - failedParserMessages_NMEA++; - - // Add this character to the beginning of the buffer - parse->length = 0; - parse->buffer[parse->length++] = data; - return waitForPreamble(parse, data); - } - return SENTENCE_TYPE_NMEA; -} - -// Read the second checksum byte -uint8_t nmeaChecksumByte2(PARSE_STATE *parse, uint8_t data) -{ - parse->nmeaLength = parse->length; - parse->state = nmeaLineTermination; - return SENTENCE_TYPE_NMEA; -} - -// Read the first checksum byte -uint8_t nmeaChecksumByte1(PARSE_STATE *parse, uint8_t data) -{ - parse->state = nmeaChecksumByte2; - return SENTENCE_TYPE_NMEA; -} - -// Read the message data -uint8_t nmeaFindAsterisk(PARSE_STATE *parse, uint8_t data) -{ - if (data != '*') - parse->crc ^= data; - else - parse->state = nmeaChecksumByte1; - return SENTENCE_TYPE_NMEA; -} - -// Read the message name -uint8_t nmeaFindFirstComma(PARSE_STATE *parse, uint8_t data) -{ - parse->crc ^= data; - if ((data != ',') || (parse->nmeaMessageNameLength == 0)) - { - if ((data < 'A') || (data > 'Z')) - { - parse->length = 0; - parse->buffer[parse->length++] = data; - return waitForPreamble(parse, data); - } - - // Save the message name - parse->nmeaMessageName[parse->nmeaMessageNameLength++] = data; - } - else - { - // Zero terminate the message name - parse->nmeaMessageName[parse->nmeaMessageNameLength++] = 0; - parse->state = nmeaFindAsterisk; - } - return SENTENCE_TYPE_NMEA; -} - -// Read the CRC -uint8_t rtcmReadCrc(PARSE_STATE *parse, uint8_t data) -{ - // Account for this data byte - parse->bytesRemaining -= 1; - - // Wait until all the data is received - if (parse->bytesRemaining > 0) - return SENTENCE_TYPE_RTCM; - - // Update the maximum message length - if (parse->length > parse->maxLength) - { - parse->maxLength = parse->length; - systemPrintf("RTCM parser error maxLength: %d bytes\r\n", parse->maxLength); - } - - // Display the RTCM messages with bad CRC - parse->crc &= 0x00ffffff; - if (settings.enablePrintBadMessages && parse->crc && (!inMainMenu)) - { - printTimeStamp(); - systemPrintf(" %s RTCM %d, %2d bytes, bad CRC, expecting 0x%02x%02x%02x, computed: 0x%06x\r\n", - parse->parserName, parse->message, parse->length, parse->buffer[parse->length - 3], - parse->buffer[parse->length - 2], parse->buffer[parse->length - 1], parse->rtcmCrc); - } - - // Process the message if CRC is valid - if (parse->crc == 0) - parse->eomCallback(parse, SENTENCE_TYPE_RTCM); - else - failedParserMessages_RTCM++; - - // Search for another preamble byte - parse->length = 0; - parse->computeCrc = false; - parse->state = waitForPreamble; - return SENTENCE_TYPE_NONE; -} - -// Read the rest of the message -uint8_t rtcmReadData(PARSE_STATE *parse, uint8_t data) -{ - // Account for this data byte - parse->bytesRemaining -= 1; - - // Wait until all the data is received - if (parse->bytesRemaining <= 0) - { - parse->rtcmCrc = parse->crc & 0x00ffffff; - parse->bytesRemaining = 3; - parse->state = rtcmReadCrc; - } - return SENTENCE_TYPE_RTCM; -} - -// Read the lower 4 bits of the message number -uint8_t rtcmReadMessage2(PARSE_STATE *parse, uint8_t data) -{ - parse->message |= data >> 4; - parse->bytesRemaining -= 1; - parse->state = rtcmReadData; - return SENTENCE_TYPE_RTCM; -} - -// Read the upper 8 bits of the message number -uint8_t rtcmReadMessage1(PARSE_STATE *parse, uint8_t data) -{ - parse->message = data << 4; - parse->bytesRemaining -= 1; - parse->state = rtcmReadMessage2; - return SENTENCE_TYPE_RTCM; -} - -// Read the lower 8 bits of the length -uint8_t rtcmReadLength2(PARSE_STATE *parse, uint8_t data) -{ - parse->bytesRemaining |= data; - parse->state = rtcmReadMessage1; - return SENTENCE_TYPE_RTCM; -} - -// Read the upper two bits of the length -uint8_t rtcmReadLength1(PARSE_STATE *parse, uint8_t data) -{ - // Verify the length byte - check the 6 MS bits are all zero - if (data & (~3)) - { - // Invalid length, place this byte at the beginning of the buffer - parse->length = 0; - parse->buffer[parse->length++] = data; - parse->computeCrc = false; - - // Start searching for a preamble byte - return waitForPreamble(parse, data); - } - - // Save the upper 2 bits of the length - parse->bytesRemaining = data << 8; - parse->state = rtcmReadLength2; - return SENTENCE_TYPE_RTCM; -} - -// Read the CK_B byte -uint8_t ubloxCkB(PARSE_STATE *parse, uint8_t data) -{ - bool badChecksum; - - // Validate the checksum - badChecksum = - ((parse->buffer[parse->length - 2] != parse->ck_a) || (parse->buffer[parse->length - 1] != parse->ck_b)); - if (settings.enablePrintBadMessages && badChecksum && (!inMainMenu)) - { - printTimeStamp(); - systemPrintf(" %s u-blox %d.%d, %2d bytes, bad checksum, expecting 0x%02X%02X, computed: 0x%02X%02X\r\n", - parse->parserName, parse->message >> 8, parse->message & 0xff, parse->length, - parse->buffer[parse->nmeaLength - 2], parse->buffer[parse->nmeaLength - 1], parse->ck_a, - parse->ck_b); - } - - // Process this message if checksum is valid - if (badChecksum == false) - parse->eomCallback(parse, SENTENCE_TYPE_UBX); - else - failedParserMessages_UBX++; - - // Search for the next preamble byte - parse->length = 0; - parse->state = waitForPreamble; - return SENTENCE_TYPE_NONE; -} - -// Read the CK_A byte -uint8_t ubloxCkA(PARSE_STATE *parse, uint8_t data) -{ - parse->state = ubloxCkB; - return SENTENCE_TYPE_UBX; -} - -// Read the payload -uint8_t ubloxPayload(PARSE_STATE *parse, uint8_t data) -{ - // Compute the checksum over the payload - if (parse->bytesRemaining--) - { - // Calculate the checksum - parse->ck_a += data; - parse->ck_b += parse->ck_a; - return SENTENCE_TYPE_UBX; - } - return ubloxCkA(parse, data); -} - -// Read the second length byte -uint8_t ubloxLength2(PARSE_STATE *parse, uint8_t data) -{ - // Calculate the checksum - parse->ck_a += data; - parse->ck_b += parse->ck_a; - - // Save the second length byte - parse->bytesRemaining |= ((uint16_t)data) << 8; - parse->state = ubloxPayload; - return SENTENCE_TYPE_UBX; -} - -// Read the first length byte -uint8_t ubloxLength1(PARSE_STATE *parse, uint8_t data) -{ - // Calculate the checksum - parse->ck_a += data; - parse->ck_b += parse->ck_a; - - // Save the first length byte - parse->bytesRemaining = data; - parse->state = ubloxLength2; - return SENTENCE_TYPE_UBX; -} - -// Read the ID byte -uint8_t ubloxId(PARSE_STATE *parse, uint8_t data) -{ - // Calculate the checksum - parse->ck_a += data; - parse->ck_b += parse->ck_a; - - // Save the ID as the lower 8-bits of the message - parse->message |= data; - parse->state = ubloxLength1; - return SENTENCE_TYPE_UBX; -} - -// Read the class byte -uint8_t ubloxClass(PARSE_STATE *parse, uint8_t data) -{ - // Start the checksum calculation - parse->ck_a = data; - parse->ck_b = data; - - // Save the class as the upper 8-bits of the message - parse->message = ((uint16_t)data) << 8; - parse->state = ubloxId; - return SENTENCE_TYPE_UBX; -} - -// Read the second sync byte -uint8_t ubloxSync2(PARSE_STATE *parse, uint8_t data) -{ - // Verify the sync 2 byte - if (data != 0x62) - { - // Display the invalid data - if (settings.enablePrintBadMessages && (!inMainMenu)) - { - dumpBuffer(parse->buffer, parse->length - 1); - systemPrintf(" %s Invalid UBX data, %d bytes\r\n", parse->parserName, parse->length - 1); - } - // Invalid sync 2 byte, place this byte at the beginning of the buffer - parse->length = 0; - parse->buffer[parse->length++] = data; - - // Start searching for a preamble byte - return waitForPreamble(parse, data); - } - - parse->state = ubloxClass; - return SENTENCE_TYPE_UBX; -} - -// Wait for the preamble byte (0xd3) -uint8_t waitForPreamble(PARSE_STATE *parse, uint8_t data) -{ - // Verify that this is the preamble byte - switch (data) - { - case '$': - - // - // NMEA Message - // - // +----------+---------+--------+---------+----------+----------+ - // | Preamble | Name | Comma | Data | Asterisk | Checksum | - // | 8 bits | n bytes | 8 bits | n bytes | 8 bits | 2 bytes | - // | $ | | , | | | | - // +----------+---------+--------+---------+----------+----------+ - // | | - // |<-------- Checksum -------->| - // - - parse->crc = 0; - parse->computeCrc = false; - parse->nmeaMessageNameLength = 0; - parse->state = nmeaFindFirstComma; - return SENTENCE_TYPE_NMEA; - - case 0xb5: - - // - // U-BLOX Message - // - // |<-- Preamble --->| - // | | - // +--------+--------+---------+--------+---------+---------+--------+--------+ - // | SYNC | SYNC | Class | ID | Length | Payload | CK_A | CK_B | - // | 8 bits | 8 bits | 8 bits | 8 bits | 2 bytes | n bytes | 8 bits | 8 bits | - // | 0xb5 | 0x62 | | | | | | | - // +--------+--------+---------+--------+---------+---------+--------+--------+ - // | | - // |<------------- Checksum ------------->| - // - // 8-Bit Fletcher Algorithm, which is used in the TCP standard (RFC 1145) - // http://www.ietf.org/rfc/rfc1145.txt - // Checksum calculation - // Initialization: CK_A = CK_B = 0 - // CK_A += data - // CK_B += CK_A - // - - parse->state = ubloxSync2; - return SENTENCE_TYPE_UBX; - - case 0xd3: - - // - // RTCM Standard 10403.2 - Chapter 4, Transport Layer - // - // |<------------- 3 bytes ------------>|<----- length ----->|<- 3 bytes ->| - // | | | | - // +----------+--------+----------------+---------+----------+-------------+ - // | Preamble | Fill | Message Length | Message | Fill | CRC-24Q | - // | 8 bits | 6 bits | 10 bits | n-bits | 0-7 bits | 24 bits | - // | 0xd3 | 000000 | (in bytes) | | zeros | | - // +----------+--------+----------------+---------+----------+-------------+ - // | | - // |<------------------------ CRC -------------------------->| - // - - // Start the CRC with this byte - parse->crc = 0; - parse->crc = COMPUTE_CRC24Q(parse, data); - parse->computeCrc = true; - - // Get the message length - parse->state = rtcmReadLength1; - return SENTENCE_TYPE_RTCM; - } - - // preamble byte not found - parse->length = 0; - parse->state = waitForPreamble; - return SENTENCE_TYPE_NONE; -} - // Make size of files human readable void stringHumanReadableSize(String &returnText, uint64_t bytes) { @@ -1161,6 +758,47 @@ void stringHumanReadableSize(String &returnText, uint64_t bytes) returnText = String(readableSize); } +// Print the NMEA checksum error +void printNmeaChecksumError(PARSE_STATE *parse) +{ + printTimeStamp(); + systemPrintf(" %s NMEA %s, %2d bytes, bad checksum, expecting 0x%c%c, computed: 0x%02x\r\n", + parse->parserName, parse->nmeaMessageName, parse->length, parse->buffer[parse->nmeaLength - 2], + parse->buffer[parse->nmeaLength - 1], parse->crc); +} + +// Print the RTCM checksum error +void printRtcmChecksumError(PARSE_STATE *parse) +{ + printTimeStamp(); + systemPrintf(" %s RTCM %d, %2d bytes, bad CRC, expecting 0x%02x%02x%02x, computed: 0x%06x\r\n", + parse->parserName, parse->message, parse->length, parse->buffer[parse->length - 3], + parse->buffer[parse->length - 2], parse->buffer[parse->length - 1], parse->rtcmCrc); +} + +// Print the RTCM maximum length +void printRtcmMaxLength(PARSE_STATE *parse) +{ + systemPrintf("RTCM parser error maxLength: %d bytes\r\n", parse->maxLength); +} + +// Print the u-blox checksum error +void printUbloxChecksumError(PARSE_STATE *parse) +{ + printTimeStamp(); + systemPrintf(" %s u-blox %d.%d, %2d bytes, bad checksum, expecting 0x%02X%02X, computed: 0x%02X%02X\r\n", + parse->parserName, parse->message >> 8, parse->message & 0xff, parse->length, + parse->buffer[parse->nmeaLength - 2], parse->buffer[parse->nmeaLength - 1], parse->ck_a, + parse->ck_b); +} + +// Print the u-blox invalid data error +void printUbloxInvalidData(PARSE_STATE *parse) +{ + dumpBuffer(parse->buffer, parse->length - 1); + systemPrintf(" %s Invalid UBX data, %d bytes\r\n", parse->parserName, parse->length - 1); +} + // Verify table sizes match enum definitions void verifyTables () {