Skip to content

Commit e7db53a

Browse files
LeeLeahy2LeeLeahy2
committed
Switch to using common RTCM and NMEA message parser
1 parent ac596eb commit e7db53a

File tree

2 files changed

+70
-132
lines changed

2 files changed

+70
-132
lines changed

‎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, actual 0x%06x, expecting 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, actual 0x%c%c, expected 0x%02x\r\n",
301+
parseState.messageName,
302+
parseState.length,
303+
parseState.checksumByte1,
304+
parseState.checksumByte2,
305+
parseState.nmeaChecksum);
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

‎Firmware/RTK_Surveyor/settings.h‎

Lines changed: 0 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -135,20 +135,6 @@ enum NTRIPServerState
135135
};
136136
volatile byte ntripServerState = NTRIP_SERVER_OFF;
137137

138-
enum RtcmTransportState
139-
{
140-
RTCM_TRANSPORT_STATE_WAIT_FOR_PREAMBLE_D3 = 0,
141-
RTCM_TRANSPORT_STATE_READ_LENGTH_1,
142-
RTCM_TRANSPORT_STATE_READ_LENGTH_2,
143-
RTCM_TRANSPORT_STATE_READ_MESSAGE_1,
144-
RTCM_TRANSPORT_STATE_READ_MESSAGE_2,
145-
RTCM_TRANSPORT_STATE_READ_DATA,
146-
RTCM_TRANSPORT_STATE_READ_CRC_1,
147-
RTCM_TRANSPORT_STATE_READ_CRC_2,
148-
RTCM_TRANSPORT_STATE_READ_CRC_3,
149-
RTCM_TRANSPORT_STATE_CHECK_CRC
150-
};
151-
152138
//Message parsing state for RTCM and NMEA messages
153139
enum ParseState
154140
{

0 commit comments

Comments
 (0)