From 7b0ef1f405088652e5a21fa04f2ff435a4da5664 Mon Sep 17 00:00:00 2001
From: Tobias Bieniek <tobias@bieniek.cloud>
Date: Thu, 6 Oct 2022 21:04:04 +0200
Subject: [PATCH] Implement `PGRMZ` support

---
 src/error.rs         |   7 +++
 src/parse.rs         |   2 +
 src/parser.rs        |  18 ++++++--
 src/sentences/mod.rs |   2 +
 src/sentences/rmz.rs | 102 +++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 128 insertions(+), 3 deletions(-)
 create mode 100644 src/sentences/rmz.rs

diff --git a/src/error.rs b/src/error.rs
index df3e961..a86cecb 100644
--- a/src/error.rs
+++ b/src/error.rs
@@ -30,6 +30,8 @@ pub enum Error<'a> {
     EmptyNavConfig,
     /// Invalid sentence number field in nmea sentence of type GSV
     InvalidGsvSentenceNum,
+    /// An unknown talker ID was found in the NMEA message.
+    UnknownTalkerId { expected: &'a str, found: &'a str },
 }
 
 impl<'a> From<nom::Err<nom::error::Error<&'a str>>> for Error<'a> {
@@ -81,6 +83,11 @@ impl<'a> fmt::Display for Error<'a> {
                 f,
                 "Invalid senetence number field in nmea sentence of type GSV"
             ),
+            Error::UnknownTalkerId { expected, found } => write!(
+                f,
+                "Unknown Talker ID (expected = '{}', found = '{}')",
+                expected, found
+            ),
         }
     }
 }
diff --git a/src/parse.rs b/src/parse.rs
index 6acb450..4cbf0dc 100644
--- a/src/parse.rs
+++ b/src/parse.rs
@@ -108,6 +108,7 @@ pub enum ParseResult {
     RMC(RmcData),
     TXT(TxtData),
     VTG(VtgData),
+    PGRMZ(PgrmzData),
     /// A message that is not supported by the crate and cannot be parsed.
     Unsupported(SentenceType),
 }
@@ -152,6 +153,7 @@ pub fn parse_str(sentence_input: &str) -> Result<ParseResult, Error> {
             SentenceType::GLL => parse_gll(nmea_sentence).map(ParseResult::GLL),
             SentenceType::TXT => parse_txt(nmea_sentence).map(ParseResult::TXT),
             SentenceType::GNS => parse_gns(nmea_sentence).map(ParseResult::GNS),
+            SentenceType::RMZ => parse_pgrmz(nmea_sentence).map(ParseResult::PGRMZ),
             sentence_type => Ok(ParseResult::Unsupported(sentence_type)),
         }
     } else {
diff --git a/src/parser.rs b/src/parser.rs
index 3e6c70a..b6bdca8 100644
--- a/src/parser.rs
+++ b/src/parser.rs
@@ -353,9 +353,11 @@ impl<'a> Nmea {
                 self.merge_txt_data(txt_data);
                 return Ok(FixType::Invalid);
             }
-            ParseResult::BWC(_) | ParseResult::BOD(_) | ParseResult::GBS(_) => {
-                return Ok(FixType::Invalid)
-            }
+            ParseResult::BWC(_)
+            | ParseResult::BOD(_)
+            | ParseResult::GBS(_)
+            | ParseResult::PGRMZ(_) => return Ok(FixType::Invalid),
+
             ParseResult::Unsupported(_) => {
                 return Ok(FixType::Invalid);
             }
@@ -679,6 +681,10 @@ define_sentence_type_enum! {
     /// - [`SentenceType::ZDA`]
     /// - [`SentenceType::ZFO`]
     /// - [`SentenceType::ZTG`]
+    ///
+    /// ### Vendor extensions
+    ///
+    /// - [`SentenceType::RMZ`]
     #[derive(Debug, Hash, PartialEq, Eq, Clone, Copy)]
     #[repr(u32)]
     #[allow(rustdoc::bare_urls)]
@@ -971,6 +977,12 @@ define_sentence_type_enum! {
         ///
         /// Type: `Navigation`
         RMC,
+        /// PGRMZ - Garmin Altitude
+        ///
+        /// <https://gpsd.gitlab.io/gpsd/NMEA.html#_pgrmz_garmin_altitude>
+        ///
+        /// Type: `Vendor extensions`
+        RMZ,
         /// ROT - Rate Of Turn
         ///
         /// <https://gpsd.gitlab.io/gpsd/NMEA.html#_rot_rate_of_turn>
diff --git a/src/sentences/mod.rs b/src/sentences/mod.rs
index e2ce52b..3acee99 100644
--- a/src/sentences/mod.rs
+++ b/src/sentences/mod.rs
@@ -9,6 +9,7 @@ mod gns;
 mod gsa;
 mod gsv;
 mod rmc;
+mod rmz;
 mod txt;
 mod utils;
 mod vtg;
@@ -30,6 +31,7 @@ pub use {
     gsa::{parse_gsa, GsaData},
     gsv::{parse_gsv, GsvData},
     rmc::{parse_rmc, RmcData, RmcStatusOfFix},
+    rmz::{parse_pgrmz, PgrmzData},
     txt::{parse_txt, TxtData},
     vtg::{parse_vtg, VtgData},
 };
diff --git a/src/sentences/rmz.rs b/src/sentences/rmz.rs
new file mode 100644
index 0000000..76bfebe
--- /dev/null
+++ b/src/sentences/rmz.rs
@@ -0,0 +1,102 @@
+use nom::{
+    character::complete::{char, one_of},
+    IResult,
+};
+
+use crate::sentences::utils::number;
+use crate::{parse::NmeaSentence, Error, SentenceType};
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum PgrmzFixType {
+    NoFix,
+    TwoDimensional,
+    ThreeDimensional,
+}
+
+/// PGRMZ - Garmin Altitude
+///
+/// <https://gpsd.gitlab.io/gpsd/NMEA.html#_pgrmz_garmin_altitude>
+///
+/// ```text
+///          1  2 3  4
+///          |  | |  |
+///  $PGRMZ,hhh,f,M*hh<CR><LF>
+/// ```
+///
+/// 1. Current Altitude Feet
+/// 2. `f` = feet
+/// 3. Mode (`1` = no fix, `2` = 2D fix, `3` = 3D fix)
+/// 4. Checksum
+///
+/// Example: `$PGRMZ,2282,f,3*21`
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub struct PgrmzData {
+    /// Current altitude in feet
+    pub altitude: u32,
+    pub fix_type: PgrmzFixType,
+}
+
+fn do_parse_pgrmz(i: &str) -> IResult<&str, PgrmzData> {
+    let (i, altitude) = number::<u32>(i)?;
+    let (i, _) = char(',')(i)?;
+    let (i, _) = char('f')(i)?;
+    let (i, _) = char(',')(i)?;
+    let (i, fix_type) = one_of("123")(i)?;
+    let fix_type = match fix_type {
+        '1' => PgrmzFixType::NoFix,
+        '2' => PgrmzFixType::TwoDimensional,
+        '3' => PgrmzFixType::ThreeDimensional,
+        _ => unreachable!(),
+    };
+    Ok((i, PgrmzData { altitude, fix_type }))
+}
+
+/// # Parse PGRMZ message
+///
+/// Example:
+///
+/// `$PGRMZ,2282,f,3*21`
+pub fn parse_pgrmz(sentence: NmeaSentence) -> Result<PgrmzData, Error> {
+    if sentence.message_id != SentenceType::RMZ {
+        Err(Error::WrongSentenceHeader {
+            expected: SentenceType::RMZ,
+            found: sentence.message_id,
+        })
+    } else if sentence.talker_id != "PG" {
+        Err(Error::UnknownTalkerId {
+            expected: "PG",
+            found: sentence.talker_id,
+        })
+    } else {
+        Ok(do_parse_pgrmz(sentence.data)?.1)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::parse::parse_nmea_sentence;
+
+    #[test]
+    fn test_successful_parse() {
+        let s = parse_nmea_sentence("$PGRMZ,2282,f,3*21").unwrap();
+        assert_eq!(s.checksum, s.calc_checksum());
+        assert_eq!(s.checksum, 0x21);
+
+        let data = parse_pgrmz(s).unwrap();
+        assert_eq!(data.altitude, 2282);
+        assert_eq!(data.fix_type, PgrmzFixType::ThreeDimensional);
+    }
+
+    #[test]
+    fn test_wrong_talker_id() {
+        let s = parse_nmea_sentence("$XXRMZ,2282,f,3*21").unwrap();
+        assert!(matches!(
+            parse_pgrmz(s),
+            Err(Error::UnknownTalkerId {
+                expected: "PG",
+                found: "XX"
+            })
+        ));
+    }
+}