Created
June 4, 2020 13:19
-
-
Save FrozenDroid/d463dc06e71d57cdf93c7941d3cfcc2c to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
use core::str::FromStr; | |
use nom::bytes::streaming::{take, take_until, take_while_m_n}; | |
use nom::character::complete::alpha0; | |
use nom::character::streaming::digit1; | |
use nom::character::{is_digit, is_hex_digit}; | |
use nom::combinator::map; | |
use nom::number::streaming::float; | |
use nom::{ | |
alt, branch::alt, bytes::streaming::tag, named, number::streaming::double, tag, IResult, | |
}; | |
use time::Time; | |
#[derive(Debug, PartialEq)] | |
pub enum LatitudeDirection { | |
North, | |
South, | |
} | |
#[derive(Debug, PartialEq)] | |
pub enum LongitudeDirection { | |
East, | |
West, | |
} | |
#[derive(Debug, PartialEq)] | |
pub struct Latitude { | |
lat: f64, | |
dir: LatitudeDirection, | |
} | |
#[derive(Debug, PartialEq)] | |
pub struct Longitude { | |
lon: f64, | |
dir: LongitudeDirection, | |
} | |
#[derive(Debug, PartialEq)] | |
pub struct GnsFixData { | |
lat: Latitude, | |
lon: Longitude, | |
utc: Time, | |
satellites: u8, | |
hdop: f32, | |
} | |
#[derive(Debug, PartialEq)] | |
pub(crate) enum NmeaMessage { | |
GnsFixData(GnsFixData), | |
} | |
fn digitn(n: usize) -> impl Fn(&[u8]) -> IResult<&[u8], &[u8]> { | |
move |s| Ok(take_while_m_n(n, n, |char| is_digit(char))(s)?) | |
} | |
fn parse_gns(start: &[u8]) -> nom::IResult<&[u8], GnsFixData, ()> { | |
let (s, _) = tag("$GNGNS,")(start)?; | |
let (s, hours) = digitn(2)(s)?; | |
let (s, minutes) = digitn(2)(s)?; | |
let (s, seconds) = digitn(2)(s)?; | |
let (s, _) = tag(".")(s)?; | |
let (s, milliseconds) = digitn(2)(s)?; | |
let (s, _) = tag(",")(s)?; | |
let (s, lat) = double(s)?; | |
let (s, _) = tag(",")(s)?; | |
let (s, lat_dir) = alt(( | |
map(tag("N"), |_| LatitudeDirection::North), | |
map(tag("S"), |_| LatitudeDirection::South), | |
))(s)?; | |
let (s, _) = tag(",")(s)?; | |
let (s, lon) = double(s)?; | |
let (s, _) = tag(",")(s)?; | |
let (s, lon_dir) = alt(( | |
map(tag("E"), |_| LongitudeDirection::East), | |
map(tag("W"), |_| LongitudeDirection::West), | |
))(s)?; | |
let (s, _) = tag(",")(s)?; | |
let (s, _mode) = alpha0(s)?; | |
let (s, _) = tag(",")(s)?; | |
let (s, satellites) = map(digit1, |num| { | |
u8::from_str(unsafe { core::str::from_utf8_unchecked(num) }).unwrap() | |
})(s)?; | |
let (s, _) = tag(",")(s)?; | |
let (s, hdop) = float(s)?; | |
let (s, _) = take_until("*")(s)?; | |
let (s, _) = tag("*")(s)?; | |
let (s, included_crc) = map(take_while_m_n(2, 2, |s| is_hex_digit(s)), |val| { | |
u8::from_str_radix(unsafe { core::str::from_utf8_unchecked(val) }, 16).unwrap() | |
})(s)?; | |
let (crc_start, _) = tag("$")(start)?; | |
let (_, crc_bytes) = take_until("*")(crc_start)?; | |
let mut calculated_crc = 0; | |
for n in crc_bytes { | |
calculated_crc ^= n; | |
} | |
if included_crc != calculated_crc { | |
return Err(nom::Err::Failure(()); | |
} | |
assert_eq!(included_crc, calculated_crc); | |
Ok(( | |
s, | |
GnsFixData { | |
lat: Latitude { lat, dir: lat_dir }, | |
lon: Longitude { lon, dir: lon_dir }, | |
utc: Time::try_from_hms_milli( | |
// This is safe because nom makes sure these are digits | |
u8::from_str(unsafe { core::str::from_utf8_unchecked(hours) }).unwrap(), | |
u8::from_str(unsafe { core::str::from_utf8_unchecked(minutes) }).unwrap(), | |
u8::from_str(unsafe { core::str::from_utf8_unchecked(seconds) }).unwrap(), | |
u16::from_str(unsafe { core::str::from_utf8_unchecked(milliseconds) }).unwrap(), | |
) | |
.unwrap(), | |
satellites, | |
hdop, | |
}, | |
)) | |
} | |
pub(crate) fn parse(s: &[u8]) -> nom::IResult<&[u8], NmeaMessage, ()> { | |
nom::combinator::map( | |
|s| { | |
let (s, res) = parse_gns(s)?; | |
Ok((s, NmeaMessage::GnsFixData(res))) | |
}, | |
|v| v, | |
)(s) | |
// alt(( | |
// nom::combinator::map(|s| { | |
// let (s, res) = parse_gns(s)?; | |
// Ok((s, NmeaMessage::GnsFixData(res))) | |
// }, |v| v), | |
// nom::combinator::map(|s| { | |
// let (s, res) = parse_gns(s)?; | |
// Ok((s, NmeaMessage::GnsFixData(res))) | |
// }, |v| v), | |
// ))(s) | |
} | |
#[cfg(test)] | |
mod tests { | |
use time::Time; | |
#[test] | |
fn parses_gns() { | |
use super::{ | |
GnsFixData, Latitude, LatitudeDirection, Longitude, LongitudeDirection, NmeaMessage, | |
}; | |
assert_eq!( | |
super::parse(b"$GNGNS,112257.00,3844.24011,N,00908.43828,W,AN,03,10.5,,,,*49\r\n"), | |
Ok(( | |
&b""[..], | |
NmeaMessage::GnsFixData(super::GnsFixData { | |
lat: Latitude { | |
lat: 3844.24011, | |
dir: LatitudeDirection::North | |
}, | |
lon: Longitude { | |
lon: 00908.43828, | |
dir: LongitudeDirection::West | |
}, | |
utc: Time::try_from_hms_milli(11, 22, 57, 00).unwrap(), | |
satellites: 3, | |
hdop: 10.5 | |
}) | |
)) | |
) | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment