ados_positioning/
ubx.rs

1//! UBX message decode for the navigation, timing, acknowledgement and version
2//! messages a host typically consumes. Offsets follow the u-blox F9 interface
3//! description; every decoder checks the payload length first and returns
4//! `None` for a short or foreign payload.
5
6/// UBX class NAV (navigation results).
7pub const CLS_NAV: u8 = 0x01;
8/// UBX class ACK (acknowledgements).
9pub const CLS_ACK: u8 = 0x05;
10/// UBX class CFG (configuration).
11pub const CLS_CFG: u8 = 0x06;
12/// UBX class MON (monitoring).
13pub const CLS_MON: u8 = 0x0A;
14/// UBX class TIM (timing).
15pub const CLS_TIM: u8 = 0x0D;
16
17/// Message id of NAV-DOP.
18pub const NAV_DOP: u8 = 0x04;
19/// Message id of NAV-PVT.
20pub const NAV_PVT: u8 = 0x07;
21/// Message id of NAV-SVIN.
22pub const NAV_SVIN: u8 = 0x3B;
23/// Message id of NAV-RELPOSNED.
24pub const NAV_RELPOSNED: u8 = 0x3C;
25/// Message id of ACK-NAK.
26pub const ACK_NAK: u8 = 0x00;
27/// Message id of ACK-ACK.
28pub const ACK_ACK: u8 = 0x01;
29/// Message id of CFG-VALSET.
30pub const CFG_VALSET: u8 = 0x8A;
31/// Message id of MON-VER.
32pub const MON_VER: u8 = 0x04;
33/// Message id of TIM-TP.
34pub const TIM_TP: u8 = 0x01;
35
36/// Seconds between the Unix epoch and the GPS epoch (1980-01-06).
37const GPS_EPOCH_UNIX_S: i64 = 315_964_800;
38/// GPS − UTC leap seconds, constant since 2017-01-01.
39pub const GPS_UTC_LEAP_S: i64 = 18;
40
41fn u16_at(p: &[u8], o: usize) -> u16 {
42    u16::from_le_bytes([p[o], p[o + 1]])
43}
44fn u32_at(p: &[u8], o: usize) -> u32 {
45    u32::from_le_bytes([p[o], p[o + 1], p[o + 2], p[o + 3]])
46}
47fn i32_at(p: &[u8], o: usize) -> i32 {
48    u32_at(p, o) as i32
49}
50
51/// UBX-NAV-PVT.
52#[derive(Debug, Clone, Copy, PartialEq)]
53pub struct NavPvt {
54    /// GNSS fix type (0 none, 1 dead reckoning, 2 2D, 3 3D, 4 combined, 5 time only).
55    pub fix_type: u8,
56    /// The fix is within the receiver's accuracy masks.
57    pub gnss_fix_ok: bool,
58    /// Differential corrections were applied.
59    pub diff_soln: bool,
60    /// 0 none, 1 float, 2 fixed.
61    pub carr_soln: u8,
62    /// Satellites used in the solution.
63    pub num_sv: u8,
64    /// Latitude, degrees.
65    pub lat_deg: f64,
66    /// Longitude, degrees.
67    pub lon_deg: f64,
68    /// Height above the ellipsoid, metres.
69    pub h_ellipsoid_m: f64,
70    /// Height above mean sea level, metres.
71    pub h_msl_m: f64,
72    /// UTC of the solution epoch, when date and time are valid and resolved.
73    pub utc_ns: Option<i64>,
74}
75
76/// Decode a NAV-PVT payload.
77pub fn nav_pvt(p: &[u8]) -> Option<NavPvt> {
78    if p.len() < 92 {
79        return None;
80    }
81    let flags = p[21];
82    let valid = p[11];
83    let utc_ns = (valid & 0x07 == 0x07)
84        .then(|| {
85            let days = days_from_civil(u16_at(p, 4) as i64, p[6] as i64, p[7] as i64);
86            let secs = days * 86_400 + p[8] as i64 * 3600 + p[9] as i64 * 60 + p[10] as i64;
87            secs * 1_000_000_000 + i32_at(p, 16) as i64
88        })
89        .filter(|_| (1..=12).contains(&p[6]) && (1..=31).contains(&p[7]));
90    Some(NavPvt {
91        fix_type: p[20],
92        gnss_fix_ok: flags & 0x01 != 0,
93        diff_soln: flags & 0x02 != 0,
94        carr_soln: (flags >> 6) & 0x03,
95        num_sv: p[23],
96        lon_deg: i32_at(p, 24) as f64 * 1e-7,
97        lat_deg: i32_at(p, 28) as f64 * 1e-7,
98        h_ellipsoid_m: i32_at(p, 32) as f64 / 1000.0,
99        h_msl_m: i32_at(p, 36) as f64 / 1000.0,
100        utc_ns,
101    })
102}
103
104/// Days since 1970-01-01 of a proleptic Gregorian date.
105pub fn days_from_civil(y: i64, m: i64, d: i64) -> i64 {
106    let y = if m <= 2 { y - 1 } else { y };
107    let era = y.div_euclid(400);
108    let yoe = y - era * 400;
109    let mp = (m + 9) % 12;
110    let doy = (153 * mp + 2) / 5 + d - 1;
111    let doe = yoe * 365 + yoe / 4 - yoe / 100 + doy;
112    era * 146_097 + doe - 719_468
113}
114
115/// UBX-NAV-DOP: the horizontal DOP.
116pub fn nav_dop_hdop(p: &[u8]) -> Option<f64> {
117    (p.len() >= 18).then(|| u16_at(p, 12) as f64 / 100.0)
118}
119
120/// UBX-NAV-SVIN.
121#[derive(Debug, Clone, Copy, PartialEq)]
122pub struct NavSvin {
123    /// Survey-in duration so far, seconds.
124    pub dur_s: u32,
125    /// Mean position, ECEF metres.
126    pub mean_ecef_m: [f64; 3],
127    /// Mean position accuracy, metres.
128    pub mean_acc_m: f64,
129    /// The survey-in met its limits and the position is valid.
130    pub valid: bool,
131    /// A survey-in is in progress.
132    pub active: bool,
133}
134
135/// Decode a NAV-SVIN payload.
136pub fn nav_svin(p: &[u8]) -> Option<NavSvin> {
137    if p.len() < 40 {
138        return None;
139    }
140    let axis = |o: usize, hp: usize| i32_at(p, o) as f64 * 0.01 + (p[hp] as i8) as f64 * 1e-4;
141    Some(NavSvin {
142        dur_s: u32_at(p, 8),
143        mean_ecef_m: [axis(12, 24), axis(16, 25), axis(20, 26)],
144        mean_acc_m: u32_at(p, 28) as f64 * 1e-4,
145        valid: p[36] != 0,
146        active: p[37] != 0,
147    })
148}
149
150/// UBX-NAV-RELPOSNED (version 1, F9).
151#[derive(Debug, Clone, Copy, PartialEq)]
152pub struct RelPosNed {
153    /// Heading of the relative position vector, degrees.
154    pub heading_deg: f64,
155    /// Heading accuracy, degrees.
156    pub acc_heading_deg: f64,
157    /// Length of the relative position vector, metres.
158    pub length_m: f64,
159    /// The fix is within the receiver's accuracy masks.
160    pub gnss_fix_ok: bool,
161    /// The relative position components are valid.
162    pub rel_pos_valid: bool,
163    /// The receiver runs in moving-base mode.
164    pub is_moving: bool,
165    /// The heading is valid.
166    pub heading_valid: bool,
167}
168
169/// Decode a NAV-RELPOSNED (version 1) payload.
170pub fn nav_relposned(p: &[u8]) -> Option<RelPosNed> {
171    if p.len() < 64 || p[0] != 0x01 {
172        return None;
173    }
174    let flags = u32_at(p, 60);
175    Some(RelPosNed {
176        heading_deg: i32_at(p, 24) as f64 * 1e-5,
177        acc_heading_deg: u32_at(p, 52) as f64 * 1e-5,
178        length_m: i32_at(p, 20) as f64 * 0.01 + (p[35] as i8) as f64 * 1e-4,
179        gnss_fix_ok: flags & (1 << 0) != 0,
180        rel_pos_valid: flags & (1 << 2) != 0,
181        is_moving: flags & (1 << 5) != 0,
182        heading_valid: flags & (1 << 8) != 0,
183    })
184}
185
186/// UBX-TIM-TP: the UTC (Unix ns) of the next time pulse.
187pub fn tim_tp_utc_ns(p: &[u8]) -> Option<i64> {
188    if p.len() < 16 {
189        return None;
190    }
191    let tow_ms = u32_at(p, 0) as i64;
192    let sub_ms = u32_at(p, 4) as i64; // 2^-32 ms
193    let week = u16_at(p, 12) as i64;
194    let utc_base = p[14] & 0x01 != 0;
195    let mut ns = (GPS_EPOCH_UNIX_S + week * 604_800) * 1_000_000_000
196        + tow_ms * 1_000_000
197        + ((sub_ms * 1_000_000) >> 32);
198    if !utc_base {
199        ns -= GPS_UTC_LEAP_S * 1_000_000_000;
200    }
201    Some(ns)
202}
203
204/// UBX-MON-VER: the module name (`MOD=` extension), else the hardware
205/// version string.
206pub fn mon_ver_model(p: &[u8]) -> Option<String> {
207    if p.len() < 40 {
208        return None;
209    }
210    let text = |b: &[u8]| {
211        let end = b.iter().position(|&c| c == 0).unwrap_or(b.len());
212        String::from_utf8_lossy(&b[..end]).trim().to_string()
213    };
214    let module = p[40..]
215        .as_chunks::<30>()
216        .0
217        .iter()
218        .map(|c| text(c.as_slice()))
219        .find_map(|e| e.strip_prefix("MOD=").map(str::to_string));
220    Some(match module {
221        Some(m) => format!("u-blox {m}"),
222        None => format!("u-blox hw {}", text(&p[30..40])),
223    })
224}
225
226/// UBX-ACK-ACK / ACK-NAK: the acknowledged (class, id).
227pub fn ack_target(p: &[u8]) -> Option<(u8, u8)> {
228    (p.len() >= 2).then(|| (p[0], p[1]))
229}
ados_positioning/ubx.rs

ados_positioning/
ubx.rs
