ados_positioning/
receiver.rs

1//! Per-receiver navigation state, fed by decoded frames.
2
3use serde::Serialize;
4
5use crate::nmea::{Gga, Rmc, Sentence};
6use crate::ubx::NavPvt;
7
8/// A solution older than this reads as no fix.
9pub const STALE_NS: u64 = 3_000_000_000;
10
11/// Reported fix quality, serialized as `none`, `2d`, `3d`, `dgps`,
12/// `rtk_float` or `rtk_fixed`.
13#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Default)]
14pub enum Fix {
15    /// No usable fix.
16    #[default]
17    #[serde(rename = "none")]
18    None,
19    /// Two-dimensional fix.
20    #[serde(rename = "2d")]
21    TwoD,
22    /// Three-dimensional fix.
23    #[serde(rename = "3d")]
24    ThreeD,
25    /// Differential (SBAS or DGNSS) fix.
26    #[serde(rename = "dgps")]
27    Dgps,
28    /// RTK with float ambiguities.
29    #[serde(rename = "rtk_float")]
30    RtkFloat,
31    /// RTK with fixed ambiguities.
32    #[serde(rename = "rtk_fixed")]
33    RtkFixed,
34}
35
36/// RTK state alone, serialized as `none`, `float` or `fixed`.
37#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Default)]
38#[serde(rename_all = "snake_case")]
39pub enum Rtk {
40    /// No RTK solution.
41    #[default]
42    None,
43    /// Float ambiguities.
44    Float,
45    /// Fixed ambiguities.
46    Fixed,
47}
48
49impl Fix {
50    /// The RTK part of this fix.
51    pub fn rtk(self) -> Rtk {
52        match self {
53            Fix::RtkFloat => Rtk::Float,
54            Fix::RtkFixed => Rtk::Fixed,
55            _ => Rtk::None,
56        }
57    }
58}
59
60/// Map NAV-PVT to the reported fix: carrier solution first, then the
61/// differential flag, then the fix type. Dead-reckoning-only and time-only
62/// solutions, or `gnssFixOK` clear, are no fix.
63pub fn fix_from_pvt(p: &NavPvt) -> Fix {
64    if !p.gnss_fix_ok {
65        return Fix::None;
66    }
67    let base = match p.fix_type {
68        2 => Fix::TwoD,
69        3 | 4 => Fix::ThreeD,
70        _ => return Fix::None,
71    };
72    match p.carr_soln {
73        2 => Fix::RtkFixed,
74        1 => Fix::RtkFloat,
75        _ if p.diff_soln => Fix::Dgps,
76        _ => base,
77    }
78}
79
80/// The solution reported for one receiver.
81#[derive(Debug, Clone, Copy, PartialEq, Default)]
82pub struct Solution {
83    /// Fix quality.
84    pub fix: Fix,
85    /// Satellites used.
86    pub sats: u8,
87    /// Horizontal dilution of precision, when known.
88    pub hdop: Option<f64>,
89    /// Latitude, degrees.
90    pub lat_deg: Option<f64>,
91    /// Longitude, degrees.
92    pub lon_deg: Option<f64>,
93    /// Height above the ellipsoid, metres.
94    pub h_ellipsoid_m: Option<f64>,
95    /// Height above mean sea level, metres.
96    pub h_msl_m: Option<f64>,
97}
98
99/// One receiver's decoded state.
100#[derive(Debug, Clone, Default)]
101pub struct ReceiverState {
102    /// Receiver model, when known.
103    pub model: Option<String>,
104    pvt: Option<(u64, NavPvt)>,
105    dop: Option<(u64, f64)>,
106    gga: Option<(u64, Gga)>,
107    rmc: Option<(u64, Rmc)>,
108}
109
110fn fresh<T>(v: &Option<(u64, T)>, now: u64) -> Option<&T> {
111    v.as_ref()
112        .filter(|(t, _)| now.saturating_sub(*t) <= STALE_NS)
113        .map(|(_, x)| x)
114}
115
116impl ReceiverState {
117    /// An empty state for a receiver of `model`.
118    pub fn new(model: Option<String>) -> Self {
119        Self {
120            model,
121            ..Self::default()
122        }
123    }
124
125    /// Record a NAV-PVT received at `now` (monotonic ns).
126    pub fn on_pvt(&mut self, now: u64, pvt: NavPvt) {
127        self.pvt = Some((now, pvt));
128    }
129
130    /// Record a NAV-DOP horizontal DOP received at `now` (monotonic ns).
131    pub fn on_dop(&mut self, now: u64, hdop: f64) {
132        self.dop = Some((now, hdop));
133    }
134
135    /// Record an NMEA sentence received at `now` (monotonic ns).
136    pub fn on_nmea(&mut self, now: u64, s: Sentence) {
137        match s {
138            Sentence::Gga(g) => self.gga = Some((now, g)),
139            Sentence::Rmc(r) => self.rmc = Some((now, r)),
140        }
141    }
142
143    /// The current solution: UBX when fresh, else NMEA GGA, else RMC.
144    pub fn solution(&self, now: u64) -> Solution {
145        if let Some(p) = fresh(&self.pvt, now) {
146            let fix = fix_from_pvt(p);
147            let has = fix != Fix::None;
148            // NAV-PVT carries only the position DOP; hdop comes from NAV-DOP.
149            let hdop = fresh(&self.dop, now).copied();
150            return Solution {
151                fix,
152                sats: p.num_sv,
153                hdop,
154                lat_deg: has.then_some(p.lat_deg),
155                lon_deg: has.then_some(p.lon_deg),
156                h_ellipsoid_m: has.then_some(p.h_ellipsoid_m),
157                h_msl_m: has.then_some(p.h_msl_m),
158            };
159        }
160        if let Some(g) = fresh(&self.gga, now) {
161            return Solution {
162                fix: g.fix,
163                sats: g.sats,
164                hdop: g.hdop,
165                lat_deg: g.lat_deg,
166                lon_deg: g.lon_deg,
167                h_ellipsoid_m: g.h_ellipsoid_m,
168                h_msl_m: g.h_msl_m,
169            };
170        }
171        if let Some(r) = fresh(&self.rmc, now) {
172            return Solution {
173                fix: r.fix,
174                lat_deg: r.lat_deg,
175                lon_deg: r.lon_deg,
176                ..Solution::default()
177            };
178        }
179        Solution::default()
180    }
181}
ados_positioning/receiver.rs

ados_positioning/
receiver.rs
