1pub const MAX_SENTENCE: usize = 120;
39
40const MAX_FIELDS: usize = 24;
45
46pub const DEGREE_SCALE: i32 = 10_000_000;
48
49#[derive(Clone, Copy, Debug, PartialEq, Eq)]
51pub enum Sentence {
52 Rmc(Rmc),
54 Gga(Gga),
56 Gsa(Gsa),
58 Gsv(Gsv),
60}
61
62#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
70pub struct Rmc {
71 pub valid: bool,
73 pub time: Option<crate::DateTime>,
76 pub latitude: Option<i32>,
78 pub longitude: Option<i32>,
80}
81
82#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
84pub struct Gga {
85 pub quality: u8,
88 pub sats_used: u8,
90 pub hdop_centi: Option<u16>,
97 pub altitude_m: Option<i32>,
99 pub latitude: Option<i32>,
101 pub longitude: Option<i32>,
103}
104
105#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
107pub struct Gsa {
108 pub fix_mode: u8,
110 pub hdop_centi: Option<u16>,
112}
113
114#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
116pub struct Gsv {
117 pub in_view: u8,
119 pub message: u8,
122 pub talker: [u8; 2],
129}
130
131pub struct Assembler {
138 buf: [u8; MAX_SENTENCE],
139 len: usize,
140 started: bool,
144 overrun: bool,
147}
148
149impl Default for Assembler {
150 fn default() -> Self {
151 Self::new()
152 }
153}
154
155impl Assembler {
156 pub const fn new() -> Self {
158 Self {
159 buf: [0; MAX_SENTENCE],
160 len: 0,
161 started: false,
162 overrun: false,
163 }
164 }
165
166 pub fn reset(&mut self) {
170 self.len = 0;
171 self.started = false;
172 self.overrun = false;
173 }
174
175 pub fn push(&mut self, byte: u8) -> Option<Sentence> {
182 match byte {
183 b'$' => {
184 self.len = 0;
185 self.started = true;
186 self.overrun = false;
187 None
188 }
189 b'\r' | b'\n' => {
190 let complete = self.started && !self.overrun && self.len > 0;
191 let parsed = complete.then(|| parse(&self.buf[..self.len])).flatten();
192 self.len = 0;
193 self.started = false;
194 parsed
195 }
196 _ => {
197 if !self.started || self.overrun {
198 return None;
199 }
200 if self.len == MAX_SENTENCE {
201 self.overrun = true;
204 return None;
205 }
206 self.buf[self.len] = byte;
207 self.len += 1;
208 None
209 }
210 }
211 }
212}
213
214pub fn parse(body: &[u8]) -> Option<Sentence> {
217 let payload = verify_checksum(body)?;
218 let mut fields = Fields::split(payload);
219 let kind = fields.next()?;
220 if kind.len() != 5 {
225 return None;
226 }
227 match &kind[2..] {
228 b"RMC" => parse_rmc(fields).map(Sentence::Rmc),
229 b"GGA" => parse_gga(fields).map(Sentence::Gga),
230 b"GSA" => parse_gsa(fields).map(Sentence::Gsa),
231 b"GSV" => parse_gsv(fields, [kind[0], kind[1]]).map(Sentence::Gsv),
232 _ => None,
233 }
234}
235
236fn verify_checksum(body: &[u8]) -> Option<&[u8]> {
242 let star = body.iter().rposition(|&byte| byte == b'*')?;
243 let (payload, trailer) = body.split_at(star);
244 let digits = trailer.get(1..3)?;
245 if trailer.len() != 3 {
246 return None;
247 }
248 let expected = (hex_digit(digits[0])? << 4) | hex_digit(digits[1])?;
249 let actual = payload.iter().fold(0u8, |sum, &byte| sum ^ byte);
250 (actual == expected).then_some(payload)
251}
252
253const fn hex_digit(byte: u8) -> Option<u8> {
254 match byte {
255 b'0'..=b'9' => Some(byte - b'0'),
256 b'A'..=b'F' => Some(byte - b'A' + 10),
257 b'a'..=b'f' => Some(byte - b'a' + 10),
258 _ => None,
259 }
260}
261
262struct Fields<'a> {
268 rest: &'a [u8],
269 yielded: usize,
270 done: bool,
271}
272
273impl<'a> Fields<'a> {
274 fn split(payload: &'a [u8]) -> Self {
275 Self {
276 rest: payload,
277 yielded: 0,
278 done: false,
279 }
280 }
281
282 fn next(&mut self) -> Option<&'a [u8]> {
283 if self.done || self.yielded == MAX_FIELDS {
284 return None;
285 }
286 self.yielded += 1;
287 match self.rest.iter().position(|&byte| byte == b',') {
288 Some(comma) => {
289 let (field, rest) = self.rest.split_at(comma);
290 self.rest = &rest[1..];
291 Some(field)
292 }
293 None => {
294 self.done = true;
295 Some(self.rest)
296 }
297 }
298 }
299
300 fn next_or_empty(&mut self) -> &'a [u8] {
305 self.next().unwrap_or(&[])
306 }
307}
308
309fn parse_rmc(mut fields: Fields<'_>) -> Option<Rmc> {
310 let time = fields.next_or_empty();
311 let status = fields.next_or_empty();
312 let lat = fields.next_or_empty();
313 let lat_hemisphere = fields.next_or_empty();
314 let lon = fields.next_or_empty();
315 let lon_hemisphere = fields.next_or_empty();
316 let _speed = fields.next_or_empty();
317 let _course = fields.next_or_empty();
318 let date = fields.next_or_empty();
319
320 Some(Rmc {
321 valid: status == b"A",
322 time: parse_instant(date, time),
323 latitude: parse_degrees(lat, lat_hemisphere, 2),
324 longitude: parse_degrees(lon, lon_hemisphere, 3),
325 })
326}
327
328fn parse_gga(mut fields: Fields<'_>) -> Option<Gga> {
329 let _time = fields.next_or_empty();
330 let lat = fields.next_or_empty();
331 let lat_hemisphere = fields.next_or_empty();
332 let lon = fields.next_or_empty();
333 let lon_hemisphere = fields.next_or_empty();
334 let quality = fields.next_or_empty();
335 let sats = fields.next_or_empty();
336 let hdop = fields.next_or_empty();
337 let altitude = fields.next_or_empty();
338
339 Some(Gga {
340 quality: parse_u8(quality).unwrap_or(0),
341 sats_used: parse_u8(sats).unwrap_or(0),
342 hdop_centi: parse_fixed(hdop, 2).map(|value| value as u16),
343 altitude_m: parse_fixed(altitude, 0).map(|value| value as i32),
347 latitude: parse_degrees(lat, lat_hemisphere, 2),
348 longitude: parse_degrees(lon, lon_hemisphere, 3),
349 })
350}
351
352fn parse_gsa(mut fields: Fields<'_>) -> Option<Gsa> {
353 let _selection = fields.next_or_empty();
354 let mode = fields.next_or_empty();
355 for _ in 0..12 {
358 let _ = fields.next_or_empty();
359 }
360 let _pdop = fields.next_or_empty();
361 let hdop = fields.next_or_empty();
362
363 Some(Gsa {
364 fix_mode: parse_u8(mode).unwrap_or(0),
365 hdop_centi: parse_fixed(hdop, 2).and_then(|value| u16::try_from(value).ok()),
366 })
367}
368
369fn parse_gsv(mut fields: Fields<'_>, talker: [u8; 2]) -> Option<Gsv> {
370 let _messages = fields.next_or_empty();
371 let message = fields.next_or_empty();
372 let in_view = fields.next_or_empty();
373
374 Some(Gsv {
375 in_view: parse_u8(in_view).unwrap_or(0),
376 message: parse_u8(message).unwrap_or(0),
377 talker,
378 })
379}
380
381fn parse_instant(date: &[u8], time: &[u8]) -> Option<crate::DateTime> {
384 if date.len() != 6 || time.len() < 6 {
385 return None;
386 }
387 let day = parse_pair(&date[0..2])?;
388 let month = parse_pair(&date[2..4])?;
389 let year = parse_pair(&date[4..6])?;
390 let hour = parse_pair(&time[0..2])?;
391 let minute = parse_pair(&time[2..4])?;
392 let second = parse_pair(&time[4..6])?;
393
394 let year = if year >= 80 {
401 1900 + i32::from(year)
402 } else {
403 2000 + i32::from(year)
404 };
405
406 let at = crate::DateTime {
407 year,
408 month,
409 day,
410 hour,
411 minute,
412 second,
413 };
414 if !at.is_valid() {
415 return None;
416 }
417 match at.to_unix() {
420 Some(epoch) if epoch >= crate::epoch::MIN_PLAUSIBLE_EPOCH => Some(at),
421 _ => None,
422 }
423}
424
425fn parse_degrees(field: &[u8], hemisphere: &[u8], degree_digits: usize) -> Option<i32> {
435 if field.len() < degree_digits {
436 return None;
437 }
438 let (degrees, minutes) = field.split_at(degree_digits);
439 let degrees = parse_unsigned(degrees)?;
440 let minutes = parse_fixed(minutes, 5)?;
443 if minutes >= 60 * 100_000 {
444 return None;
445 }
446 let scaled = degrees as i64 * DEGREE_SCALE as i64 + (minutes as i64 * 100) / 60;
447 let signed = match hemisphere {
448 b"N" | b"E" => scaled,
449 b"S" | b"W" => -scaled,
450 _ => return None,
451 };
452 i32::try_from(signed).ok()
453}
454
455fn parse_fixed(field: &[u8], decimals: u32) -> Option<i64> {
461 let (negative, digits) = match field.split_first() {
462 Some((b'-', rest)) => (true, rest),
463 Some((b'+', rest)) => (false, rest),
464 _ => (false, field),
465 };
466 if digits.is_empty() {
467 return None;
468 }
469 let (whole, fraction) = match digits.iter().position(|&byte| byte == b'.') {
470 Some(point) => (&digits[..point], &digits[point + 1..]),
471 None => (digits, &[][..]),
472 };
473 let mut value = 0i64;
474 for &byte in whole {
475 value = value
476 .checked_mul(10)?
477 .checked_add(i64::from(digit(byte)?))?;
478 }
479 for index in 0..decimals {
480 value = value.checked_mul(10)?;
481 if let Some(&byte) = fraction.get(index as usize) {
482 value = value.checked_add(i64::from(digit(byte)?))?;
483 }
484 }
485 for &byte in fraction.iter().skip(decimals as usize) {
488 digit(byte)?;
489 }
490 Some(if negative { -value } else { value })
491}
492
493fn parse_unsigned(field: &[u8]) -> Option<u32> {
494 if field.is_empty() {
495 return None;
496 }
497 let mut value = 0u32;
498 for &byte in field {
499 value = value
500 .checked_mul(10)?
501 .checked_add(u32::from(digit(byte)?))?;
502 }
503 Some(value)
504}
505
506fn parse_u8(field: &[u8]) -> Option<u8> {
507 u8::try_from(parse_unsigned(field)?).ok()
508}
509
510fn parse_pair(field: &[u8]) -> Option<u8> {
512 match field {
513 [high, low] => Some(digit(*high)? * 10 + digit(*low)?),
514 _ => None,
515 }
516}
517
518const fn digit(byte: u8) -> Option<u8> {
519 match byte {
520 b'0'..=b'9' => Some(byte - b'0'),
521 _ => None,
522 }
523}
524
525#[cfg(test)]
526mod tests {
527 use super::*;
528 use core::fmt::Write as _;
529
530 fn feed(assembler: &mut Assembler, line: &str) -> Option<Sentence> {
532 let mut last = None;
533 for byte in line.bytes() {
534 if let Some(sentence) = assembler.push(byte) {
535 last = Some(sentence);
536 }
537 }
538 last
539 }
540
541 fn line(body: &str) -> heapless::String<MAX_SENTENCE> {
551 let checksum = body.bytes().fold(0u8, |sum, byte| sum ^ byte);
552 let mut out = heapless::String::new();
553 out.push('$').unwrap();
554 out.push_str(body).unwrap();
555 write!(out, "*{checksum:02X}\r\n").unwrap();
556 out
557 }
558
559 fn one(body: &str) -> Option<Sentence> {
561 feed(&mut Assembler::new(), &line(body))
562 }
563
564 fn one_raw(raw: &str) -> Option<Sentence> {
567 feed(&mut Assembler::new(), raw)
568 }
569
570 #[test]
571 fn a_valid_rmc_carries_a_position_and_an_instant() {
572 let Some(Sentence::Rmc(rmc)) =
573 one("GPRMC,123519.00,A,4807.038,N,01131.000,E,022.4,084.4,230326,003.1,W")
574 else {
575 panic!("RMC did not parse");
576 };
577 assert!(rmc.valid);
578 assert_eq!(
579 rmc.time,
580 Some(crate::DateTime {
581 year: 2026,
582 month: 3,
583 day: 23,
584 hour: 12,
585 minute: 35,
586 second: 19,
587 })
588 );
589 assert_eq!(rmc.latitude, Some(481_173_000));
591 assert_eq!(rmc.longitude, Some(115_166_666));
592 }
593
594 #[test]
598 fn a_void_rmc_can_still_carry_a_time() {
599 let Some(Sentence::Rmc(rmc)) = one("GPRMC,081836.00,V,,,,,,,130926,,") else {
600 panic!("void RMC did not parse");
601 };
602 assert!(!rmc.valid, "a void fix must not read as valid");
603 assert_eq!(rmc.latitude, None);
604 assert_eq!(rmc.longitude, None);
605 assert_eq!(
606 rmc.time.map(|at| (at.year, at.month, at.day, at.hour)),
607 Some((2026, 9, 13, 8))
608 );
609 }
610
611 #[test]
619 fn a_receiver_reporting_its_own_epoch_is_reporting_no_clock() {
620 let Some(Sentence::Rmc(rmc)) = one("GNRMC,000346.000,V,,,,,,,060180,,,N,V") else {
621 panic!("RMC did not parse");
622 };
623 assert_eq!(
624 rmc.time, None,
625 "the GPS epoch was accepted as the current time"
626 );
627 }
628
629 #[test]
631 fn two_digit_years_window_on_the_gps_epoch() {
632 let Some(Sentence::Rmc(rmc)) = one("GPRMC,081836.00,V,,,,,,,130826,,") else {
634 panic!("RMC did not parse");
635 };
636 assert_eq!(rmc.time.map(|at| at.year), Some(2026));
637
638 let Some(Sentence::Rmc(rmc)) = one("GPRMC,081836.00,V,,,,,,,130899,,") else {
641 panic!("RMC did not parse");
642 };
643 assert_eq!(rmc.time, None);
644 }
645
646 #[test]
648 fn a_cold_rmc_carries_nothing() {
649 let Some(Sentence::Rmc(rmc)) = one("GPRMC,,V,,,,,,,,,,N") else {
650 panic!("cold RMC did not parse");
651 };
652 assert!(!rmc.valid);
653 assert_eq!(rmc.time, None);
654 assert_eq!(rmc.latitude, None);
655 }
656
657 #[test]
658 fn gga_carries_quality_altitude_and_satellite_count() {
659 let Some(Sentence::Gga(gga)) =
660 one("$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47\r\n")
661 else {
662 panic!("GGA did not parse");
663 };
664 assert_eq!(gga.quality, 1);
665 assert_eq!(gga.sats_used, 8);
666 assert_eq!(gga.altitude_m, Some(545));
667 assert_eq!(gga.latitude, Some(481_173_000));
668 }
669
670 #[test]
671 fn gga_handles_a_negative_altitude_below_sea_level() {
672 let Some(Sentence::Gga(gga)) =
673 one("GPGGA,123519,3129.000,N,03521.000,E,1,09,0.9,-412.5,M,17.2,M,,")
674 else {
675 panic!("GGA did not parse");
676 };
677 assert_eq!(gga.altitude_m, Some(-412));
678 }
679
680 #[test]
681 fn gsa_reports_the_solution_dimension_and_dilution() {
682 let Some(Sentence::Gsa(gsa)) = one("$GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39\r\n")
683 else {
684 panic!("GSA did not parse");
685 };
686 assert_eq!(gsa.fix_mode, 3);
687 assert_eq!(gsa.hdop_centi, Some(130));
688
689 let Some(Sentence::Gsa(gsa)) = one("$GPGSA,A,1,,,,,,,,,,,,,,,*1E\r\n") else {
690 panic!("no-fix GSA did not parse");
691 };
692 assert_eq!(gsa.fix_mode, 1);
693 assert_eq!(gsa.hdop_centi, None);
694 }
695
696 #[test]
697 fn gsv_reports_satellites_in_view() {
698 let Some(Sentence::Gsv(gsv)) =
699 one("$GPGSV,3,1,11,03,03,111,00,04,15,270,00,06,01,010,00,13,06,292,00*74\r\n")
700 else {
701 panic!("GSV did not parse");
702 };
703 assert_eq!(gsv.in_view, 11);
704 assert_eq!(gsv.message, 1);
705 }
706
707 #[test]
710 fn any_talker_prefix_is_accepted() {
711 for body in [
712 "GNRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130926,011.3,E",
713 "BDRMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130926,011.3,E",
714 "GARMC,081836,A,3751.65,S,14507.36,E,000.0,360.0,130926,011.3,E",
715 ] {
716 assert!(
717 matches!(one(body), Some(Sentence::Rmc(_))),
718 "talker rejected: {body}"
719 );
720 }
721 }
722
723 #[test]
724 fn southern_and_western_hemispheres_are_negative() {
725 let Some(Sentence::Rmc(rmc)) =
726 one("GPRMC,081836,A,3751.65,S,14507.36,W,000.0,360.0,130926,011.3,E")
727 else {
728 panic!("RMC did not parse");
729 };
730 assert_eq!(rmc.latitude, Some(-378_608_333));
732 assert_eq!(rmc.longitude, Some(-1_451_226_666));
733 }
734
735 #[test]
736 fn a_bad_checksum_is_dropped_silently() {
737 assert_eq!(
738 one_raw("$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230326,003.1,W*00\r\n"),
739 None
740 );
741 assert_eq!(one_raw("$GPRMC,123519,A*6\r\n"), None);
743 assert_eq!(one_raw("$GPRMC,123519,A\r\n"), None);
744 assert_eq!(one_raw("$GPRMC,123519,A*ZZ\r\n"), None);
745 }
746
747 #[test]
750 fn a_torn_leading_line_is_discarded_and_the_next_one_parses() {
751 let mut assembler = Assembler::new();
752 assert_eq!(feed(&mut assembler, "038,N,01131.000,E*11\r\n"), None);
755 assert!(matches!(
756 feed(&mut assembler, &line("GPGSA,A,3,04,,,,,,,,,,,,2.5,1.3,2.1")),
757 Some(Sentence::Gsa(_))
758 ));
759 }
760
761 #[test]
764 fn a_restart_mid_sentence_abandons_the_partial_line() {
765 let mut assembler = Assembler::new();
766 assert_eq!(feed(&mut assembler, "$GPRMC,1235"), None);
767 assert!(matches!(
768 feed(&mut assembler, &line("GPGSV,3,1,11,03,03,111,00")),
769 Some(Sentence::Gsv(_))
770 ));
771 }
772
773 #[test]
774 fn an_over_long_line_is_dropped_whole_and_recovery_is_immediate() {
775 let mut assembler = Assembler::new();
776 let mut body = heapless::String::<{ MAX_SENTENCE * 2 }>::new();
777 body.push_str("GPGSV,3,1,11").unwrap();
778 while body.len() < MAX_SENTENCE + 20 {
779 body.push_str(",03,03,111,00").unwrap();
780 }
781 body.push_str("*4E\r\n").unwrap();
782 assert_eq!(feed(&mut assembler, &body), None);
783 assert!(matches!(
784 feed(&mut assembler, &line("GPGSV,3,1,11,03,03,111,00")),
785 Some(Sentence::Gsv(_))
786 ));
787 }
788
789 #[test]
792 fn any_line_terminator_ends_a_sentence() {
793 for terminator in ["\r\n", "\n", "\r", "\n\r\n"] {
794 let mut assembler = Assembler::new();
795 let mut raw = line("GPGSA,A,3,04,,,,,,,,,,,,2.5,1.3,2.1");
796 raw.truncate(raw.len() - 2);
799 raw.push_str(terminator).unwrap();
800 assert!(
801 matches!(feed(&mut assembler, &raw), Some(Sentence::Gsa(_))),
802 "terminator {terminator:?} did not end the sentence"
803 );
804 }
805 }
806
807 #[test]
808 fn unknown_sentence_types_are_ignored() {
809 assert_eq!(
811 one_raw("$GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48\r\n"),
812 None
813 );
814 assert_eq!(one_raw("$PAIR001,066,0*3B\r\n"), None);
816 }
817
818 #[test]
819 fn malformed_numeric_fields_do_not_produce_a_position() {
820 let Some(Sentence::Rmc(rmc)) = one("GPRMC,123519,A,48zz.038,N,01131.000,E,,,230326,,")
822 else {
823 panic!("RMC did not parse");
824 };
825 assert_eq!(rmc.latitude, None, "garbage parsed as a latitude");
826 assert_eq!(rmc.longitude, Some(115_166_666), "the good field was lost");
827 }
828
829 #[test]
830 fn an_out_of_range_date_or_time_yields_no_instant() {
831 let Some(Sentence::Rmc(rmc)) = one("GPRMC,123519,V,,,,,,,231326,,") else {
833 panic!("RMC did not parse");
834 };
835 assert_eq!(rmc.time, None);
836 let Some(Sentence::Rmc(rmc)) = one("GPRMC,253519,V,,,,,,,230326,,") else {
838 panic!("RMC did not parse");
839 };
840 assert_eq!(rmc.time, None);
841 }
842
843 #[test]
845 fn minutes_at_or_past_sixty_are_rejected() {
846 let Some(Sentence::Rmc(rmc)) = one("GPRMC,123519,A,4860.000,N,01131.000,E,,,230326,,")
847 else {
848 panic!("RMC did not parse");
849 };
850 assert_eq!(rmc.latitude, None);
851 }
852
853 #[test]
854 fn fixed_point_parsing_truncates_rather_than_rounding() {
855 assert_eq!(parse_fixed(b"1.29", 1), Some(12));
859 assert_eq!(parse_fixed(b"1.2", 3), Some(1_200));
860 assert_eq!(parse_fixed(b"1", 2), Some(100));
861 assert_eq!(parse_fixed(b"-0.5", 1), Some(-5));
862 assert_eq!(parse_fixed(b"", 2), None);
863 assert_eq!(parse_fixed(b"1.2x", 1), None);
864 assert_eq!(parse_fixed(b"x", 0), None);
865 }
866
867 #[test]
869 fn resetting_discards_the_partial_line() {
870 let mut assembler = Assembler::new();
871 feed(&mut assembler, "$GPRMC,1235");
872 assembler.reset();
873 assert_eq!(feed(&mut assembler, "19,A*00\r\n"), None);
875 assert!(matches!(
876 feed(&mut assembler, &line("GPGSV,3,1,11,03,03,111,00")),
877 Some(Sentence::Gsv(_))
878 ));
879 }
880}