Skip to content

Commit df6a633

Browse files
LeeLeahy2LeeLeahy2
committed
Add common NMEA and RTCM message parser
1 parent 45e5d75 commit df6a633

File tree

2 files changed

+224
-0
lines changed

2 files changed

+224
-0
lines changed

‎Firmware/RTK_Surveyor/settings.h‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -149,6 +149,53 @@ enum RtcmTransportState
149149
RTCM_TRANSPORT_STATE_CHECK_CRC
150150
};
151151

152+
//Message parsing state for RTCM and NMEA messages
153+
enum ParseState
154+
{
155+
PARSE_STATE_WAIT_FOR_PREAMBLE = 0,
156+
PARSE_STATE_RTCM_READ_LENGTH_1,
157+
PARSE_STATE_RTCM_READ_LENGTH_2,
158+
PARSE_STATE_RTCM_READ_MESSAGE_1,
159+
PARSE_STATE_RTCM_READ_MESSAGE_2,
160+
PARSE_STATE_RTCM_READ_DATA,
161+
PARSE_STATE_RTCM_READ_CRC_1,
162+
PARSE_STATE_RTCM_READ_CRC_2,
163+
PARSE_STATE_RTCM_READ_CRC_3,
164+
PARSE_STATE_RTCM_CHECK_CRC,
165+
PARSE_STATE_NMEA_FIND_COMMA,
166+
PARSE_STATE_NMEA_FIND_ASTERISK,
167+
PARSE_STATE_NMEA_VERIFY_CHECKSUM_1,
168+
PARSE_STATE_NMEA_VERIFY_CHECKSUM_2
169+
};
170+
171+
typedef struct _PARSE_STATE
172+
{
173+
uint32_t crc;
174+
uint32_t rtcmCrc;
175+
uint32_t rtcmPackets;
176+
uint32_t invalidCharacters;
177+
uint32_t invalidNmeaChecksums;
178+
uint32_t invalidRtcmCrcs;
179+
uint16_t bytesRemaining;
180+
uint16_t length;
181+
uint16_t message;
182+
uint16_t messageNumber;
183+
uint16_t nameLength;
184+
byte checksumByte1;
185+
byte checksumByte2;
186+
byte crcByte[3];
187+
byte nmeaChecksum;
188+
char messageName[16];
189+
char name[16];
190+
bool invalidByte;
191+
bool invalidNmeaChecksum;
192+
bool invalidRtcmCrc;
193+
bool printMessageName;
194+
bool printMessageNumber;
195+
bool sendMessage;
196+
byte state;
197+
} PARSE_STATE;
198+
152199
//Radio status LED goes from off (LED off), no connection (blinking), to connected (solid)
153200
enum BTState
154201
{

‎Firmware/RTK_Surveyor/support.ino‎

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -405,3 +405,180 @@ byte readLine(char* buffer, byte bufferLength, int numberOfSeconds)
405405

406406
return readLength;
407407
}
408+
409+
//Verify the NMEA checksum byte
410+
bool invalidChecksumByte(uint8_t data, uint32_t checksum)
411+
{
412+
//Convert the data into a binary value
413+
if ((data >= 'A') && (data <= 'F'))
414+
data -= 'A' - 10;
415+
else if ((data >= 'a') && (data <= 'f'))
416+
data -= 'a' - 10;
417+
else if ((data >= '0') && (data <= '9'))
418+
data -= '0';
419+
420+
//Compare with the lower 4 bits of the checksum
421+
return ((checksum & 0xf) != data);
422+
}
423+
424+
//Parse the NMEA and RTCM messages
425+
bool parseNmeaAndRtcmMessages(PARSE_STATE * parse, uint8_t data, bool sendMessage)
426+
{
427+
// NMEA
428+
//
429+
// |<--------------- Checksum --------------->|
430+
// | |
431+
// +----------+----------+-- *** --+----------+----------+----------+----------+
432+
// | Preamble | Message | | Asterisk | Checksum | Carriage | Linefeed |
433+
// | 8 bits | Name | | | 2 Hex | | |
434+
// | $ | G..... | | * | Bytes | 0x0d | 0x0a |
435+
// +----------+----------+-- ***---+----------+----------+----------+----------+
436+
//
437+
// RTCM Standard 10403.2 - Chapter 4, Transport Layer
438+
//
439+
// |<------------- 3 bytes ------------>|<----- length ----->|<- 3 bytes ->|
440+
// | | | |
441+
// +----------+--------+----------------+---------+----------+-------------+
442+
// | Preamble | Fill | Message Length | Message | Fill | CRC-24Q |
443+
// | 8 bits | 6 bits | 10 bits | n-bits | 0-7 bits | 24 bits |
444+
// | 0xd3 | 000000 | (in bytes) | | zeros | |
445+
// +----------+--------+----------------+---------+----------+-------------+
446+
// | |
447+
// |<-------------------------------- CRC -------------------------------->|
448+
//
449+
450+
switch (parse->state)
451+
{
452+
//Wait for the preamble byte (0xd3)
453+
case PARSE_STATE_WAIT_FOR_PREAMBLE:
454+
parse->sendMessage = false;
455+
if (data == 0xd3)
456+
{
457+
parse->sendMessage = sendMessage;
458+
parse->state = PARSE_STATE_RTCM_READ_LENGTH_1;
459+
}
460+
else if (data == '$')
461+
{
462+
parse->crc = 0;
463+
parse->length = 1;
464+
parse->nameLength = 0;
465+
parse->sendMessage = sendMessage;
466+
parse->state = PARSE_STATE_NMEA_FIND_COMMA;
467+
}
468+
else if ((data != '\r') && (data != '\n'))
469+
{
470+
parse->invalidByte = true;
471+
parse->invalidCharacters++;
472+
}
473+
break;
474+
475+
//Read the upper two bits of the length
476+
case PARSE_STATE_RTCM_READ_LENGTH_1:
477+
parse->length = data << 8;
478+
parse->state = PARSE_STATE_RTCM_READ_LENGTH_2;
479+
break;
480+
481+
//Read the lower 8 bits of the length
482+
case PARSE_STATE_RTCM_READ_LENGTH_2:
483+
parse->length |= data;
484+
parse->bytesRemaining = parse->length;
485+
parse->state = PARSE_STATE_RTCM_READ_MESSAGE_1;
486+
break;
487+
488+
//Read the upper 8 bits of the message number
489+
case PARSE_STATE_RTCM_READ_MESSAGE_1:
490+
parse->message = data << 4;
491+
parse->bytesRemaining -= 1;
492+
parse->state = PARSE_STATE_RTCM_READ_MESSAGE_2;
493+
break;
494+
495+
//Read the lower 4 bits of the message number
496+
case PARSE_STATE_RTCM_READ_MESSAGE_2:
497+
parse->message |= data >> 4;
498+
parse->bytesRemaining -= 1;
499+
parse->state = PARSE_STATE_RTCM_READ_DATA;
500+
break;
501+
502+
//Read the rest of the message
503+
case PARSE_STATE_RTCM_READ_DATA:
504+
parse->bytesRemaining -= 1;
505+
if (parse->bytesRemaining <= 0)
506+
parse->state = PARSE_STATE_RTCM_READ_CRC_1;
507+
break;
508+
509+
//Read the upper 8 bits of the CRC
510+
case PARSE_STATE_RTCM_READ_CRC_1:
511+
parse->crcByte[0] = data;
512+
parse->state = PARSE_STATE_RTCM_READ_CRC_2;
513+
break;
514+
515+
//Read the middle 8 bits of the CRC
516+
case PARSE_STATE_RTCM_READ_CRC_2:
517+
parse->crcByte[1] = data;
518+
parse->state = PARSE_STATE_RTCM_READ_CRC_3;
519+
break;
520+
521+
//Read the lower 8 bits of the CRC
522+
case PARSE_STATE_RTCM_READ_CRC_3:
523+
parse->crcByte[2] = data;
524+
parse->rtcmCrc = parse->crc;
525+
parse->rtcmPackets++;
526+
parse->messageNumber = parse->message;
527+
parse->printMessageNumber = true;
528+
parse->state = PARSE_STATE_WAIT_FOR_PREAMBLE;
529+
break;
530+
531+
//Check for the end of the message name
532+
case PARSE_STATE_NMEA_FIND_COMMA:
533+
parse->length++;
534+
parse->crc ^= data;
535+
if (data == ',')
536+
{
537+
parse->name[parse->nameLength++] = 0;
538+
parse->state = PARSE_STATE_NMEA_FIND_ASTERISK;
539+
}
540+
else
541+
{
542+
if (parse->nameLength < (sizeof(parse->name) - 1))
543+
parse->name[parse->nameLength++] = data;
544+
}
545+
break;
546+
547+
//Check for the end of the NMEA message
548+
case PARSE_STATE_NMEA_FIND_ASTERISK:
549+
parse->length++;
550+
if (data == '*')
551+
{
552+
parse->nmeaChecksum = (byte)parse->crc;
553+
parse->state = PARSE_STATE_NMEA_VERIFY_CHECKSUM_1;
554+
}
555+
else
556+
parse->crc ^= data;
557+
break;
558+
559+
//Read the ASCII hex character representing the upper 4-bits of the checksum
560+
case PARSE_STATE_NMEA_VERIFY_CHECKSUM_1:
561+
parse->length++;
562+
parse->checksumByte1 = data;
563+
parse->state = PARSE_STATE_NMEA_VERIFY_CHECKSUM_2;
564+
break;
565+
566+
//Read the ASCII hex character representing the lower 4-bits of the checksum
567+
case PARSE_STATE_NMEA_VERIFY_CHECKSUM_2:
568+
parse->length++;
569+
parse->checksumByte2 = data;
570+
if (invalidChecksumByte(parse->checksumByte1, parse->crc >> 4)
571+
|| invalidChecksumByte(data, parse->crc))
572+
{
573+
parse->invalidNmeaChecksum = true;
574+
parse->invalidNmeaChecksums++;
575+
}
576+
parse->state = PARSE_STATE_WAIT_FOR_PREAMBLE;
577+
parse->printMessageName = true;
578+
strcpy(parse->messageName, parse->name);
579+
break;
580+
}
581+
582+
//Let the upper layer know if this message should be sent
583+
return parse->sendMessage && sendMessage;
584+
}

0 commit comments

Comments
 (0)