kinavis_nmea2000/
gnss.rs

1//! GNSS position data, PGN 129029: the full fix, once per second.
2
3use core::fmt;
4
5use kinavis_kernel::geodesy::Height;
6use kinavis_kernel::gnss::{Dop, FixType, GnssFix};
7use kinavis_kernel::position::Position;
8use kinavis_kernel::time::{Instant, Utc};
9use kinavis_kernel::units::Distance;
10
11use crate::error::Nmea2000Error;
12use crate::fields::{distance, position, resolution, value_of, Fields};
13use crate::frame::Payload;
14
15/// Satellite system(s) of a fix.
16///
17/// `#[non_exhaustive]`; match with a wildcard arm.
18#[non_exhaustive]
19#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
20#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
21pub enum GnssSystem {
22    /// GPS.
23    Gps,
24    /// GLONASS.
25    Glonass,
26    /// GPS and GLONASS together.
27    GpsAndGlonass,
28    /// GPS with SBAS (WAAS, EGNOS) corrections.
29    GpsWithSbas,
30    /// GPS with SBAS, and GLONASS.
31    GpsWithSbasAndGlonass,
32    /// Chayka.
33    Chayka,
34    /// Integrated navigation system.
35    Integrated,
36    /// Surveyed position.
37    Surveyed,
38    /// Galileo.
39    Galileo,
40    /// Reserved code, kept as received.
41    Reserved(u8),
42}
43
44impl GnssSystem {
45    /// System for a code; the field has no "not available" value.
46    #[must_use]
47    pub const fn from_code(code: u8) -> Self {
48        match code {
49            0 => Self::Gps,
50            1 => Self::Glonass,
51            2 => Self::GpsAndGlonass,
52            3 => Self::GpsWithSbas,
53            4 => Self::GpsWithSbasAndGlonass,
54            5 => Self::Chayka,
55            6 => Self::Integrated,
56            7 => Self::Surveyed,
57            8 => Self::Galileo,
58            _ => Self::Reserved(code),
59        }
60    }
61}
62
63impl fmt::Display for GnssSystem {
64    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
65        match self {
66            Self::Gps => f.write_str("GPS"),
67            Self::Glonass => f.write_str("GLONASS"),
68            Self::GpsAndGlonass => f.write_str("GPS and GLONASS"),
69            Self::GpsWithSbas => f.write_str("GPS with SBAS"),
70            Self::GpsWithSbasAndGlonass => f.write_str("GPS with SBAS, and GLONASS"),
71            Self::Chayka => f.write_str("Chayka"),
72            Self::Integrated => f.write_str("integrated navigation"),
73            Self::Surveyed => f.write_str("surveyed"),
74            Self::Galileo => f.write_str("Galileo"),
75            Self::Reserved(code) => write!(f, "reserved system {code}"),
76        }
77    }
78}
79
80/// Receiver integrity monitoring status.
81///
82/// `#[non_exhaustive]`; match with a wildcard arm.
83#[non_exhaustive]
84#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
85#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
86pub enum Integrity {
87    /// No integrity checking.
88    NotChecked,
89    /// Checked, safe.
90    Safe,
91    /// Checked, caution.
92    Caution,
93}
94
95/// GNSS position data, PGN 129029.
96///
97/// Position, time, constellation, method and quality — the content of the
98/// kernel's [`GnssFix`], built by [`GnssPosition::fix`] when time and position
99/// are both present.
100#[derive(Debug, Clone, Copy, PartialEq)]
101#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
102pub struct GnssPosition {
103    /// Sequence identifier, if sent.
104    pub sid: Option<u8>,
105    /// Fix time, from the date in days and time of day in 1e-4 s.
106    pub taken_at: Option<Instant<Utc>>,
107    /// Position, 1e-16° resolution.
108    pub position: Option<Position>,
109    /// Height above the WGS 84 ellipsoid, 1e-6 m resolution.
110    pub altitude: Option<Height>,
111    /// Satellite system.
112    pub system: GnssSystem,
113    /// Fix method; `None` for a code without a kernel equivalent.
114    pub method: Option<FixType>,
115    /// Integrity status; `None` for a reserved code.
116    pub integrity: Option<Integrity>,
117    /// Satellites in use.
118    pub satellites: Option<u8>,
119    /// HDOP, 0.01 resolution.
120    pub hdop: Option<Dop>,
121    /// PDOP, 0.01 resolution.
122    pub pdop: Option<Dop>,
123    /// Geoidal separation, 0.01 m resolution; altitude minus this is height
124    /// above MSL.
125    pub geoidal_separation: Option<Distance>,
126    /// Number of reference stations that follow (not read).
127    pub reference_stations: Option<u8>,
128}
129
130impl GnssPosition {
131    /// Fixed part of the group, before the reference stations.
132    const NEEDED: usize = 43;
133
134    pub(crate) fn decode(payload: &Payload) -> Result<Self, Nmea2000Error> {
135        let field = Fields::of(payload, Self::NEEDED)?;
136        // Four bits.
137        #[allow(clippy::cast_possible_truncation)]
138        let system = GnssSystem::from_code(field.raw_bits(31 * 8, 4) as u8);
139        let method = match field.raw_bits(31 * 8 + 4, 4) {
140            0 => Some(FixType::None),
141            1 => Some(FixType::Autonomous),
142            2 => Some(FixType::Differential),
143            3 => Some(FixType::Precise),
144            4 => Some(FixType::RtkFixed),
145            5 => Some(FixType::RtkFloat),
146            6 => Some(FixType::Estimated),
147            7 => Some(FixType::Manual),
148            8 => Some(FixType::Simulated),
149            _ => None,
150        };
151        let integrity = match field.raw_bits(32 * 8, 2) {
152            0 => Some(Integrity::NotChecked),
153            1 => Some(Integrity::Safe),
154            2 => Some(Integrity::Caution),
155            _ => None,
156        };
157        Ok(Self {
158            sid: field.u8(0),
159            taken_at: taken_at(field.unsigned(1, 2), field.unsigned(3, 4)),
160            position: position(
161                field.signed(7, 8),
162                field.signed(15, 8),
163                resolution::PRECISE_POSITION_DEG,
164            )?,
165            altitude: distance(field.signed(23, 8), resolution::ALTITUDE_M, "altitude")?
166                .map(Height::above_ellipsoid),
167            system,
168            method,
169            integrity,
170            satellites: field.u8(33),
171            hdop: dop(field.signed(34, 2))?,
172            pdop: dop(field.signed(36, 2))?,
173            geoidal_separation: distance(
174                field.signed(38, 4),
175                resolution::CENTIMETRE_M,
176                "geoidal separation",
177            )?,
178            reference_stations: field.u8(42),
179        })
180    }
181
182    /// Kernel fix, if time and position are both present.
183    #[must_use]
184    pub fn fix(&self) -> Option<GnssFix> {
185        let mut builder = GnssFix::builder(self.taken_at?, self.position?);
186        if let Some(method) = self.method {
187            builder = builder.fix_type(method);
188        }
189        if let Some(satellites) = self.satellites {
190            builder = builder.satellites(satellites);
191        }
192        if let Some(hdop) = self.hdop {
193            builder = builder.hdop(hdop);
194        }
195        if let Some(pdop) = self.pdop {
196            builder = builder.pdop(pdop);
197        }
198        Some(builder.build())
199    }
200}
201
202/// Date in days since 1970 and time of day in 1e-4 s.
203fn taken_at(days: Option<u64>, time: Option<u64>) -> Option<Instant<Utc>> {
204    let days = i64::try_from(days?).ok()?;
205    let time = i64::try_from(time?).ok()?;
206    // 1e-4 s = 100 000 ns.
207    let nanos = days
208        .checked_mul(86_400 * 1_000_000_000)?
209        .checked_add(time.checked_mul(100_000)?)?;
210    Some(Instant::from_unix_nanos(nanos))
211}
212
213/// Dilution of precision in 0.01 units.
214fn dop(field: Option<i64>) -> Result<Option<Dop>, Nmea2000Error> {
215    let Some(field) = field else {
216        return Ok(None);
217    };
218    // At most 16 bits.
219    #[allow(clippy::cast_precision_loss)]
220    Dop::new(field as f64 * resolution::DOP)
221        .map(Some)
222        .map_err(value_of("dilution of precision"))
223}
kinavis_nmea2000/gnss.rs

kinavis_nmea2000/
gnss.rs
