@@ -39,75 +39,136 @@ License: MIT. Please see LICENSE.md for more details
3939// |<-------- Checksum -------->|
4040//
4141
42- // Read the line termination
43- bool sempNmeaLineTermination (SEMP_PARSE_STATE *parse, uint8_t data )
42+ // Validate the checksum
43+ void sempNmeaValidateChecksum (SEMP_PARSE_STATE *parse)
4444{
4545 int checksum;
46+ SEMP_SCRATCH_PAD *scratchPad = (SEMP_SCRATCH_PAD *)parse->scratchPad ;
4647
47- // Process the line termination
48- if ((data == ' \r ' ' \n '
48+ // Convert the checksum characters into binary
49+ checksum = sempAsciiToNibble (parse->buffer [parse->length - 2 ]) << 4 ;
50+ checksum |= sempAsciiToNibble (parse->buffer [parse->length - 1 ]);
51+
52+ // Validate the checksum
53+ if ((checksum == parse->crc ) || (parse->badCrc && (!parse->badCrc (parse))))
54+ {
55+ // Always add the carriage return and line feed
56+ parse->buffer [parse->length ++] = ' \r '
57+ parse->buffer [parse->length ++] = ' \n '
58+
59+ // Zero terminate the NMEA sentence, don't count this in the length
60+ parse->buffer [parse->length ] = 0 ;
61+
62+ // Process this NMEA sentence
63+ parse->eomCallback (parse, parse->type ); // Pass parser array index
64+ }
65+ else
66+ // Display the checksum error
67+ sempPrintf (parse->printDebug ,
68+ " SEMP: %s NMEA %s, 0x%04x (%d) bytes, bad checksum, "
69+ " received 0x%c%c, computed: 0x%02x"
70+ parse->parserName ,
71+ scratchPad->nmea .sentenceName ,
72+ parse->length , parse->length ,
73+ parse->buffer [parse->length - 2 ],
74+ parse->buffer [parse->length - 1 ],
75+ parse->crc );
76+ }
77+
78+ // Read the linefeed
79+ bool sempNmeaLineFeed (SEMP_PARSE_STATE *parse, uint8_t data)
80+ {
81+ int checksum;
82+
83+ // Don't add the current character to the length
84+ parse->length -= 1 ;
85+
86+ // Process the LF
87+ if (data == ' \n '
88+ {
89+ // Pass the sentence to the upper layer
90+ sempNmeaValidateChecksum (parse);
91+
92+ // Start searching for a preamble byte
93+ parse->state = sempFirstByte;
4994 return true ;
95+ }
96+
97+ // Pass the sentence to the upper layer
98+ sempNmeaValidateChecksum (parse);
5099
51100 // Start searching for a preamble byte
52101 return sempFirstByte (parse, data);
53102}
54103
55- // Read the second checksum byte
56- bool sempNmeaChecksumByte2 (SEMP_PARSE_STATE *parse, uint8_t data)
104+ // Read the remaining carriage return
105+ bool sempNmeaCarriageReturn (SEMP_PARSE_STATE *parse, uint8_t data)
57106{
58- int checksum;
59- SEMP_SCRATCH_PAD *scratchPad = (SEMP_SCRATCH_PAD *) parse->scratchPad ;
107+ // Don't add the current character to the length
108+ parse->length -= 1 ;
60109
61- // Convert the checksum characters into binary
62- checksum = sempAsciiToNibble (parse->buffer [parse->length - 1 ]);
63- checksum |= sempAsciiToNibble (parse->buffer [parse->length - 2 ]) << 4 ;
64-
65- // Validate the checksum character
66- if (checksum < 0 )
110+ // Process the CR
111+ if (data == ' \r '
67112 {
68- // Invalid checksum character
69- sempPrintf (parse->printDebug ,
70- " SEMP %s: NMEA invalid second checksum character"
71- parse->parserName );
113+ // Pass the sentence to the upper layer
114+ sempNmeaValidateChecksum (parse);
72115
73116 // Start searching for a preamble byte
74- return sempFirstByte (parse, data);
117+ parse->state = sempFirstByte;
118+ return true ;
75119 }
76120
77- // Add CR and LF to the sentence
78- parse->buffer [parse->length ++] = ' \r '
79- parse->buffer [parse->length ++] = ' \n '
121+ // Pass the sentence to the upper layer
122+ sempNmeaValidateChecksum (parse);
80123
81- // Zero terminate the string, don't count this in the length
82- parse->buffer [parse->length ] = 0 ;
124+ // Start searching for a preamble byte
125+ return sempFirstByte (parse, data);
126+ }
83127
84- // Validate the checksum
85- if ((checksum == parse->crc )
86- || (parse->badCrc && (!parse->badCrc (parse))))
128+ // Read the line termination
129+ bool sempNmeaLineTermination (SEMP_PARSE_STATE *parse, uint8_t data)
130+ {
131+ int checksum;
132+
133+ // Don't add the current character to the length
134+ parse->length -= 1 ;
135+
136+ // Process the line termination
137+ if (data == ' \r '
87138 {
88- // Process this NMEA sentence
89- parse->eomCallback (parse, parse->type ); // Pass parser array index
139+ parse->state = sempNmeaLineFeed;
140+ return true ;
141+ }
142+ else if (data == ' \n '
143+ {
144+ parse->state = sempNmeaCarriageReturn;
145+ return true ;
146+ }
147+
148+ // Pass the sentence to the upper layer
149+ sempNmeaValidateChecksum (parse);
150+
151+ // Start searching for a preamble byte
152+ return sempFirstByte (parse, data);
153+ }
90154
91- // Remove any CR or LF that follow
155+ // Read the second checksum byte
156+ bool sempNmeaChecksumByte2 (SEMP_PARSE_STATE *parse, uint8_t data)
157+ {
158+ // Validate the checksum character
159+ if (sempAsciiToNibble (parse->buffer [parse->length - 1 ]) >= 0 )
160+ {
92161 parse->state = sempNmeaLineTermination;
93- parse->length = 0 ;
94162 return true ;
95163 }
96164
97- // Display the checksum error
165+ // Invalid checksum character
98166 sempPrintf (parse->printDebug ,
99- " SEMP: %s NMEA %s, %2d bytes, bad checksum, "
100- " received 0x%c%c, computed: 0x%02x"
101- parse->parserName ,
102- scratchPad->nmea .sentenceName ,
103- parse->length ,
104- parse->buffer [parse->length - 4 ],
105- parse->buffer [parse->length - 3 ],
106- parse->crc );
167+ " SEMP %s: NMEA invalid second checksum character"
168+ parse->parserName );
107169
108170 // Start searching for a preamble byte
109- parse->state = sempFirstByte;
110- return false ;
171+ return sempFirstByte (parse, data);
111172}
112173
113174// Read the first checksum byte
@@ -231,6 +292,10 @@ const char * sempNmeaGetStateName(const SEMP_PARSE_STATE *parse)
231292 return " sempNmeaChecksumByte2"
232293 if (parse->state == sempNmeaLineTermination)
233294 return " sempNmeaLineTermination"
295+ if (parse->state == sempNmeaCarriageReturn)
296+ return " sempNmeaCarriageReturn"
297+ if (parse->state == sempNmeaLineFeed)
298+ return " sempNmeaLineFeed"
234299 if (parse->state == sempNmeaHashPreamble)
235300 return " sempNmeaHashPreamble"
236301 return nullptr ;
0 commit comments