umsh_ulcp/
gnss.rs

1//! Codecs for the positioning properties (`PROP_GNSS_*`).
2//!
3//! The receiver's view of the world is one [`GnssSnapshot`], but it
4//! reaches a host as five independent properties so that a host that only
5//! wants a position never pays for the rest. [`GnssSnapshot::encode`] and
6//! [`GnssSnapshot::absorb`] are the two halves of that split: a device
7//! encodes whichever property was asked for, and a host folds the
8//! properties it read back into one snapshot.
9//!
10//! Two of the five always answer: `PROP_GNSS_FIX` and
11//! `PROP_GNSS_SATELLITES` read `0` when the receiver is off or searching,
12//! because "no fix" is a fact the device is sure of. The three that
13//! describe a position — location, altitude, precision — answer the empty
14//! value until there *is* a position to describe.
15
16use crate::ids::prop;
17
18/// Maximum length of a `PROP_GNSS_LOCATION` value, and of any value this
19/// module encodes.
20pub const MAX_LOCATION_LEN: usize = 7;
21
22/// Largest encoded property value produced here.
23pub const MAX_VALUE_LEN: usize = MAX_LOCATION_LEN;
24
25/// Assumed user-equivalent range error, in decimeters, used to turn a
26/// dilution of precision into the horizontal-accuracy estimate reported
27/// by `PROP_GNSS_PRECISION`.
28const UERE_DM: u32 = 50;
29
30/// `PROP_GNSS_FIX` values.
31#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
32pub enum FixKind {
33    /// No position solution.
34    #[default]
35    None = 0,
36    /// A two-dimensional solution: position without altitude.
37    TwoD = 1,
38    /// A three-dimensional solution.
39    ThreeD = 2,
40}
41
42impl FixKind {
43    /// The wire code for this fix quality.
44    pub const fn code(self) -> u8 {
45        self as u8
46    }
47
48    /// Strict conversion from a wire code.
49    pub const fn from_code(code: u8) -> Option<Self> {
50        match code {
51            0 => Some(Self::None),
52            1 => Some(Self::TwoD),
53            2 => Some(Self::ThreeD),
54            _ => None,
55        }
56    }
57
58    /// Whether there is a position solution at all.
59    pub const fn is_fixed(self) -> bool {
60        !matches!(self, Self::None)
61    }
62}
63
64/// Encode or decode error.
65#[derive(Clone, Copy, Debug, PartialEq, Eq)]
66pub enum GnssError {
67    /// A value whose length, range, or code the property does not allow.
68    Malformed,
69    /// The output buffer cannot hold the encoded value.
70    BufferTooSmall,
71    /// The key is not a positioning property this module encodes.
72    UnknownProperty,
73}
74
75/// The receiver's current view of position and constellation.
76///
77/// The default is what a disabled or searching receiver reports: no fix,
78/// no satellites, nothing positional to say.
79#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
80pub struct GnssSnapshot {
81    /// Fix quality.
82    pub fix: FixKind,
83    location: [u8; MAX_LOCATION_LEN],
84    location_len: u8,
85    /// Altitude above the WGS-84 ellipsoid in meters, matching the units
86    /// of node identity option 2. `None` without a three-dimensional fix.
87    pub altitude_m: Option<i32>,
88    /// Estimated horizontal accuracy in decimeters.
89    pub accuracy_dm: Option<u16>,
90    /// Satellites contributing to the solution.
91    pub sats_used: u8,
92    /// Satellites the receiver can see, whether or not they are used.
93    /// `None` when the receiver does not report it.
94    pub sats_in_view: Option<u8>,
95}
96
97impl GnssSnapshot {
98    /// What a receiver that is off — or on but still searching — reports.
99    pub const SEARCHING: Self = Self {
100        fix: FixKind::None,
101        location: [0; MAX_LOCATION_LEN],
102        location_len: 0,
103        altitude_m: None,
104        accuracy_dm: None,
105        sats_used: 0,
106        sats_in_view: None,
107    };
108
109    /// The encoded location, in the variable-precision interleaved
110    /// format. Empty when there is no position.
111    pub fn location(&self) -> &[u8] {
112        &self.location[..self.location_len as usize]
113    }
114
115    /// Replace the location, silently truncating past [`MAX_LOCATION_LEN`].
116    pub fn set_location(&mut self, bytes: &[u8]) {
117        let len = bytes.len().min(MAX_LOCATION_LEN);
118        self.location = [0; MAX_LOCATION_LEN];
119        self.location[..len].copy_from_slice(&bytes[..len]);
120        self.location_len = len as u8;
121    }
122
123    /// Turn a horizontal dilution of precision, in hundredths, into the
124    /// accuracy estimate `PROP_GNSS_PRECISION` reports.
125    ///
126    /// This is an estimate scaled by an assumed range error, not a
127    /// measured error bound; receivers that report a real accuracy figure
128    /// should set [`accuracy_dm`](Self::accuracy_dm) from that instead.
129    /// The product cannot overflow: the largest dilution the argument can
130    /// express still scales to half of `u16::MAX`.
131    pub const fn accuracy_from_hdop_centi(hdop_centi: u16) -> u16 {
132        ((hdop_centi as u32 * UERE_DM) / 100) as u16
133    }
134
135    /// Encode the value of one positioning property.
136    ///
137    /// Returns the number of bytes written; zero is the empty value, and
138    /// is what the three positional properties answer without a fix.
139    pub fn encode(&self, key: u32, out: &mut [u8]) -> Result<usize, GnssError> {
140        let mut write = |bytes: &[u8]| -> Result<usize, GnssError> {
141            let dst = out
142                .get_mut(..bytes.len())
143                .ok_or(GnssError::BufferTooSmall)?;
144            dst.copy_from_slice(bytes);
145            Ok(bytes.len())
146        };
147        match key {
148            prop::GNSS_LOCATION => write(self.location()),
149            prop::GNSS_ALTITUDE => match self.altitude_m {
150                Some(meters) => write(&meters.to_le_bytes()),
151                None => Ok(0),
152            },
153            prop::GNSS_FIX => write(&[self.fix.code()]),
154            prop::GNSS_PRECISION => match self.accuracy_dm {
155                Some(dm) => write(&dm.to_le_bytes()),
156                None => Ok(0),
157            },
158            prop::GNSS_SATELLITES => match self.sats_in_view {
159                Some(in_view) => write(&[self.sats_used, in_view]),
160                None => write(&[self.sats_used]),
161            },
162            _ => Err(GnssError::UnknownProperty),
163        }
164    }
165
166    /// Fold one property value back into the snapshot.
167    ///
168    /// A host reads the positioning properties one at a time; this is how
169    /// it reassembles them. The empty value clears the corresponding
170    /// field rather than leaving a stale one behind.
171    pub fn absorb(&mut self, key: u32, value: &[u8]) -> Result<(), GnssError> {
172        match key {
173            prop::GNSS_LOCATION => {
174                if value.len() > MAX_LOCATION_LEN {
175                    return Err(GnssError::Malformed);
176                }
177                self.set_location(value);
178            }
179            prop::GNSS_ALTITUDE => {
180                self.altitude_m = match value {
181                    [] => None,
182                    [a, b, c, d] => Some(i32::from_le_bytes([*a, *b, *c, *d])),
183                    _ => return Err(GnssError::Malformed),
184                };
185            }
186            prop::GNSS_FIX => {
187                let [code] = value else {
188                    return Err(GnssError::Malformed);
189                };
190                self.fix = FixKind::from_code(*code).ok_or(GnssError::Malformed)?;
191            }
192            prop::GNSS_PRECISION => {
193                self.accuracy_dm = match value {
194                    [] => None,
195                    [low, high] => Some(u16::from_le_bytes([*low, *high])),
196                    _ => return Err(GnssError::Malformed),
197                };
198            }
199            prop::GNSS_SATELLITES => match value {
200                [used] => {
201                    self.sats_used = *used;
202                    self.sats_in_view = None;
203                }
204                [used, in_view] => {
205                    self.sats_used = *used;
206                    self.sats_in_view = Some(*in_view);
207                }
208                _ => return Err(GnssError::Malformed),
209            },
210            _ => return Err(GnssError::UnknownProperty),
211        }
212        Ok(())
213    }
214}
215
216/// Whether `key` is one of the positioning properties this module codes.
217pub const fn is_positioning_property(key: u32) -> bool {
218    matches!(
219        key,
220        prop::GNSS_LOCATION
221            | prop::GNSS_ALTITUDE
222            | prop::GNSS_FIX
223            | prop::GNSS_PRECISION
224            | prop::GNSS_SATELLITES
225    )
226}
227
228#[cfg(test)]
229mod tests {
230    use super::*;
231
232    fn fixed() -> GnssSnapshot {
233        let mut snapshot = GnssSnapshot {
234            fix: FixKind::ThreeD,
235            altitude_m: Some(-31),
236            accuracy_dm: Some(62),
237            sats_used: 9,
238            sats_in_view: Some(14),
239            ..GnssSnapshot::SEARCHING
240        };
241        snapshot.set_location(&[0x8a, 0x1f, 0x4c, 0x00, 0xd3]);
242        snapshot
243    }
244
245    #[track_caller]
246    fn round_trip(snapshot: &GnssSnapshot, key: u32, expected: &[u8]) {
247        let mut buf = [0u8; MAX_VALUE_LEN];
248        let len = snapshot.encode(key, &mut buf).unwrap();
249        assert_eq!(&buf[..len], expected, "encoding of {key}");
250        let mut folded = GnssSnapshot::SEARCHING;
251        folded.absorb(key, expected).unwrap();
252        let mut re = [0u8; MAX_VALUE_LEN];
253        let re_len = folded.encode(key, &mut re).unwrap();
254        assert_eq!(&re[..re_len], expected, "re-encoding of {key}");
255    }
256
257    #[test]
258    fn a_fix_encodes_every_property() {
259        let snapshot = fixed();
260        round_trip(
261            &snapshot,
262            prop::GNSS_LOCATION,
263            &[0x8a, 0x1f, 0x4c, 0x00, 0xd3],
264        );
265        round_trip(&snapshot, prop::GNSS_ALTITUDE, &[0xe1, 0xff, 0xff, 0xff]);
266        round_trip(&snapshot, prop::GNSS_FIX, &[2]);
267        round_trip(&snapshot, prop::GNSS_PRECISION, &[62, 0]);
268        round_trip(&snapshot, prop::GNSS_SATELLITES, &[9, 14]);
269    }
270
271    #[test]
272    fn searching_answers_zero_for_facts_and_empty_for_positions() {
273        let snapshot = GnssSnapshot::SEARCHING;
274        round_trip(&snapshot, prop::GNSS_FIX, &[0]);
275        round_trip(&snapshot, prop::GNSS_SATELLITES, &[0]);
276        round_trip(&snapshot, prop::GNSS_LOCATION, &[]);
277        round_trip(&snapshot, prop::GNSS_ALTITUDE, &[]);
278        round_trip(&snapshot, prop::GNSS_PRECISION, &[]);
279    }
280
281    #[test]
282    fn a_two_dimensional_fix_has_a_position_but_no_altitude() {
283        let mut snapshot = fixed();
284        snapshot.fix = FixKind::TwoD;
285        snapshot.altitude_m = None;
286        round_trip(&snapshot, prop::GNSS_FIX, &[1]);
287        round_trip(&snapshot, prop::GNSS_ALTITUDE, &[]);
288        assert_eq!(snapshot.location().len(), 5);
289    }
290
291    #[test]
292    fn absorbing_an_empty_value_clears_a_stale_field() {
293        let mut snapshot = fixed();
294        snapshot.absorb(prop::GNSS_LOCATION, &[]).unwrap();
295        snapshot.absorb(prop::GNSS_ALTITUDE, &[]).unwrap();
296        snapshot.absorb(prop::GNSS_PRECISION, &[]).unwrap();
297        assert_eq!(snapshot.location(), &[] as &[u8]);
298        assert_eq!(snapshot.altitude_m, None);
299        assert_eq!(snapshot.accuracy_dm, None);
300    }
301
302    #[test]
303    fn rejects_malformed_values() {
304        let mut snapshot = GnssSnapshot::SEARCHING;
305        assert_eq!(
306            snapshot.absorb(prop::GNSS_LOCATION, &[0; 8]),
307            Err(GnssError::Malformed)
308        );
309        assert_eq!(
310            snapshot.absorb(prop::GNSS_ALTITUDE, &[0, 0]),
311            Err(GnssError::Malformed)
312        );
313        assert_eq!(
314            snapshot.absorb(prop::GNSS_FIX, &[]),
315            Err(GnssError::Malformed)
316        );
317        assert_eq!(
318            snapshot.absorb(prop::GNSS_FIX, &[3]),
319            Err(GnssError::Malformed)
320        );
321        assert_eq!(
322            snapshot.absorb(prop::GNSS_PRECISION, &[1]),
323            Err(GnssError::Malformed)
324        );
325        assert_eq!(
326            snapshot.absorb(prop::GNSS_SATELLITES, &[1, 2, 3]),
327            Err(GnssError::Malformed)
328        );
329        assert_eq!(
330            snapshot.absorb(prop::TIME, &[]),
331            Err(GnssError::UnknownProperty)
332        );
333    }
334
335    #[test]
336    fn location_truncates_past_the_maximum_precision() {
337        let mut snapshot = GnssSnapshot::SEARCHING;
338        snapshot.set_location(&[1, 2, 3, 4, 5, 6, 7, 8, 9]);
339        assert_eq!(snapshot.location(), &[1, 2, 3, 4, 5, 6, 7]);
340    }
341
342    #[test]
343    fn encode_reports_short_buffers_and_unknown_keys() {
344        let snapshot = fixed();
345        let mut small = [0u8; 3];
346        assert_eq!(
347            snapshot.encode(prop::GNSS_LOCATION, &mut small),
348            Err(GnssError::BufferTooSmall)
349        );
350        let mut buf = [0u8; MAX_VALUE_LEN];
351        assert_eq!(
352            snapshot.encode(prop::GNSS_ENABLED, &mut buf),
353            Err(GnssError::UnknownProperty)
354        );
355    }
356
357    #[test]
358    fn accuracy_scales_dilution_of_precision() {
359        // HDOP 1.00 → 5.0 m → 50 dm.
360        assert_eq!(GnssSnapshot::accuracy_from_hdop_centi(100), 50);
361        // HDOP 2.40 → 12.0 m.
362        assert_eq!(GnssSnapshot::accuracy_from_hdop_centi(240), 120);
363        // The worst dilution the argument can express still fits.
364        assert_eq!(GnssSnapshot::accuracy_from_hdop_centi(u16::MAX), 32_767);
365    }
366
367    #[test]
368    fn fix_codes_round_trip_strictly() {
369        assert_eq!(FixKind::from_code(0), Some(FixKind::None));
370        assert_eq!(FixKind::from_code(2), Some(FixKind::ThreeD));
371        assert_eq!(FixKind::from_code(3), None);
372        assert!(!FixKind::default().is_fixed());
373        assert!(FixKind::TwoD.is_fixed());
374    }
375
376    #[test]
377    fn positioning_properties_are_exactly_the_five() {
378        assert!(is_positioning_property(prop::GNSS_LOCATION));
379        assert!(is_positioning_property(prop::GNSS_SATELLITES));
380        assert!(!is_positioning_property(prop::GNSS_ENABLED));
381        assert!(!is_positioning_property(prop::TIME));
382    }
383}
umsh_ulcp/gnss.rs

umsh_ulcp/
gnss.rs
