ados_positioning/
base.rs

1//! RTK base state machine: `off → surveying → fixed | failed`.
2//!
3//! The machine is pure: [`Base::apply`] returns the bytes to write to the
4//! receiver, and receiver reports (NAV-SVIN, ACK, MON-VER) advance it. Nothing
5//! is reported as progress until the receiver itself reports it.
6
7use serde::Serialize;
8
9use crate::ubx::NavSvin;
10use crate::valset::{self, RxPort};
11
12/// How long the receiver has to confirm a base command (survey-in running,
13/// or the fixed-mode VALSET acknowledged).
14pub const CONFIRM_TIMEOUT_NS: u64 = 10_000_000_000;
15
16/// Failure reason recorded when the receiver cannot act as a base.
17pub const E_UNSUPPORTED: &str = "E_GNSS_UNSUPPORTED";
18
19/// A requested base mode.
20#[derive(Debug, Clone, Copy, PartialEq)]
21pub enum BaseMode {
22    /// Not a base.
23    Off,
24    /// Survey-in until both the minimum duration and the accuracy are met.
25    Survey {
26        /// Minimum survey duration, seconds.
27        min_dur_s: u32,
28        /// Target mean position accuracy, metres.
29        accuracy_m: f64,
30    },
31    /// A known base position.
32    Fixed(Llh),
33}
34
35impl BaseMode {
36    /// `off`, `survey` or `fixed`.
37    pub fn as_str(&self) -> &'static str {
38        match self {
39            BaseMode::Off => "off",
40            BaseMode::Survey { .. } => "survey",
41            BaseMode::Fixed(_) => "fixed",
42        }
43    }
44}
45
46/// Geodetic position, WGS84.
47#[derive(Debug, Clone, Copy, PartialEq, Serialize)]
48pub struct Llh {
49    /// Latitude, degrees.
50    pub lat_deg: f64,
51    /// Longitude, degrees.
52    pub lon_deg: f64,
53    /// Height above the ellipsoid, metres.
54    pub h_ellipsoid_m: f64,
55}
56
57/// Base machine state, serialized in snake case.
58#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize)]
59#[serde(rename_all = "snake_case")]
60pub enum BaseState {
61    /// Not a base.
62    Off,
63    /// Survey-in running.
64    Surveying,
65    /// Base position fixed (surveyed or configured).
66    Fixed,
67    /// The receiver rejected or did not confirm the command.
68    Failed,
69}
70
71/// What the base receiver can do.
72#[derive(Debug, Clone, Default)]
73pub struct Capability {
74    /// Configuration bytes can reach the receiver.
75    pub writable: bool,
76    /// Model string, when known (config or MON-VER).
77    pub model: Option<String>,
78    /// Receiver port the host is attached to.
79    pub port: RxPort,
80}
81
82/// Modules with the RTK base function (CFG-TMODE survey-in and fixed mode
83/// plus RTCM3 base output), as the MON-VER `MOD=` extension names them. The
84/// ZED-F9R (a sensor-fusion rover) and the ZED-F9T (a timing receiver whose
85/// time mode emits no RTCM3 base messages) are not bases.
86pub const BASE_MODULES: [&str; 3] = ["ZED-F9P", "NEO-F9P", "ZED-X20P"];
87
88/// A model string (`u-blox ZED-F9P`, or the bare `ZED-F9P`) names a module in
89/// [`BASE_MODULES`]: one whitespace-separated word must equal an entry,
90/// ignoring case.
91pub fn survey_capable(model: &str) -> bool {
92    model
93        .split_whitespace()
94        .any(|w| BASE_MODULES.iter().any(|m| m.eq_ignore_ascii_case(w)))
95}
96
97/// The base machine.
98#[derive(Debug, Clone)]
99pub struct Base {
100    mode: BaseMode,
101    state: BaseState,
102    reason: Option<String>,
103    commanded_ns: Option<u64>,
104    confirmed: bool,
105    elapsed_s: u32,
106    accuracy_m: Option<f64>,
107    fixed: Option<Llh>,
108    min_dur_s: u32,
109    target_accuracy_m: f64,
110}
111
112impl Base {
113    /// An idle machine with the configured survey defaults.
114    pub fn new(min_dur_s: u32, target_accuracy_m: f64) -> Self {
115        Self {
116            mode: BaseMode::Off,
117            state: BaseState::Off,
118            reason: None,
119            commanded_ns: None,
120            confirmed: false,
121            elapsed_s: 0,
122            accuracy_m: None,
123            fixed: None,
124            min_dur_s,
125            target_accuracy_m,
126        }
127    }
128
129    /// The requested mode.
130    pub fn mode(&self) -> BaseMode {
131        self.mode
132    }
133    /// The machine state.
134    pub fn state(&self) -> BaseState {
135        self.state
136    }
137    /// Why the machine is `failed`.
138    pub fn reason(&self) -> Option<&str> {
139        self.reason.as_deref()
140    }
141    /// The survey defaults `(min_dur_s, target_accuracy_m)`.
142    pub fn defaults(&self) -> (u32, f64) {
143        (self.min_dur_s, self.target_accuracy_m)
144    }
145    /// True while this receiver acts as a base (its RTCM3 output is forwarded).
146    pub fn active(&self) -> bool {
147        !matches!(self.mode, BaseMode::Off) && self.state != BaseState::Failed
148    }
149
150    /// The receiver confirmed the current command (a running survey-in
151    /// report, or the fixed-mode acknowledgement).
152    pub fn confirmed(&self) -> bool {
153        self.confirmed && self.state != BaseState::Failed
154    }
155
156    fn fail(&mut self, reason: String) {
157        tracing::warn!(reason = %reason, mode = self.mode.as_str(), "gnss_base_failed");
158        self.state = BaseState::Failed;
159        self.reason = Some(reason);
160        self.commanded_ns = None;
161    }
162
163    /// Record the configured mode while the receiver port is not open yet.
164    /// Nothing is claimed: the state stays `off` until [`Base::apply`]
165    /// commands the receiver.
166    pub fn request(&mut self, mode: BaseMode) {
167        self.reset(mode);
168    }
169
170    fn reset(&mut self, mode: BaseMode) {
171        self.mode = mode;
172        self.reason = None;
173        self.confirmed = false;
174        self.elapsed_s = 0;
175        self.accuracy_m = None;
176        self.fixed = None;
177        self.commanded_ns = None;
178        self.state = BaseState::Off;
179        if let BaseMode::Survey {
180            min_dur_s,
181            accuracy_m,
182        } = mode
183        {
184            self.min_dur_s = min_dur_s;
185            self.target_accuracy_m = accuracy_m;
186        }
187    }
188
189    /// Apply a requested mode. `Ok(bytes)` are to be written to the receiver
190    /// (none when it cannot be written and none are needed); `Err` is the
191    /// error code and detail of a refused request, which also leaves the
192    /// machine `failed`.
193    pub fn apply(
194        &mut self,
195        mode: BaseMode,
196        cap: &Capability,
197        now: u64,
198    ) -> Result<Option<Vec<u8>>, (&'static str, String)> {
199        self.reset(mode);
200        if mode == BaseMode::Off {
201            return Ok(cap.writable.then(|| valset::off(cap.port)));
202        }
203        if !cap.writable {
204            let detail = "the receiver is attached through the sensor hub and cannot be configured";
205            self.fail(detail.to_string());
206            return Err((E_UNSUPPORTED, detail.to_string()));
207        }
208        if let Some(model) = cap.model.as_deref().filter(|m| !survey_capable(m)) {
209            let detail = format!("receiver model {model} has no base mode");
210            self.fail(detail.clone());
211            return Err((E_UNSUPPORTED, detail));
212        }
213        self.commanded_ns = Some(now);
214        let bytes = match mode {
215            BaseMode::Survey {
216                min_dur_s,
217                accuracy_m,
218            } => {
219                self.state = BaseState::Surveying;
220                valset::survey_in(cap.port, min_dur_s, accuracy_m)
221            }
222            BaseMode::Fixed(p) => valset::fixed(cap.port, p.lat_deg, p.lon_deg, p.h_ellipsoid_m),
223            BaseMode::Off => unreachable!("handled above"),
224        };
225        Ok(Some(bytes))
226    }
227
228    /// The receiver reported its model (MON-VER).
229    pub fn on_model(&mut self, model: &str) {
230        if self.mode != BaseMode::Off && self.state != BaseState::Failed && !survey_capable(model) {
231            self.fail(format!("receiver model {model} has no base mode"));
232        }
233    }
234
235    /// NAV-SVIN from the base receiver.
236    pub fn on_svin(&mut self, s: &NavSvin) {
237        if !matches!(self.mode, BaseMode::Survey { .. }) || self.state != BaseState::Surveying {
238            return;
239        }
240        if !(s.active || s.valid) {
241            return;
242        }
243        self.confirmed = true;
244        self.elapsed_s = s.dur_s;
245        self.accuracy_m = Some(s.mean_acc_m);
246        if s.valid {
247            self.state = BaseState::Fixed;
248            self.fixed = Some(ecef_to_llh(s.mean_ecef_m));
249        }
250    }
251
252    /// ACK-ACK (`ok`) or ACK-NAK for a CFG-VALSET.
253    pub fn on_valset_ack(&mut self, ok: bool) {
254        if self.commanded_ns.is_none() || self.state == BaseState::Failed {
255            return;
256        }
257        if !ok {
258            self.fail("the receiver rejected the base configuration".to_string());
259            return;
260        }
261        if let BaseMode::Fixed(p) = self.mode {
262            self.confirmed = true;
263            self.state = BaseState::Fixed;
264            self.fixed = Some(p);
265        }
266    }
267
268    /// Fail a command the receiver never confirmed.
269    pub fn tick(&mut self, now: u64) {
270        let Some(t) = self.commanded_ns else {
271            return;
272        };
273        if self.confirmed || self.state == BaseState::Failed {
274            return;
275        }
276        if now.saturating_sub(t) > CONFIRM_TIMEOUT_NS {
277            let reason = match self.mode {
278                BaseMode::Survey { .. } => "no survey-in report (NAV-SVIN) from the receiver",
279                _ => "the receiver did not acknowledge the base configuration",
280            };
281            self.fail(reason.to_string());
282        }
283    }
284
285    /// The `base` object minus `rtcm_out`.
286    pub fn view(&self) -> BaseView {
287        BaseView {
288            mode: self.mode.as_str(),
289            state: self.state,
290            elapsed_s: self.elapsed_s,
291            min_dur_s: self.min_dur_s,
292            accuracy_m: self.accuracy_m,
293            target_accuracy_m: self.target_accuracy_m,
294            fixed: self.fixed.filter(|_| self.state == BaseState::Fixed),
295        }
296    }
297}
298
299/// A serializable snapshot of the base machine.
300#[derive(Debug, Clone, PartialEq, Serialize)]
301pub struct BaseView {
302    /// Mode name, as [`BaseMode::as_str`].
303    pub mode: &'static str,
304    /// Machine state.
305    pub state: BaseState,
306    /// Survey-in elapsed time, seconds.
307    pub elapsed_s: u32,
308    /// Survey-in minimum duration, seconds.
309    pub min_dur_s: u32,
310    /// Current survey-in accuracy, metres.
311    pub accuracy_m: Option<f64>,
312    /// Target survey-in accuracy, metres.
313    pub target_accuracy_m: f64,
314    /// The base position, once fixed.
315    pub fixed: Option<Llh>,
316}
317
318/// WGS84 ECEF (m) to geodetic (Bowring, iterated).
319pub fn ecef_to_llh([x, y, z]: [f64; 3]) -> Llh {
320    const A: f64 = 6_378_137.0;
321    const F: f64 = 1.0 / 298.257_223_563;
322    let e2 = F * (2.0 - F);
323    let p = (x * x + y * y).sqrt();
324    let lon = y.atan2(x);
325    let mut lat = z.atan2(p * (1.0 - e2));
326    let mut h = 0.0;
327    for _ in 0..8 {
328        let s = lat.sin();
329        let n = A / (1.0 - e2 * s * s).sqrt();
330        h = if lat.cos().abs() > 1e-9 {
331            p / lat.cos() - n
332        } else {
333            z.abs() - n * (1.0 - e2)
334        };
335        lat = z.atan2(p * (1.0 - e2 * n / (n + h)));
336    }
337    Llh {
338        lat_deg: lat.to_degrees(),
339        lon_deg: lon.to_degrees(),
340        h_ellipsoid_m: h,
341    }
342}
343
344#[cfg(test)]
345mod tests;
ados_positioning/base.rs

ados_positioning/
base.rs
