Skip to content

Commit 813f669

Browse files
nseidlenseidle
authored
Merge pull request #243 from LeeLeahy2/ring-buffer
Validate the RTCM and NMEA message flowing to and from the GNSS
2 parents 10699cb + 8bce3b5 commit 813f669

File tree

6 files changed

+483
-144
lines changed

6 files changed

+483
-144
lines changed

‎Firmware/RTK_Surveyor/Crc24q.ino‎

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
/*
2+
* This is an implementation of the CRC-24Q cyclic redundancy checksum
3+
* used by Qualcomm, RTCM104V3, and PGP 6.5.1. According to the RTCM104V3
4+
* standard, it uses the error polynomial
5+
*
6+
* x^24+ x^23+ x^18+ x^17+ x^14+ x^11+ x^10+ x^7+ x^6+ x^5+ x^4+ x^3+ x+1
7+
*
8+
* This corresponds to a mask of 0x1864CFB. For a primer on CRC theory,
9+
* including detailed discussion of how and why the error polynomial is
10+
* expressed by this mask, see <http://www.ross.net/crc/>.
11+
*
12+
* 1) It detects all single bit errors per 24-bit code word.
13+
* 2) It detects all double bit error combinations in a code word.
14+
* 3) It detects any odd number of errors.
15+
* 4) It detects any burst error for which the length of the burst is less than
16+
* or equal to 24 bits.
17+
* 5) It detects most large error bursts with length greater than 24 bits;
18+
* the odds of a false positive are at most 2^-23.
19+
*
20+
* This hash should not be considered cryptographically secure, but it
21+
* is extremely good at detecting noise errors.
22+
*
23+
* Note that this version has a seed of 0 wired in. The RTCM104V3 standard
24+
* requires this.
25+
*
26+
* This file is Copyright 2008 by the GPSD project
27+
* SPDX-License-Identifier: BSD-2-clause
28+
*/
29+
30+
//This file is originally from: https://gitlab.com/gpsd/gpsd/-/blob/master/gpsd/crc24q.c
31+
32+
static const int unsigned crc24q[256] = {
33+
0x00000000u, 0x01864CFBu, 0x028AD50Du, 0x030C99F6u,
34+
0x0493E6E1u, 0x0515AA1Au, 0x061933ECu, 0x079F7F17u,
35+
0x08A18139u, 0x0927CDC2u, 0x0A2B5434u, 0x0BAD18CFu,
36+
0x0C3267D8u, 0x0DB42B23u, 0x0EB8B2D5u, 0x0F3EFE2Eu,
37+
0x10C54E89u, 0x11430272u, 0x124F9B84u, 0x13C9D77Fu,
38+
0x1456A868u, 0x15D0E493u, 0x16DC7D65u, 0x175A319Eu,
39+
0x1864CFB0u, 0x19E2834Bu, 0x1AEE1ABDu, 0x1B685646u,
40+
0x1CF72951u, 0x1D7165AAu, 0x1E7DFC5Cu, 0x1FFBB0A7u,
41+
0x200CD1E9u, 0x218A9D12u, 0x228604E4u, 0x2300481Fu,
42+
0x249F3708u, 0x25197BF3u, 0x2615E205u, 0x2793AEFEu,
43+
0x28AD50D0u, 0x292B1C2Bu, 0x2A2785DDu, 0x2BA1C926u,
44+
0x2C3EB631u, 0x2DB8FACAu, 0x2EB4633Cu, 0x2F322FC7u,
45+
0x30C99F60u, 0x314FD39Bu, 0x32434A6Du, 0x33C50696u,
46+
0x345A7981u, 0x35DC357Au, 0x36D0AC8Cu, 0x3756E077u,
47+
0x38681E59u, 0x39EE52A2u, 0x3AE2CB54u, 0x3B6487AFu,
48+
0x3CFBF8B8u, 0x3D7DB443u, 0x3E712DB5u, 0x3FF7614Eu,
49+
0x4019A3D2u, 0x419FEF29u, 0x429376DFu, 0x43153A24u,
50+
0x448A4533u, 0x450C09C8u, 0x4600903Eu, 0x4786DCC5u,
51+
0x48B822EBu, 0x493E6E10u, 0x4A32F7E6u, 0x4BB4BB1Du,
52+
0x4C2BC40Au, 0x4DAD88F1u, 0x4EA11107u, 0x4F275DFCu,
53+
0x50DCED5Bu, 0x515AA1A0u, 0x52563856u, 0x53D074ADu,
54+
0x544F0BBAu, 0x55C94741u, 0x56C5DEB7u, 0x5743924Cu,
55+
0x587D6C62u, 0x59FB2099u, 0x5AF7B96Fu, 0x5B71F594u,
56+
0x5CEE8A83u, 0x5D68C678u, 0x5E645F8Eu, 0x5FE21375u,
57+
0x6015723Bu, 0x61933EC0u, 0x629FA736u, 0x6319EBCDu,
58+
0x648694DAu, 0x6500D821u, 0x660C41D7u, 0x678A0D2Cu,
59+
0x68B4F302u, 0x6932BFF9u, 0x6A3E260Fu, 0x6BB86AF4u,
60+
0x6C2715E3u, 0x6DA15918u, 0x6EADC0EEu, 0x6F2B8C15u,
61+
0x70D03CB2u, 0x71567049u, 0x725AE9BFu, 0x73DCA544u,
62+
0x7443DA53u, 0x75C596A8u, 0x76C90F5Eu, 0x774F43A5u,
63+
0x7871BD8Bu, 0x79F7F170u, 0x7AFB6886u, 0x7B7D247Du,
64+
0x7CE25B6Au, 0x7D641791u, 0x7E688E67u, 0x7FEEC29Cu,
65+
0x803347A4u, 0x81B50B5Fu, 0x82B992A9u, 0x833FDE52u,
66+
0x84A0A145u, 0x8526EDBEu, 0x862A7448u, 0x87AC38B3u,
67+
0x8892C69Du, 0x89148A66u, 0x8A181390u, 0x8B9E5F6Bu,
68+
0x8C01207Cu, 0x8D876C87u, 0x8E8BF571u, 0x8F0DB98Au,
69+
0x90F6092Du, 0x917045D6u, 0x927CDC20u, 0x93FA90DBu,
70+
0x9465EFCCu, 0x95E3A337u, 0x96EF3AC1u, 0x9769763Au,
71+
0x98578814u, 0x99D1C4EFu, 0x9ADD5D19u, 0x9B5B11E2u,
72+
0x9CC46EF5u, 0x9D42220Eu, 0x9E4EBBF8u, 0x9FC8F703u,
73+
0xA03F964Du, 0xA1B9DAB6u, 0xA2B54340u, 0xA3330FBBu,
74+
0xA4AC70ACu, 0xA52A3C57u, 0xA626A5A1u, 0xA7A0E95Au,
75+
0xA89E1774u, 0xA9185B8Fu, 0xAA14C279u, 0xAB928E82u,
76+
0xAC0DF195u, 0xAD8BBD6Eu, 0xAE872498u, 0xAF016863u,
77+
0xB0FAD8C4u, 0xB17C943Fu, 0xB2700DC9u, 0xB3F64132u,
78+
0xB4693E25u, 0xB5EF72DEu, 0xB6E3EB28u, 0xB765A7D3u,
79+
0xB85B59FDu, 0xB9DD1506u, 0xBAD18CF0u, 0xBB57C00Bu,
80+
0xBCC8BF1Cu, 0xBD4EF3E7u, 0xBE426A11u, 0xBFC426EAu,
81+
0xC02AE476u, 0xC1ACA88Du, 0xC2A0317Bu, 0xC3267D80u,
82+
0xC4B90297u, 0xC53F4E6Cu, 0xC633D79Au, 0xC7B59B61u,
83+
0xC88B654Fu, 0xC90D29B4u, 0xCA01B042u, 0xCB87FCB9u,
84+
0xCC1883AEu, 0xCD9ECF55u, 0xCE9256A3u, 0xCF141A58u,
85+
0xD0EFAAFFu, 0xD169E604u, 0xD2657FF2u, 0xD3E33309u,
86+
0xD47C4C1Eu, 0xD5FA00E5u, 0xD6F69913u, 0xD770D5E8u,
87+
0xD84E2BC6u, 0xD9C8673Du, 0xDAC4FECBu, 0xDB42B230u,
88+
0xDCDDCD27u, 0xDD5B81DCu, 0xDE57182Au, 0xDFD154D1u,
89+
0xE026359Fu, 0xE1A07964u, 0xE2ACE092u, 0xE32AAC69u,
90+
0xE4B5D37Eu, 0xE5339F85u, 0xE63F0673u, 0xE7B94A88u,
91+
0xE887B4A6u, 0xE901F85Du, 0xEA0D61ABu, 0xEB8B2D50u,
92+
0xEC145247u, 0xED921EBCu, 0xEE9E874Au, 0xEF18CBB1u,
93+
0xF0E37B16u, 0xF16537EDu, 0xF269AE1Bu, 0xF3EFE2E0u,
94+
0xF4709DF7u, 0xF5F6D10Cu, 0xF6FA48FAu, 0xF77C0401u,
95+
0xF842FA2Fu, 0xF9C4B6D4u, 0xFAC82F22u, 0xFB4E63D9u,
96+
0xFCD11CCEu, 0xFD575035u, 0xFE5BC9C3u, 0xFFDD8538u,
97+
};

‎Firmware/RTK_Surveyor/NtripServer.ino‎

Lines changed: 70 additions & 118 deletions
Original file line numberDiff line numberDiff line change
@@ -154,108 +154,6 @@ void ntripServerResponse(char * response, size_t maxLength)
154154
*response = '\0';
155155
}
156156

157-
//Parse the RTCM transport data
158-
bool ntripServerRtcmMessage(uint8_t data)
159-
{
160-
static uint16_t bytesRemaining;
161-
static byte crcState = RTCM_TRANSPORT_STATE_WAIT_FOR_PREAMBLE_D3;
162-
static uint16_t length;
163-
static uint16_t message;
164-
static bool sendMessage = false;
165-
166-
//
167-
// RTCM Standard 10403.2 - Chapter 4, Transport Layer
168-
//
169-
// |<------------- 3 bytes ------------>|<----- length ----->|<- 3 bytes ->|
170-
// | | | |
171-
// +----------+--------+----------------+---------+----------+-------------+
172-
// | Preamble | Fill | Message Length | Message | Fill | CRC-24Q |
173-
// | 8 bits | 6 bits | 10 bits | n-bits | 0-7 bits | 24 bits |
174-
// | 0xd3 | 000000 | (in bytes) | | zeros | |
175-
// +----------+--------+----------------+---------+----------+-------------+
176-
// | |
177-
// |<-------------------------------- CRC -------------------------------->|
178-
//
179-
180-
switch (crcState)
181-
{
182-
//Wait for the preamble byte (0xd3)
183-
case RTCM_TRANSPORT_STATE_WAIT_FOR_PREAMBLE_D3:
184-
sendMessage = false;
185-
if (data == 0xd3)
186-
{
187-
crcState = RTCM_TRANSPORT_STATE_READ_LENGTH_1;
188-
sendMessage = (ntripServerState == NTRIP_SERVER_CASTING);
189-
}
190-
break;
191-
192-
//Read the upper two bits of the length
193-
case RTCM_TRANSPORT_STATE_READ_LENGTH_1:
194-
length = data << 8;
195-
crcState = RTCM_TRANSPORT_STATE_READ_LENGTH_2;
196-
break;
197-
198-
//Read the lower 8 bits of the length
199-
case RTCM_TRANSPORT_STATE_READ_LENGTH_2:
200-
length |= data;
201-
bytesRemaining = length;
202-
crcState = RTCM_TRANSPORT_STATE_READ_MESSAGE_1;
203-
break;
204-
205-
//Read the upper 8 bits of the message number
206-
case RTCM_TRANSPORT_STATE_READ_MESSAGE_1:
207-
message = data << 4;
208-
bytesRemaining -= 1;
209-
crcState = RTCM_TRANSPORT_STATE_READ_MESSAGE_2;
210-
break;
211-
212-
//Read the lower 4 bits of the message number
213-
case RTCM_TRANSPORT_STATE_READ_MESSAGE_2:
214-
message |= data >> 4;
215-
bytesRemaining -= 1;
216-
crcState = RTCM_TRANSPORT_STATE_READ_DATA;
217-
break;
218-
219-
//Read the rest of the message
220-
case RTCM_TRANSPORT_STATE_READ_DATA:
221-
bytesRemaining -= 1;
222-
if (bytesRemaining <= 0)
223-
crcState = RTCM_TRANSPORT_STATE_READ_CRC_1;
224-
break;
225-
226-
//Read the upper 8 bits of the CRC
227-
case RTCM_TRANSPORT_STATE_READ_CRC_1:
228-
crcState = RTCM_TRANSPORT_STATE_READ_CRC_2;
229-
break;
230-
231-
//Read the middle 8 bits of the CRC
232-
case RTCM_TRANSPORT_STATE_READ_CRC_2:
233-
crcState = RTCM_TRANSPORT_STATE_READ_CRC_3;
234-
break;
235-
236-
//Read the lower 8 bits of the CRC
237-
case RTCM_TRANSPORT_STATE_READ_CRC_3:
238-
crcState = RTCM_TRANSPORT_STATE_CHECK_CRC;
239-
break;
240-
}
241-
242-
//Check the CRC
243-
if (crcState == RTCM_TRANSPORT_STATE_CHECK_CRC)
244-
{
245-
crcState = RTCM_TRANSPORT_STATE_WAIT_FOR_PREAMBLE_D3;
246-
247-
//Account for this message
248-
rtcmPacketsSent++;
249-
250-
//Display the RTCM message header
251-
if (settings.enablePrintNtripServerRtcm && (!inMainMenu))
252-
Serial.printf (" Message %d, %2d bytes\r\n", message, 3 + 1 + length + 3);
253-
}
254-
255-
//Let the upper layer know if this message should be sent
256-
return sendMessage && (ntripServerState == NTRIP_SERVER_CASTING);
257-
}
258-
259157
//Update the state of the NTRIP server state machine
260158
void ntripServerSetState(byte newState)
261159
{
@@ -335,29 +233,83 @@ void ntripServerProcessRTCM(uint8_t incoming)
335233
#ifdef COMPILE_WIFI
336234
if (online.rtc)
337235
{
338-
//Timestamp the RTCM messages
339-
currentMilliseconds = millis();
340-
if (settings.enablePrintNtripServerRtcm
341-
&& (!inMainMenu)
342-
&& ((currentMilliseconds - previousMilliseconds) > 1))
343-
{
344-
// 1 2 3
345-
//123456789012345678901234567890
346-
//YYYY-mm-dd HH:MM:SS.xxxrn0
347-
struct tm timeinfo = rtc.getTimeStruct();
348-
char timestamp[30];
349-
strftime(timestamp, sizeof(timestamp), "%Y-%m-%d %H:%M:%S", &timeinfo);
350-
Serial.printf("RTCM: %s.%03ld\r\n", timestamp, rtc.getMillis());
351-
}
352-
previousMilliseconds = currentMilliseconds;
236+
static PARSE_STATE parseState;
353237

354238
//Parse the RTCM message
355-
if (ntripServerRtcmMessage(incoming))
239+
if (parseNmeaAndRtcmMessages(&parseState, incoming, (ntripServerState == NTRIP_SERVER_CASTING)))
356240
{
357241
ntripServer->write(incoming); //Send this byte to socket
358242
ntripServerBytesSent++;
359243
ntripServerTimer = millis();
360244
online.txNtripDataCasting = true;
245+
246+
//Timestamp the RTCM messages
247+
currentMilliseconds = millis();
248+
if (settings.enablePrintNtripServerRtcm
249+
&& parseState.rtcmPackets
250+
&& (!inMainMenu)
251+
&& ((currentMilliseconds - previousMilliseconds) >= 1000))
252+
{
253+
// 1 2 3
254+
//123456789012345678901234567890
255+
//YYYY-mm-dd HH:MM:SS.xxxrn0
256+
struct tm timeinfo = rtc.getTimeStruct();
257+
char timestamp[30];
258+
strftime(timestamp, sizeof(timestamp), "%Y-%m-%d %H:%M:%S", &timeinfo);
259+
Serial.printf("RTCM: %s.%03ld\r\n", timestamp, rtc.getMillis());
260+
previousMilliseconds += 1000;
261+
}
262+
263+
//Update the RTCM message count
264+
rtcmPacketsSent += parseState.rtcmPackets;
265+
parseState.rtcmPackets = 0;
266+
267+
//Display the RTCM message header
268+
if (settings.enablePrintNtripServerRtcm && (!inMainMenu))
269+
{
270+
if (parseState.invalidByte)
271+
{
272+
Serial.printf (" Invalid byte: 0x%02x\r\n", incoming);
273+
parseState.invalidByte = false;
274+
}
275+
if (parseState.printMessageNumber)
276+
{
277+
parseState.printMessageNumber = false;
278+
if (parseState.invalidRtcmCrc)
279+
{
280+
parseState.invalidRtcmCrc = false;
281+
Serial.printf(" RTCM %d, %2d bytes, bad CRC, computed 0x%06x, sent 0x%02x%02x%02x\r\n",
282+
parseState.messageNumber,
283+
3 + 1 + parseState.length + 3,
284+
parseState.rtcmCrc,
285+
parseState.crcByte[0],
286+
parseState.crcByte[1],
287+
parseState.crcByte[2]);
288+
}
289+
else
290+
Serial.printf(" RTCM %d, %2d bytes\r\n",
291+
parseState.messageNumber,
292+
3 + 1 + parseState.length + 3);
293+
}
294+
if (parseState.printMessageName)
295+
{
296+
parseState.printMessageName = false;
297+
if (parseState.invalidNmeaChecksum)
298+
{
299+
parseState.invalidNmeaChecksum = false;
300+
Serial.printf(" NMEA %s, %2d bytes, bad checksum, computed 0x%02x, sent 0x%c%c\r\n",
301+
parseState.messageName,
302+
parseState.length,
303+
parseState.nmeaChecksum,
304+
parseState.checksumByte1,
305+
parseState.checksumByte2);
306+
}
307+
else
308+
Serial.printf(" NMEA %s, %2d bytes\r\n",
309+
parseState.messageName,
310+
parseState.length);
311+
}
312+
}
361313
}
362314

363315
//Indicate that the GNSS is providing correction data

0 commit comments

Comments
 (0)