From 6b8f8ca5cdc8115730b8c3335804ed604364fc43 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Kr=C3=B3lak?= Date: Mon, 10 Apr 2023 09:24:22 +0200 Subject: [PATCH] Added parsing zfo and ztg messages --- README.md | 2 + src/parse.rs | 2 + src/parser.rs | 5 +- src/sentences/mod.rs | 2 + src/sentences/utils.rs | 24 ++++--- src/sentences/zfo.rs | 27 +++++-- src/sentences/ztg.rs | 120 ++++++++++++++++++++++++++++++++ tests/all_supported_messages.rs | 4 ++ 8 files changed, 167 insertions(+), 19 deletions(-) create mode 100644 src/sentences/ztg.rs diff --git a/README.md b/README.md index 5827766..f63bf71 100644 --- a/README.md +++ b/README.md @@ -31,6 +31,8 @@ NMEA Standard Sentences - VHW - VTG * - ZDA +- ZFO +- ZTG Other Sentences - TXT * diff --git a/src/parse.rs b/src/parse.rs index 4116234..fceecd3 100644 --- a/src/parse.rs +++ b/src/parse.rs @@ -119,6 +119,7 @@ pub enum ParseResult { VTG(VtgData), ZDA(ZdaData), ZFO(ZfoData), + ZTG(ZtgData), PGRMZ(PgrmzData), /// A message that is not supported by the crate and cannot be parsed. Unsupported(SentenceType), @@ -148,6 +149,7 @@ impl From<&ParseResult> for SentenceType { ParseResult::VHW(_) => SentenceType::VHW, ParseResult::VTG(_) => SentenceType::VTG, ParseResult::ZFO(_) => SentenceType::ZFO, + ParseResult::ZTG(_) => SentenceType::ZTG, ParseResult::PGRMZ(_) => SentenceType::RMZ, ParseResult::ZDA(_) => SentenceType::ZDA, ParseResult::Unsupported(sentence_type) => *sentence_type, diff --git a/src/parser.rs b/src/parser.rs index 45dd904..4e49f54 100644 --- a/src/parser.rs +++ b/src/parser.rs @@ -365,8 +365,9 @@ impl<'a> Nmea { | ParseResult::MWV(_) | ParseResult::MDA(_) | ParseResult::VHW(_) - | ParseResult::ZDA(_) => return Ok(FixType::Invalid), - | ParseResult::ZFO(_) => return Ok(FixType::Invalid), + | ParseResult::ZDA(_) + | ParseResult::ZFO(_) + | ParseResult::ZTG(_) => return Ok(FixType::Invalid), ParseResult::Unsupported(_) => { return Ok(FixType::Invalid); diff --git a/src/sentences/mod.rs b/src/sentences/mod.rs index 56a6798..8b075aa 100644 --- a/src/sentences/mod.rs +++ b/src/sentences/mod.rs @@ -24,6 +24,7 @@ mod vhw; mod vtg; mod zda; mod zfo; +mod ztg; pub(crate) mod faa_mode; mod fix_type; @@ -56,6 +57,7 @@ pub use { vtg::{parse_vtg, VtgData}, zda::{parse_zda, ZdaData}, zfo::{parse_zfo, ZfoData}, + ztg::{parse_ztg, ZtgData}, }; pub(crate) fn nom_parse_failure(inp: &str) -> nom::Err> { diff --git a/src/sentences/utils.rs b/src/sentences/utils.rs index f274ce5..9d66f9d 100644 --- a/src/sentences/utils.rs +++ b/src/sentences/utils.rs @@ -1,7 +1,7 @@ use core::str; use arrayvec::ArrayString; -use chrono::{NaiveDate, NaiveTime, Duration}; +use chrono::{Duration, NaiveDate, NaiveTime}; use nom::{ branch::alt, bytes::complete::{tag, take, take_until}, @@ -56,7 +56,6 @@ const MILLISECS_PER_MINUTE: i64 = 60000; /// The number of milliseconds in a hour. const MILLISECS_PER_HOUR: i64 = 3600000; - pub(crate) fn parse_duration_hms(i: &str) -> IResult<&str, Duration> { map_res( tuple(( @@ -77,13 +76,12 @@ pub(crate) fn parse_duration_hms(i: &str) -> IResult<&str, Duration> { if sec >= 60. { return Err("Invalid time: sec >= 60"); } - Ok( - Duration::milliseconds( - hour * MILLISECS_PER_HOUR + Ok(Duration::milliseconds( + hour * MILLISECS_PER_HOUR + minutes * MILLISECS_PER_MINUTE + (sec.trunc() as i64) * MILLISECS_PER_SECOND - + (sec.fract() * 1_000f64).round() as i64) - ) + + (sec.fract() * 1_000f64).round() as i64, + )) }, )(i) } @@ -244,13 +242,19 @@ mod tests { let (_, time) = parse_duration_hms("125619,").unwrap(); assert_eq!(time.num_hours(), 12); assert_eq!(time.num_minutes(), 12 * 60 + 56); - assert_eq!(time.num_seconds(), 12 * 60*60 + 56 * 60 + 19); - assert_eq!(time.num_nanoseconds().unwrap(), (12 * 60 * 60 + 56 * 60 + 19) * 1_000_000_000); + assert_eq!(time.num_seconds(), 12 * 60 * 60 + 56 * 60 + 19); + assert_eq!( + time.num_nanoseconds().unwrap(), + (12 * 60 * 60 + 56 * 60 + 19) * 1_000_000_000 + ); let (_, time) = parse_duration_hms("125619.5,").unwrap(); assert_eq!(time.num_hours(), 12); assert_eq!(time.num_minutes(), 12 * 60 + 56); assert_eq!(time.num_seconds(), 12 * 60 * 60 + 56 * 60 + 19); - assert_eq!(time.num_nanoseconds().unwrap(), (12 * 60 * 60 + 56 * 60 + 19) * 1_000_000_000 + 500_000_000); + assert_eq!( + time.num_nanoseconds().unwrap(), + (12 * 60 * 60 + 56 * 60 + 19) * 1_000_000_000 + 500_000_000 + ); } #[test] diff --git a/src/sentences/zfo.rs b/src/sentences/zfo.rs index 69631c0..148dd86 100644 --- a/src/sentences/zfo.rs +++ b/src/sentences/zfo.rs @@ -1,12 +1,10 @@ -use chrono::{NaiveTime, Duration}; use arrayvec::ArrayString; -use nom::{ - bytes::complete::is_not, character::complete::char, combinator::opt, -}; +use chrono::{Duration, NaiveTime}; +use nom::{bytes::complete::is_not, character::complete::char, combinator::opt}; use crate::{ parse::NmeaSentence, - sentences::utils::{parse_hms, parse_duration_hms}, + sentences::utils::{parse_duration_hms, parse_hms}, Error, SentenceType, }; @@ -18,6 +16,11 @@ const MAX_LEN: usize = 64; /// | | | | /// $--ZFO,hhmmss.ss,hhmmss.ss,c--c*hh ///``` +/// Field Number: +/// 1. Universal Time Coordinated (UTC) hh is hours, mm is minutes, ss.ss is seconds. +/// 2. Elapsed Time +/// 3. Origin Waypoint ID +/// 4. Checksum #[derive(Debug, PartialEq)] pub struct ZfoData { pub fix_time: Option, @@ -81,7 +84,12 @@ mod tests { fn test_parse_zfo() { assert_eq!( ZfoData { - fix_duration: Some(Duration::hours(4) + Duration::minutes(23) + Duration::seconds(59) + Duration::milliseconds(170)), + fix_duration: Some( + Duration::hours(4) + + Duration::minutes(23) + + Duration::seconds(59) + + Duration::milliseconds(170) + ), fix_time: NaiveTime::from_hms_milli_opt(14, 58, 32, 120), waypoint_id: Some(ArrayString::from("WPT").unwrap()), }, @@ -97,7 +105,12 @@ mod tests { ); assert_eq!( ZfoData { - fix_duration: Some(Duration::hours(4) + Duration::minutes(23) + Duration::seconds(59) + Duration::milliseconds(170)), + fix_duration: Some( + Duration::hours(4) + + Duration::minutes(23) + + Duration::seconds(59) + + Duration::milliseconds(170) + ), fix_time: None, waypoint_id: None, }, diff --git a/src/sentences/ztg.rs b/src/sentences/ztg.rs new file mode 100644 index 0000000..634660e --- /dev/null +++ b/src/sentences/ztg.rs @@ -0,0 +1,120 @@ +use arrayvec::ArrayString; +use chrono::{Duration, NaiveTime}; +use nom::{bytes::complete::is_not, character::complete::char, combinator::opt}; + +use crate::{ + parse::NmeaSentence, + sentences::utils::{parse_duration_hms, parse_hms}, + Error, SentenceType, +}; + +const MAX_LEN: usize = 64; + +/// ZTG - UTC & Time to Destination Waypoint +///```text +/// 1 2 3 4 +/// | | | | +/// $--ZTG,hhmmss.ss,hhmmss.ss,c--c*hh +///``` +/// Field Number: +/// 1. UTC of observation hh is hours, mm is minutes, ss.ss is seconds. +/// 2. Time Remaining +/// 3. Destination Waypoint ID +/// 4. Checksum +#[derive(Debug, PartialEq)] +pub struct ZtgData { + pub fix_time: Option, + pub fix_duration: Option, + pub waypoint_id: Option>, +} + +fn do_parse_ztg(i: &str) -> Result { + // 1. UTC Time or observation + let (i, fix_time) = opt(parse_hms)(i)?; + let (i, _) = char(',')(i)?; + // 2. Duration + let (i, fix_duration) = opt(parse_duration_hms)(i)?; + let (i, _) = char(',')(i)?; + + // 12. Waypoint ID + let (_i, waypoint_id) = opt(is_not(",*"))(i)?; + + let waypoint_id = if let Some(waypoint_id) = waypoint_id { + Some( + ArrayString::from(waypoint_id) + .map_err(|_e| Error::SentenceLength(waypoint_id.len()))?, + ) + } else { + None + }; + + Ok(ZtgData { + fix_time, + fix_duration, + waypoint_id, + }) +} + +/// # Parse ZTG message +/// +/// See: +pub fn parse_ztg(sentence: NmeaSentence) -> Result { + if sentence.message_id != SentenceType::ZTG { + Err(Error::WrongSentenceHeader { + expected: SentenceType::ZTG, + found: sentence.message_id, + }) + } else { + Ok(do_parse_ztg(sentence.data)?) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::{parse::parse_nmea_sentence, Error}; + + fn run_parse_ztg(line: &str) -> Result { + let s = parse_nmea_sentence(line).expect("ZTG sentence initial parse failed"); + assert_eq!(s.checksum, s.calc_checksum()); + parse_ztg(s) + } + + #[test] + fn test_parse_ztg() { + assert_eq!( + ZtgData { + fix_duration: Some( + Duration::hours(4) + + Duration::minutes(23) + + Duration::seconds(59) + + Duration::milliseconds(170) + ), + fix_time: NaiveTime::from_hms_milli_opt(14, 58, 32, 120), + waypoint_id: Some(ArrayString::from("WPT").unwrap()), + }, + run_parse_ztg("$GPZTG,145832.12,042359.17,WPT*24").unwrap() + ); + assert_eq!( + ZtgData { + fix_duration: None, + fix_time: None, + waypoint_id: None, + }, + run_parse_ztg("$GPZTG,,,*72").unwrap() + ); + assert_eq!( + ZtgData { + fix_duration: Some( + Duration::hours(4) + + Duration::minutes(23) + + Duration::seconds(59) + + Duration::milliseconds(170) + ), + fix_time: None, + waypoint_id: None, + }, + run_parse_ztg("$GPZTG,,042359.17,*53").unwrap() + ); + } +} diff --git a/tests/all_supported_messages.rs b/tests/all_supported_messages.rs index 8081575..0de48e3 100644 --- a/tests/all_supported_messages.rs +++ b/tests/all_supported_messages.rs @@ -41,6 +41,10 @@ fn test_all_supported_messages() { (SentenceType::VTG, "$GPVTG,360.0,T,348.7,M,000.0,N,000.0,K*43"), // ZDA (SentenceType::ZDA, "$GPZDA,160012.71,11,03,2004,-1,00*7D"), + // ZFO + (SentenceType::ZFO, "$GPZFO,145832.12,042359.17,WPT*3E"), + // ZTG + (SentenceType::ZTG, "$GPZTG,145832.12,042359.17,WPT*24"), ] .into_iter() .collect::>();