#[allow(unused)]
#[derive(Clone, Copy, Debug, PartialEq)]
enum NmeaEvent {
NoEvent,
Start,
PayloadByte(u8),
Complete,
}
#[allow(unused)]
#[derive(Clone, Copy, Debug, Default, PartialEq)]
enum NmeaState {
#[default]
WaitingForStart,
InPayload {
calculated_checksum: u8,
},
WaitingForChecksum1 {
calculated_checksum: u8,
},
WaitingForChecksum2 {
calculated_checksum: u8,
received_checksum_high: u8,
},
WaitingForCr,
WaitingForLf,
}
#[allow(unused)]
impl NmeaState {
fn on_data_received(&mut self, data: u8) -> NmeaEvent {
if data == b'$' {
*self = Self::InPayload { calculated_checksum: 0 };
return NmeaEvent::Start;
}
match *self {
Self::WaitingForStart => {
if data == b'$' {
*self = Self::InPayload { calculated_checksum: 0 };
NmeaEvent::Start
} else {
NmeaEvent::NoEvent
}
}
Self::InPayload { mut calculated_checksum } => {
if data == b'*' {
*self = Self::WaitingForChecksum1 { calculated_checksum };
NmeaEvent::NoEvent
} else if data.is_ascii_graphic() || data == b' ' {
calculated_checksum ^= data;
*self = Self::InPayload { calculated_checksum };
NmeaEvent::PayloadByte(data)
} else {
*self = Self::WaitingForStart;
NmeaEvent::NoEvent
}
}
Self::WaitingForChecksum1 { calculated_checksum } => {
if let Some(val) = Self::parse_hex_digit(data) {
*self = Self::WaitingForChecksum2 { calculated_checksum, received_checksum_high: val };
} else {
*self = Self::WaitingForStart;
}
NmeaEvent::NoEvent
}
Self::WaitingForChecksum2 { calculated_checksum, received_checksum_high } => {
if let Some(val) = Self::parse_hex_digit(data) {
let received_checksum = (received_checksum_high << 4) | val;
if calculated_checksum == received_checksum {
*self = Self::WaitingForCr;
} else {
*self = Self::WaitingForStart;
}
} else {
*self = Self::WaitingForStart;
}
NmeaEvent::NoEvent
}
Self::WaitingForCr => {
if data == b'\r' {
*self = Self::WaitingForLf;
} else {
*self = Self::WaitingForStart;
}
NmeaEvent::NoEvent
}
Self::WaitingForLf => {
if data == b'\n' {
*self = Self::WaitingForStart;
NmeaEvent::Complete
} else {
*self = Self::WaitingForStart;
NmeaEvent::NoEvent
}
}
}
}
const fn parse_hex_digit(byte: u8) -> Option<u8> {
match byte {
b'0'..=b'9' => Some(byte - b'0'),
b'A'..=b'F' => Some(byte - b'A' + 10),
b'a'..=b'f' => Some(byte - b'a' + 10),
_ => None,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct NmeaParser {
state: NmeaState,
payload_buf: [u8; Self::BUFFER_SIZE],
payload_index: usize,
complete: bool,
}
impl Default for NmeaParser {
fn default() -> Self {
Self::new()
}
}
impl NmeaParser {
const BUFFER_SIZE: usize = 80;
pub const fn new() -> Self {
Self {
state: NmeaState::WaitingForStart,
payload_buf: [0u8; Self::BUFFER_SIZE],
payload_index: 0,
complete: false,
}
}
pub fn on_data_received(&mut self, data: u8) -> bool {
self.complete = false;
match self.state.on_data_received(data) {
NmeaEvent::NoEvent => {}
NmeaEvent::Start => {
self.payload_index = 0;
}
NmeaEvent::PayloadByte(byte) => {
if self.payload_index >= self.payload_buf.len() {
self.state = NmeaState::WaitingForStart;
self.payload_index = 0;
} else {
self.payload_buf[self.payload_index] = byte;
self.payload_index += 1;
}
}
NmeaEvent::Complete => {
self.complete = true;
}
}
self.complete
}
pub fn payload(&self) -> Option<&[u8]> {
if self.complete { Some(&self.payload_buf[..self.payload_index]) } else { None }
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum NmeaRecordType {
Gga,
Rmc,
Gsa,
}
#[allow(unused)]
impl NmeaRecordType {
pub fn from_record(record: &[u8]) -> Option<Self> {
if record.len() < 6 {
return None;
}
match &record[2..5] {
b"GGA" => Some(Self::Gga),
b"RMC" => Some(Self::Rmc),
b"GSA" => Some(Self::Gsa),
_ => None,
}
}
}
pub struct Parse;
#[allow(unused)]
impl Parse {
pub fn int(bytes: &[u8]) -> Option<u32> {
if bytes.is_empty() {
return None;
}
let mut val = 0u32;
for &b in bytes {
if !b.is_ascii_digit() {
return None;
}
val = val.checked_mul(10)?.checked_add(u32::from(b - b'0'))?;
}
Some(val)
}
pub fn fixed_point(bytes: &[u8], scale: u32) -> Option<i32> {
if bytes.is_empty() || scale == 0 {
return None;
}
let mut index = 0;
let negative = bytes[0] == b'-';
if negative {
index = 1;
if index == bytes.len() {
return None;
}
}
let mut integer_part = 0i32;
let mut saw_integer_digit = false;
while index < bytes.len() {
let byte = bytes[index];
if byte == b'.' {
break;
}
let digit = byte.checked_sub(b'0')?;
if digit > 9 {
return None;
}
saw_integer_digit = true;
integer_part = integer_part.checked_mul(10)?.checked_add(i32::from(digit))?;
index += 1;
}
if !saw_integer_digit {
return None;
}
let scale_i32 = i32::try_from(scale).ok()?;
let mut result = integer_part.checked_mul(scale_i32)?;
if index < bytes.len() {
index += 1;
let mut fractional_scale = scale;
while index < bytes.len() {
let byte = bytes[index];
let digit = byte.checked_sub(b'0')?;
if digit > 9 {
return None;
}
fractional_scale /= 10;
if fractional_scale != 0 {
let fractional_scale_i32 = i32::try_from(fractional_scale).ok()?;
let contribution = i32::from(digit).checked_mul(fractional_scale_i32)?;
result = result.checked_add(contribution)?;
}
index += 1;
}
}
if negative { result.checked_neg() } else { Some(result) }
}
pub fn nmea_coordinate(value: &[u8], direction: u8) -> Option<i32> {
if value.is_empty() {
return None;
}
let negative = match direction {
b'N' | b'E' => false,
b'S' | b'W' => true,
_ => return None,
};
let decimal_pos = value.iter().position(|&b| b == b'.')?;
if decimal_pos != 4 && decimal_pos != 5 {
return None;
}
let minute_start = decimal_pos - 2;
let mut degrees = 0u32;
for &byte in &value[..minute_start] {
let digit = byte.checked_sub(b'0')?;
if digit > 9 {
return None;
}
degrees = degrees.checked_mul(10)?.checked_add(u32::from(digit))?;
}
let minute_tens = value[minute_start].checked_sub(b'0')?;
let minute_units = value[minute_start + 1].checked_sub(b'0')?;
if minute_tens > 9 || minute_units > 9 {
return None;
}
let minutes = u32::from(minute_tens) * 10 + u32::from(minute_units);
if minutes >= 60 {
return None;
}
let mut fractional = 0u32;
let mut fractional_digits = 0;
for &byte in &value[decimal_pos + 1..] {
let digit = byte.checked_sub(b'0')?;
if digit > 9 {
return None;
}
if fractional_digits < 4 {
fractional = fractional * 10 + u32::from(digit);
fractional_digits += 1;
}
}
while fractional_digits < 4 {
fractional *= 10;
fractional_digits += 1;
}
let minutes_x1e4 = minutes.checked_mul(10_000)?.checked_add(fractional)?;
let minute_degrees_x1e7 = minutes_x1e4.checked_mul(50)?.checked_add(1)? / 3;
let result = degrees.checked_mul(10_000_000)?.checked_add(minute_degrees_x1e7)?;
let result = i32::try_from(result).ok()?;
if negative { result.checked_neg() } else { Some(result) }
}
pub fn nmea_time(bytes: &[u8]) -> Option<u32> {
if bytes.len() < 6 {
return None;
}
let hour = Self::two_digits(&bytes[0..2])?;
let minute = Self::two_digits(&bytes[2..4])?;
let second = Self::two_digits(&bytes[4..6])?;
if hour >= 24 || minute >= 60 || second >= 60 {
return None;
}
let mut milliseconds = u32::from(hour) * 3_600_000 + u32::from(minute) * 60_000 + u32::from(second) * 1_000;
if bytes.len() > 6 {
if bytes[6] != b'.' {
return None;
}
let mut fraction_ms = 0u32;
for (index, &byte) in bytes[7..].iter().enumerate() {
let digit = byte.checked_sub(b'0')?;
if digit > 9 {
return None;
}
match index {
0 => fraction_ms += u32::from(digit) * 100,
1 => fraction_ms += u32::from(digit) * 10,
2 => fraction_ms += u32::from(digit),
_ => {}
}
}
milliseconds = milliseconds.checked_add(fraction_ms)?;
}
Some(milliseconds)
}
fn two_digits(bytes: &[u8]) -> Option<u8> {
if bytes.len() != 2 {
return None;
}
let tens = bytes[0].checked_sub(b'0')?;
let units = bytes[1].checked_sub(b'0')?;
if tens > 9 || units > 9 {
return None;
}
Some(tens * 10 + units)
}
}
pub struct NmeaFields<'a> {
remainder: Option<&'a [u8]>,
}
impl<'a> NmeaFields<'a> {
pub const fn new(payload: &'a [u8]) -> Self {
Self { remainder: Some(payload) }
}
}
impl<'a> Iterator for NmeaFields<'a> {
type Item = &'a [u8];
fn next(&mut self) -> Option<Self::Item> {
let remainder = self.remainder.take()?;
if let Some(comma_pos) = remainder.iter().position(|&b| b == b',') {
let field = &remainder[..comma_pos];
self.remainder = Some(&remainder[comma_pos + 1..]);
Some(field)
} else {
Some(remainder)
}
}
}
#[cfg(test)]
mod tests_record_type {
#![allow(clippy::expect_used)]
use super::*;
#[test]
fn identifies_gga_record() {
assert_eq!(NmeaRecordType::from_record(b"GPGGA,123519,..."), Some(NmeaRecordType::Gga));
}
#[test]
fn identifies_rmc_record() {
assert_eq!(NmeaRecordType::from_record(b"GPRMC,123519,..."), Some(NmeaRecordType::Rmc));
}
#[test]
fn identifies_gsa_record() {
assert_eq!(NmeaRecordType::from_record(b"GPGSA,A,3,..."), Some(NmeaRecordType::Gsa));
}
#[test]
fn unknown_record_type_is_rejected() {
assert_eq!(NmeaRecordType::from_record(b"GPXXX,123,..."), None);
}
#[test]
fn short_record_is_rejected() {
assert_eq!(NmeaRecordType::from_record(b"GP"), None);
}
#[test]
fn record_type_does_not_depend_on_talker() {
assert_eq!(NmeaRecordType::from_record(b"GNGGA,123519,..."), Some(NmeaRecordType::Gga));
assert_eq!(NmeaRecordType::from_record(b"GNRMC,123519,..."), Some(NmeaRecordType::Rmc));
}
}
#[cfg(test)]
mod tests {
#![allow(clippy::expect_used)]
use super::*;
#[test]
fn valid_sentence_is_accepted() {
let sentence = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*70\r\n";
let mut parser = NmeaParser::new();
let mut complete = false;
for &byte in sentence {
complete = parser.on_data_received(byte);
}
assert!(complete);
assert_eq!(
parser.payload(),
Some(b"GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30".as_slice())
);
}
#[test]
fn invalid_checksum_is_rejected() {
let sentence = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*71\r\n";
let mut parser = NmeaParser::new();
for &byte in sentence {
assert!(!parser.on_data_received(byte));
}
assert_eq!(parser.payload(), None);
}
#[test]
fn invalid_checksum_character_is_rejected() {
let sentence = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*7G\r\n";
let mut parser = NmeaParser::new();
for &byte in sentence {
assert!(!parser.on_data_received(byte));
}
assert_eq!(parser.payload(), None);
}
#[test]
fn missing_lf_is_rejected() {
let sentence = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*70\r";
let mut parser = NmeaParser::new();
for &byte in sentence {
assert!(!parser.on_data_received(byte));
}
assert_eq!(parser.payload(), None);
}
#[test]
fn missing_cr_is_rejected() {
let sentence = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*70\n";
let mut parser = NmeaParser::new();
for &byte in sentence {
assert!(!parser.on_data_received(byte));
}
assert_eq!(parser.payload(), None);
}
#[test]
fn back_to_back_sentences_are_parsed() {
let sentence1 = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*70\r\n";
let sentence2 = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*70\r\n";
let mut parser = NmeaParser::new();
let mut complete_count = 0;
for &byte in sentence1.iter().chain(sentence2) {
if parser.on_data_received(byte) {
complete_count += 1;
assert_eq!(
parser.payload(),
Some(b"GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30".as_slice())
);
}
}
assert_eq!(complete_count, 2);
}
#[test]
fn garbage_before_sentence_is_ignored() {
let garbage = b"hello123";
let sentence = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*70\r\n";
let mut parser = NmeaParser::new();
for &byte in garbage.iter().chain(sentence) {
_ = parser.on_data_received(byte);
}
assert_eq!(
parser.payload(),
Some(b"GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30".as_slice())
);
}
#[test]
fn recovers_after_invalid_sentence() {
let invalid = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*71\r\n";
let valid = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*70\r\n";
let mut parser = NmeaParser::new();
for &byte in invalid.iter().chain(valid) {
_ = parser.on_data_received(byte);
}
assert_eq!(
parser.payload(),
Some(b"GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30".as_slice())
);
}
#[test]
fn resynchronises_on_new_dollar() {
let valid = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*70\r\n";
let corrupt_prefix = b"$GPGSV,3,1,11,10,63,137,17,07$";
let mut parser = NmeaParser::new();
let mut complete_count = 0;
for &byte in corrupt_prefix.iter().chain(valid) {
if parser.on_data_received(byte) {
complete_count += 1;
}
}
assert_eq!(complete_count, 1);
assert_eq!(
parser.payload(),
Some(b"GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30".as_slice())
);
}
#[test]
fn payload_overflow_is_rejected() {
let mut parser = NmeaParser::new();
assert!(!parser.on_data_received(b'$'));
for _ in 0..NmeaParser::BUFFER_SIZE {
assert!(!parser.on_data_received(b'A'));
}
assert!(!parser.on_data_received(b'B'));
assert_eq!(parser.payload(), None);
}
#[test]
fn recovers_after_payload_overflow() {
let valid = b"$GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30*70\r\n";
let mut parser = NmeaParser::new();
assert!(!parser.on_data_received(b'$'));
for _ in 0..NmeaParser::BUFFER_SIZE {
assert!(!parser.on_data_received(b'A'));
}
assert!(!parser.on_data_received(b'B'));
let mut complete = false;
for &byte in valid {
complete = parser.on_data_received(byte);
}
assert!(complete);
assert_eq!(
parser.payload(),
Some(b"GPGSV,3,1,11,10,63,137,17,07,61,098,15,05,59,290,20,08,54,157,30".as_slice())
);
}
#[test]
fn fixed_point_basic() {
assert_eq!(Parse::fixed_point(b"12.34", 100), Some(1234));
assert_eq!(Parse::fixed_point(b"12.3", 100), Some(1230));
assert_eq!(Parse::fixed_point(b"12", 100), Some(1200));
}
#[test]
fn fixed_point_negative() {
assert_eq!(Parse::fixed_point(b"-12.34", 100), Some(-1234));
assert_eq!(Parse::fixed_point(b"-12.3", 100), Some(-1230));
}
#[test]
fn fixed_point_truncates_extra_precision() {
assert_eq!(Parse::fixed_point(b"12.345", 100), Some(1234));
}
#[test]
fn fixed_point_rejects_invalid_input() {
assert_eq!(Parse::fixed_point(b"", 100), None);
assert_eq!(Parse::fixed_point(b"-", 100), None);
assert_eq!(Parse::fixed_point(b"12.x", 100), None);
assert_eq!(Parse::fixed_point(b"12.3.4", 100), None);
assert_eq!(Parse::fixed_point(b"abc", 100), None);
}
#[test]
fn fixed_point_rejects_overflow() {
assert_eq!(Parse::fixed_point(b"999999999999999999", 100), None);
}
#[test]
fn parse_nmea_coordinate_north() {
assert_eq!(Parse::nmea_coordinate(b"4916.45", b'N'), Some(492_741_667));
}
#[test]
fn parse_nmea_coordinate_south() {
assert_eq!(Parse::nmea_coordinate(b"4916.45", b'S'), Some(-492_741_667));
}
#[test]
fn parse_nmea_coordinate_east() {
assert_eq!(Parse::nmea_coordinate(b"12311.12", b'E'), Some(1_231_853_333));
}
#[test]
fn parse_nmea_coordinate_west() {
assert_eq!(Parse::nmea_coordinate(b"12311.12", b'W'), Some(-1_231_853_333));
}
#[test]
fn parse_nmea_coordinate_rejects_invalid_direction() {
assert_eq!(Parse::nmea_coordinate(b"4916.45", b'X'), None);
}
#[test]
fn parse_nmea_coordinate_rejects_invalid_minutes() {
assert_eq!(Parse::nmea_coordinate(b"4960.00", b'N'), None);
}
#[test]
fn parse_nmea_coordinate_rejects_missing_decimal() {
assert_eq!(Parse::nmea_coordinate(b"491645", b'N'), None);
}
#[test]
fn parse_nmea_coordinate_rejects_bad_digit() {
assert_eq!(Parse::nmea_coordinate(b"49x6.45", b'N'), None);
}
#[test]
fn parse_nmea_coordinate_rejects_empty() {
assert_eq!(Parse::nmea_coordinate(b"", b'N'), None);
}
#[test]
fn parse_nmea_coordinate_zero() {
assert_eq!(Parse::nmea_coordinate(b"0000.00", b'N'), Some(0));
}
#[test]
fn parse_nmea_coordinate_exact_degree() {
assert_eq!(Parse::nmea_coordinate(b"4900.00", b'N'), Some(490_000_000));
}
#[test]
fn parse_nmea_coordinate_near_sixty_minutes() {
assert_eq!(Parse::nmea_coordinate(b"4959.9999", b'N'), Some(499_999_983));
}
#[test]
fn parse_nmea_time_basic() {
assert_eq!(Parse::nmea_time(b"123519"), Some(45_319_000));
}
#[test]
fn parse_nmea_time_with_fraction() {
assert_eq!(Parse::nmea_time(b"123519.500"), Some(45_319_500));
}
#[test]
fn parse_nmea_time_truncates_excess_precision() {
assert_eq!(Parse::nmea_time(b"123519.5009"), Some(45_319_500));
}
#[test]
fn parse_nmea_time_midnight() {
assert_eq!(Parse::nmea_time(b"000000"), Some(0));
}
#[test]
fn parse_nmea_time_end_of_day() {
assert_eq!(Parse::nmea_time(b"235959.999"), Some(86_399_999));
}
#[test]
fn parse_nmea_time_rejects_invalid_hour() {
assert_eq!(Parse::nmea_time(b"240000"), None);
}
#[test]
fn parse_nmea_time_rejects_invalid_minute() {
assert_eq!(Parse::nmea_time(b"126000"), None);
}
#[test]
fn parse_nmea_time_rejects_invalid_second() {
assert_eq!(Parse::nmea_time(b"125960"), None);
}
#[test]
fn parse_nmea_time_rejects_bad_digit() {
assert_eq!(Parse::nmea_time(b"1235X9"), None);
}
#[test]
fn parse_nmea_time_rejects_bad_separator() {
assert_eq!(Parse::nmea_time(b"123519X500"), None);
}
}