Skip to content

Commit 6f5c2ad

Browse files
nseidlenseidle
authored
Merge pull request #720 from LeeLeahy2/parser
Break out the GPS message parser
2 parents 36422e7 + d607e05 commit 6f5c2ad

9 files changed

+636
-444
lines changed

‎Firmware/RTK_Surveyor/GpsMessageParser.h

Lines changed: 146 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,146 @@
1+
/*------------------------------------------------------------------------------
2+
GpsMessageParser.h
3+
4+
Constant and routine declarations for the GPS message parser.
5+
------------------------------------------------------------------------------*/
6+
7+
#ifndef __GPS_MESSAGE_PARSER_H__
8+
#define __GPS_MESSAGE_PARSER_H__
9+
10+
#include <stdint.h>
11+
12+
#include "crc24q.h" // 24-bit CRC-24Q cyclic redundancy checksum for RTCM parsing
13+
14+
//----------------------------------------
15+
// Constants
16+
//----------------------------------------
17+
18+
#define PARSE_BUFFER_LENGTH 3000 // Some USB RAWX messages can be > 2k
19+
20+
enum
21+
{
22+
SENTENCE_TYPE_NONE = 0,
23+
24+
// Add new sentence types below in alphabetical order
25+
SENTENCE_TYPE_NMEA,
26+
SENTENCE_TYPE_RTCM,
27+
SENTENCE_TYPE_UBX,
28+
};
29+
30+
//----------------------------------------
31+
// Types
32+
//----------------------------------------
33+
34+
typedef struct _PARSE_STATE *P_PARSE_STATE;
35+
36+
// Parse routine
37+
typedef uint8_t (*PARSE_ROUTINE)(P_PARSE_STATE parse, // Parser state
38+
uint8_t data); // Incoming data byte
39+
40+
// End of message callback routine
41+
typedef void (*PARSE_EOM_CALLBACK)(P_PARSE_STATE parse, // Parser state
42+
uint8_t type); // Message type
43+
44+
typedef struct _PARSE_STATE
45+
{
46+
PARSE_ROUTINE state; // Parser state routine
47+
PARSE_EOM_CALLBACK eomCallback; // End of message callback routine
48+
const char *parserName; // Name of parser
49+
uint32_t crc; // RTCM computed CRC
50+
uint32_t rtcmCrc; // Computed CRC value for the RTCM message
51+
uint32_t invalidRtcmCrcs; // Number of bad RTCM CRCs detected
52+
uint16_t bytesRemaining; // Bytes remaining in RTCM CRC calculation
53+
uint16_t length; // Message length including line termination
54+
uint16_t maxLength; // Maximum message length including line termination
55+
uint16_t message; // RTCM message number
56+
uint16_t nmeaLength; // Length of the NMEA message without line termination
57+
uint8_t buffer[PARSE_BUFFER_LENGTH]; // Buffer containing the message
58+
uint8_t nmeaMessageName[16]; // Message name
59+
uint8_t nmeaMessageNameLength; // Length of the message name
60+
uint8_t ck_a; // U-blox checksum byte 1
61+
uint8_t ck_b; // U-blox checksum byte 2
62+
bool computeCrc; // Compute the CRC when true
63+
} PARSE_STATE;
64+
65+
//----------------------------------------
66+
// Macros
67+
//----------------------------------------
68+
69+
#ifdef PARSE_NMEA_MESSAGES
70+
#define NMEA_PREAMBLE nmeaPreamble,
71+
#else
72+
#define NMEA_PREAMBLE
73+
#endif // PARSE_NMEA_MESSAGES
74+
75+
#ifdef PARSE_RTCM_MESSAGES
76+
#define RTCM_PREAMBLE rtcmPreamble,
77+
#else
78+
#define RTCM_PREAMBLE
79+
#endif // PARSE_RTCM_MESSAGES
80+
81+
#ifdef PARSE_UBLOX_MESSAGES
82+
#define UBLOX_PREAMBLE ubloxPreamble,
83+
#else
84+
#define UBLOX_PREAMBLE
85+
#endif // PARSE_UBLOX_MESSAGES
86+
87+
#define GPS_PARSE_TABLE \
88+
PARSE_ROUTINE const gpsParseTable[] = \
89+
{ \
90+
NMEA_PREAMBLE \
91+
RTCM_PREAMBLE \
92+
UBLOX_PREAMBLE \
93+
}; \
94+
\
95+
const int gpsParseTableEntries = sizeof(gpsParseTable) / sizeof(gpsParseTable[0]);
96+
97+
//----------------------------------------
98+
// External values
99+
//----------------------------------------
100+
101+
extern PARSE_ROUTINE const gpsParseTable[];
102+
extern const int gpsParseTableEntries;
103+
104+
//----------------------------------------
105+
// External routines
106+
//----------------------------------------
107+
108+
// Main parser routine
109+
uint8_t gpsMessageParserFirstByte(PARSE_STATE *parse, uint8_t data);
110+
111+
// NMEA parse routines
112+
uint8_t nmeaPreamble(PARSE_STATE *parse, uint8_t data);
113+
uint8_t nmeaFindFirstComma(PARSE_STATE *parse, uint8_t data);
114+
uint8_t nmeaFindAsterisk(PARSE_STATE *parse, uint8_t data);
115+
uint8_t nmeaChecksumByte1(PARSE_STATE *parse, uint8_t data);
116+
uint8_t nmeaChecksumByte2(PARSE_STATE *parse, uint8_t data);
117+
uint8_t nmeaLineTermination(PARSE_STATE *parse, uint8_t data);
118+
119+
// RTCM parse routines
120+
uint8_t rtcmPreamble(PARSE_STATE *parse, uint8_t data);
121+
uint8_t rtcmReadLength1(PARSE_STATE *parse, uint8_t data);
122+
uint8_t rtcmReadLength2(PARSE_STATE *parse, uint8_t data);
123+
uint8_t rtcmReadMessage1(PARSE_STATE *parse, uint8_t data);
124+
uint8_t rtcmReadMessage2(PARSE_STATE *parse, uint8_t data);
125+
uint8_t rtcmReadData(PARSE_STATE *parse, uint8_t data);
126+
uint8_t rtcmReadCrc(PARSE_STATE *parse, uint8_t data);
127+
128+
// u-blox parse routines
129+
uint8_t ubloxPreamble(PARSE_STATE *parse, uint8_t data);
130+
uint8_t ubloxSync2(PARSE_STATE *parse, uint8_t data);
131+
uint8_t ubloxClass(PARSE_STATE *parse, uint8_t data);
132+
uint8_t ubloxId(PARSE_STATE *parse, uint8_t data);
133+
uint8_t ubloxLength1(PARSE_STATE *parse, uint8_t data);
134+
uint8_t ubloxLength2(PARSE_STATE *parse, uint8_t data);
135+
uint8_t ubloxPayload(PARSE_STATE *parse, uint8_t data);
136+
uint8_t ubloxCkA(PARSE_STATE *parse, uint8_t data);
137+
uint8_t ubloxCkB(PARSE_STATE *parse, uint8_t data);
138+
139+
// External print routines
140+
void printNmeaChecksumError(PARSE_STATE *parse);
141+
void printRtcmChecksumError(PARSE_STATE *parse);
142+
void printRtcmMaxLength(PARSE_STATE *parse);
143+
void printUbloxChecksumError(PARSE_STATE *parse);
144+
void printUbloxInvalidData(PARSE_STATE *parse);
145+
146+
#endif // __GPS_MESSAGE_PARSER_H__

‎Firmware/RTK_Surveyor/GpsMessageParser.ino

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
/*------------------------------------------------------------------------------
2+
GpsMessageParser.ino
3+
4+
Parse messages from GPS radios
5+
------------------------------------------------------------------------------*/
6+
7+
// Wait for the first byte in the GPS message
8+
uint8_t gpsMessageParserFirstByte(PARSE_STATE *parse, uint8_t data)
9+
{
10+
int index;
11+
PARSE_ROUTINE parseRoutine;
12+
uint8_t sentenceType;
13+
14+
// Walk through the parse table
15+
for (index = 0; index < gpsParseTableEntries; index++)
16+
{
17+
parseRoutine = gpsParseTable[index];
18+
sentenceType = parseRoutine(parse, data);
19+
if (sentenceType)
20+
return sentenceType;
21+
}
22+
23+
// preamble byte not found
24+
parse->length = 0;
25+
parse->state = gpsMessageParserFirstByte;
26+
return SENTENCE_TYPE_NONE;
27+
}

‎Firmware/RTK_Surveyor/Parse_NMEA.ino

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
/*------------------------------------------------------------------------------
2+
Parse_NMEA.ino
3+
4+
NMEA message parsing support routines
5+
------------------------------------------------------------------------------*/
6+
7+
//
8+
// NMEA Message
9+
//
10+
// +----------+---------+--------+---------+----------+----------+
11+
// | Preamble | Name | Comma | Data | Asterisk | Checksum |
12+
// | 8 bits | n bytes | 8 bits | n bytes | 8 bits | 2 bytes |
13+
// | $ | | , | | | |
14+
// +----------+---------+--------+---------+----------+----------+
15+
// | |
16+
// |<-------- Checksum -------->|
17+
//
18+
19+
// Check for the preamble
20+
uint8_t nmeaPreamble(PARSE_STATE *parse, uint8_t data)
21+
{
22+
if (data == '$')
23+
{
24+
parse->crc = 0;
25+
parse->computeCrc = false;
26+
parse->nmeaMessageNameLength = 0;
27+
parse->state = nmeaFindFirstComma;
28+
return SENTENCE_TYPE_NMEA;
29+
}
30+
return SENTENCE_TYPE_NONE;
31+
}
32+
33+
// Read the message name
34+
uint8_t nmeaFindFirstComma(PARSE_STATE *parse, uint8_t data)
35+
{
36+
parse->crc ^= data;
37+
if ((data != ',') || (parse->nmeaMessageNameLength == 0))
38+
{
39+
if ((data < 'A') || (data > 'Z'))
40+
{
41+
parse->length = 0;
42+
parse->buffer[parse->length++] = data;
43+
return gpsMessageParserFirstByte(parse, data);
44+
}
45+
46+
// Save the message name
47+
parse->nmeaMessageName[parse->nmeaMessageNameLength++] = data;
48+
}
49+
else
50+
{
51+
// Zero terminate the message name
52+
parse->nmeaMessageName[parse->nmeaMessageNameLength++] = 0;
53+
parse->state = nmeaFindAsterisk;
54+
}
55+
return SENTENCE_TYPE_NMEA;
56+
}
57+
58+
// Read the message data
59+
uint8_t nmeaFindAsterisk(PARSE_STATE *parse, uint8_t data)
60+
{
61+
if (data != '*')
62+
parse->crc ^= data;
63+
else
64+
parse->state = nmeaChecksumByte1;
65+
return SENTENCE_TYPE_NMEA;
66+
}
67+
68+
// Read the first checksum byte
69+
uint8_t nmeaChecksumByte1(PARSE_STATE *parse, uint8_t data)
70+
{
71+
parse->state = nmeaChecksumByte2;
72+
return SENTENCE_TYPE_NMEA;
73+
}
74+
75+
// Read the second checksum byte
76+
uint8_t nmeaChecksumByte2(PARSE_STATE *parse, uint8_t data)
77+
{
78+
parse->nmeaLength = parse->length;
79+
parse->state = nmeaLineTermination;
80+
return SENTENCE_TYPE_NMEA;
81+
}
82+
83+
// Read the line termination
84+
uint8_t nmeaLineTermination(PARSE_STATE *parse, uint8_t data)
85+
{
86+
int checksum;
87+
88+
// Process the line termination
89+
if ((data != '\r') && (data != '\n'))
90+
{
91+
// Don't include this character in the buffer
92+
parse->length--;
93+
94+
// Convert the checksum characters into binary
95+
checksum = AsciiToNibble(parse->buffer[parse->nmeaLength - 2]) << 4;
96+
checksum |= AsciiToNibble(parse->buffer[parse->nmeaLength - 1]);
97+
98+
// Validate the checksum
99+
if (checksum == parse->crc)
100+
parse->crc = 0;
101+
if (settings.enablePrintBadMessages && parse->crc && (!inMainMenu))
102+
printNmeaChecksumError(parse);
103+
104+
// Process this message if CRC is valid
105+
if (parse->crc == 0)
106+
parse->eomCallback(parse, SENTENCE_TYPE_NMEA);
107+
else
108+
failedParserMessages_NMEA++;
109+
110+
// Add this character to the beginning of the buffer
111+
parse->length = 0;
112+
parse->buffer[parse->length++] = data;
113+
return gpsMessageParserFirstByte(parse, data);
114+
}
115+
return SENTENCE_TYPE_NMEA;
116+
}

0 commit comments

Comments
 (0)