ados_positioning/
valset.rs

1//! UBX-CFG-VALSET builders for the RTK base (u-blox F9 configuration keys).
2//!
3//! A key id carries its value size in bits 28..30 (1 = one bit stored in a
4//! byte, 2 = one byte, 3 = two, 4 = four, 5 = eight); [`Item`] holds the value
5//! and writes exactly that many little-endian bytes.
6
7use crate::stream::ubx_frame;
8use crate::ubx::{CFG_VALSET, CLS_CFG};
9
10/// CFG-TMODE-MODE: 0 disabled, 1 survey-in, 2 fixed.
11pub const TMODE_MODE: u32 = 0x2003_0001;
12/// CFG-TMODE-POS_TYPE: 0 ECEF, 1 LLH.
13pub const TMODE_POS_TYPE: u32 = 0x2003_0002;
14/// CFG-TMODE-LAT: fixed latitude, 1e-7 degrees.
15pub const TMODE_LAT: u32 = 0x4003_0009;
16/// CFG-TMODE-LON: fixed longitude, 1e-7 degrees.
17pub const TMODE_LON: u32 = 0x4003_000A;
18/// CFG-TMODE-HEIGHT: fixed ellipsoid height, centimetres.
19pub const TMODE_HEIGHT: u32 = 0x4003_000B;
20/// CFG-TMODE-LAT_HP: high-precision latitude, 1e-9 degrees.
21pub const TMODE_LAT_HP: u32 = 0x2003_000C;
22/// CFG-TMODE-LON_HP: high-precision longitude, 1e-9 degrees.
23pub const TMODE_LON_HP: u32 = 0x2003_000D;
24/// CFG-TMODE-HEIGHT_HP: high-precision height, 0.1 mm.
25pub const TMODE_HEIGHT_HP: u32 = 0x2003_000E;
26/// CFG-TMODE-SVIN_MIN_DUR: survey-in minimum duration, seconds.
27pub const TMODE_SVIN_MIN_DUR: u32 = 0x4003_0010;
28/// CFG-TMODE-SVIN_ACC_LIMIT: survey-in accuracy limit, 0.1 mm.
29pub const TMODE_SVIN_ACC_LIMIT: u32 = 0x4003_0011;
30
31/// `CFG-VALSET` layer bit for RAM (applied now, lost on power cycle).
32pub const LAYER_RAM: u8 = 0x01;
33
34/// The receiver port the host is attached to, which selects the per-port
35/// CFG-MSGOUT keys.
36#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
37pub enum RxPort {
38    /// UART1.
39    #[default]
40    Uart1,
41    /// USB.
42    Usb,
43}
44
45impl RxPort {
46    /// Parse `uart1` or `usb`.
47    pub fn parse(s: &str) -> Option<Self> {
48        match s {
49            "uart1" => Some(Self::Uart1),
50            "usb" => Some(Self::Usb),
51            _ => None,
52        }
53    }
54}
55
56/// CFG-MSGOUT key ids `(uart1, usb)` for the RTCM3 base messages
57/// 1005, 1074, 1084, 1094, 1124 and 1230.
58const RTCM_MSGOUT: [(u16, u32, u32); 6] = [
59    (1005, 0x2091_02BE, 0x2091_02C0),
60    (1074, 0x2091_035F, 0x2091_0361),
61    (1084, 0x2091_0364, 0x2091_0366),
62    (1094, 0x2091_0369, 0x2091_036B),
63    (1124, 0x2091_036E, 0x2091_0370),
64    (1230, 0x2091_0304, 0x2091_0306),
65];
66/// CFG-MSGOUT-UBX_NAV_PVT key ids `(uart1, usb)`.
67pub const NAV_PVT_MSGOUT: (u32, u32) = (0x2091_0007, 0x2091_0009);
68/// CFG-MSGOUT-UBX_NAV_DOP key ids `(uart1, usb)`.
69pub const NAV_DOP_MSGOUT: (u32, u32) = (0x2091_0039, 0x2091_003B);
70/// CFG-MSGOUT-UBX_NAV_SVIN key ids `(uart1, usb)`.
71pub const NAV_SVIN_MSGOUT: (u32, u32) = (0x2091_0089, 0x2091_008B);
72/// CFG-MSGOUT-UBX_NAV_RELPOSNED key ids `(uart1, usb)`.
73pub const NAV_RELPOSNED_MSGOUT: (u32, u32) = (0x2091_008E, 0x2091_0090);
74/// CFG-MSGOUT-UBX_TIM_TP key ids `(uart1, usb)`.
75pub const TIM_TP_MSGOUT: (u32, u32) = (0x2091_017E, 0x2091_0180);
76
77fn port_key(keys: (u32, u32), port: RxPort) -> u32 {
78    match port {
79        RxPort::Uart1 => keys.0,
80        RxPort::Usb => keys.1,
81    }
82}
83
84/// One key/value pair; the value is truncated to the key's size.
85#[derive(Debug, Clone, Copy, PartialEq, Eq)]
86pub struct Item(pub u32, pub i64);
87
88fn value_len(key: u32) -> usize {
89    match (key >> 28) & 0x07 {
90        1 | 2 => 1,
91        3 => 2,
92        4 => 4,
93        _ => 8,
94    }
95}
96
97/// A CFG-VALSET frame applying `items` to `layers`.
98pub fn valset(layers: u8, items: &[Item]) -> Vec<u8> {
99    let mut p = vec![0x00, layers, 0x00, 0x00];
100    for &Item(key, value) in items {
101        p.extend_from_slice(&key.to_le_bytes());
102        p.extend_from_slice(&value.to_le_bytes()[..value_len(key)]);
103    }
104    ubx_frame(CLS_CFG, CFG_VALSET, &p)
105}
106
107/// RTCM3 base message output on `port` at `rate` (0 disables).
108fn rtcm_items(port: RxPort, rate: i64) -> impl Iterator<Item = Item> {
109    RTCM_MSGOUT
110        .into_iter()
111        .map(move |(_, uart, usb)| Item(port_key((uart, usb), port), rate))
112}
113
114/// Output rate 1 for the RTCM3 base messages and NAV-SVIN on `port`.
115fn msgout_items(port: RxPort) -> Vec<Item> {
116    let mut v: Vec<Item> = rtcm_items(port, 1).collect();
117    v.push(Item(port_key(NAV_SVIN_MSGOUT, port), 1));
118    v
119}
120
121/// Enable, once per solution, the UBX messages this crate decodes on
122/// `port`: NAV-PVT and NAV-DOP always; TIM-TP and NAV-SVIN on the base
123/// receiver; NAV-RELPOSNED on a heading rover. A factory-default receiver
124/// emits only NMEA, so this runs every time the port opens.
125pub fn nav_outputs(port: RxPort, base: bool, heading: bool) -> Vec<u8> {
126    let mut keys = vec![NAV_PVT_MSGOUT, NAV_DOP_MSGOUT];
127    if base {
128        keys.extend([TIM_TP_MSGOUT, NAV_SVIN_MSGOUT]);
129    }
130    if heading {
131        keys.push(NAV_RELPOSNED_MSGOUT);
132    }
133    let items: Vec<Item> = keys
134        .into_iter()
135        .map(|k| Item(port_key(k, port), 1))
136        .collect();
137    valset(LAYER_RAM, &items)
138}
139
140/// Start a survey-in (TMODE 1) and enable the base output.
141pub fn survey_in(port: RxPort, min_dur_s: u32, accuracy_m: f64) -> Vec<u8> {
142    let acc = (accuracy_m * 10_000.0).round().clamp(1.0, u32::MAX as f64) as i64;
143    let mut items = vec![
144        Item(TMODE_MODE, 1),
145        Item(TMODE_SVIN_MIN_DUR, min_dur_s as i64),
146        Item(TMODE_SVIN_ACC_LIMIT, acc),
147    ];
148    items.extend(msgout_items(port));
149    valset(LAYER_RAM, &items)
150}
151
152/// Split a value into the standard part (`unit`) and a high-precision rest
153/// (`unit / 100`), the TMODE LLH encoding.
154fn split_hp(value: f64, unit_per: f64) -> (i64, i64) {
155    let scaled = value * unit_per;
156    let main = scaled.trunc();
157    let hp = ((scaled - main) * 100.0).round().clamp(-99.0, 99.0);
158    (main as i64, hp as i64)
159}
160
161/// Fixed base position (TMODE 2, LLH) and enable the base output.
162pub fn fixed(port: RxPort, lat_deg: f64, lon_deg: f64, h_ellipsoid_m: f64) -> Vec<u8> {
163    let (lat, lat_hp) = split_hp(lat_deg, 1e7);
164    let (lon, lon_hp) = split_hp(lon_deg, 1e7);
165    let (h, h_hp) = split_hp(h_ellipsoid_m, 100.0);
166    let mut items = vec![
167        Item(TMODE_MODE, 2),
168        Item(TMODE_POS_TYPE, 1),
169        Item(TMODE_LAT, lat),
170        Item(TMODE_LON, lon),
171        Item(TMODE_HEIGHT, h),
172        Item(TMODE_LAT_HP, lat_hp),
173        Item(TMODE_LON_HP, lon_hp),
174        Item(TMODE_HEIGHT_HP, h_hp),
175    ];
176    items.extend(msgout_items(port));
177    valset(LAYER_RAM, &items)
178}
179
180/// Leave base mode (TMODE 0) and stop the RTCM3 base output on `port`.
181pub fn off(port: RxPort) -> Vec<u8> {
182    let mut items = vec![Item(TMODE_MODE, 0)];
183    items.extend(rtcm_items(port, 0));
184    valset(LAYER_RAM, &items)
185}
186
187/// Poll UBX-MON-VER (empty payload).
188pub fn poll_mon_ver() -> Vec<u8> {
189    ubx_frame(crate::ubx::CLS_MON, crate::ubx::MON_VER, &[])
190}
191
192#[cfg(test)]
193mod tests;
ados_positioning/valset.rs

ados_positioning/
valset.rs
