ados_positioning/
fixtures.rs

1//! Test fixtures: byte-exact receiver frames with correct checksums.
2
3use crate::stream::{crc24q, ubx_frame};
4use crate::ubx;
5
6/// `$body*hh\r\n`.
7pub fn nmea(body: &str) -> Vec<u8> {
8    let ck = body.bytes().fold(0u8, |a, c| a ^ c);
9    format!("${body}*{ck:02X}\r\n").into_bytes()
10}
11
12/// An RTCM3 frame of message `msg` with `extra` filler payload bytes.
13pub fn rtcm(msg: u16, extra: usize) -> Vec<u8> {
14    let mut payload = vec![(msg >> 4) as u8, ((msg & 0x0F) << 4) as u8];
15    payload.extend((0..extra).map(|i| i as u8));
16    let len = payload.len();
17    let mut f = vec![0xD3, (len >> 8) as u8 & 0x03, len as u8];
18    f.extend_from_slice(&payload);
19    let crc = crc24q(&f);
20    f.extend_from_slice(&[(crc >> 16) as u8, (crc >> 8) as u8, crc as u8]);
21    f
22}
23
24fn put_u16(p: &mut [u8], o: usize, v: u16) {
25    p[o..o + 2].copy_from_slice(&v.to_le_bytes());
26}
27fn put_u32(p: &mut [u8], o: usize, v: u32) {
28    p[o..o + 4].copy_from_slice(&v.to_le_bytes());
29}
30fn put_i32(p: &mut [u8], o: usize, v: i32) {
31    p[o..o + 4].copy_from_slice(&v.to_le_bytes());
32}
33
34/// NAV-PVT fields a test sets.
35#[derive(Debug, Clone, Copy)]
36pub struct Pvt {
37    /// `fixType`.
38    pub fix_type: u8,
39    /// `flags` byte (bit 0 `gnssFixOK`, bit 1 `diffSoln`, bits 6..7 `carrSoln`).
40    pub flags: u8,
41    /// Satellites used.
42    pub num_sv: u8,
43    /// Latitude, 1e-7 degrees.
44    pub lat_e7: i32,
45    /// Longitude, 1e-7 degrees.
46    pub lon_e7: i32,
47    /// Height above the ellipsoid, millimetres.
48    pub height_mm: i32,
49    /// Height above mean sea level, millimetres.
50    pub hmsl_mm: i32,
51    /// Position DOP, 0.01 units.
52    pub pdop_centi: u16,
53    /// `(year, month, day, hour, min, sec, nano)`; `None` leaves time invalid.
54    pub utc: Option<(u16, u8, u8, u8, u8, u8, i32)>,
55}
56
57impl Default for Pvt {
58    fn default() -> Self {
59        Self {
60            fix_type: 3,
61            flags: 0x01,
62            num_sv: 14,
63            lat_e7: 129_716_000,
64            lon_e7: 775_946_000,
65            height_mm: 920_400,
66            hmsl_mm: 1_006_100,
67            pdop_centi: 120,
68            utc: None,
69        }
70    }
71}
72
73/// A NAV-PVT frame carrying `v`.
74pub fn nav_pvt(v: Pvt) -> Vec<u8> {
75    let mut p = vec![0u8; 92];
76    if let Some((y, mo, d, h, mi, s, nano)) = v.utc {
77        put_u16(&mut p, 4, y);
78        p[6] = mo;
79        p[7] = d;
80        p[8] = h;
81        p[9] = mi;
82        p[10] = s;
83        p[11] = 0x07;
84        put_i32(&mut p, 16, nano);
85    }
86    p[20] = v.fix_type;
87    p[21] = v.flags;
88    p[23] = v.num_sv;
89    put_i32(&mut p, 24, v.lon_e7);
90    put_i32(&mut p, 28, v.lat_e7);
91    put_i32(&mut p, 32, v.height_mm);
92    put_i32(&mut p, 36, v.hmsl_mm);
93    put_u16(&mut p, 76, v.pdop_centi);
94    ubx_frame(ubx::CLS_NAV, ubx::NAV_PVT, &p)
95}
96
97/// A NAV-DOP frame with the horizontal DOP in 0.01 units.
98pub fn nav_dop(hdop_centi: u16) -> Vec<u8> {
99    let mut p = vec![0u8; 18];
100    put_u16(&mut p, 12, hdop_centi);
101    ubx_frame(ubx::CLS_NAV, ubx::NAV_DOP, &p)
102}
103
104/// NAV-SVIN with the mean in cm and accuracy in 0.1 mm.
105pub fn nav_svin(
106    dur_s: u32,
107    mean_cm: [i32; 3],
108    acc_01mm: u32,
109    valid: bool,
110    active: bool,
111) -> Vec<u8> {
112    let mut p = vec![0u8; 40];
113    put_u32(&mut p, 8, dur_s);
114    put_i32(&mut p, 12, mean_cm[0]);
115    put_i32(&mut p, 16, mean_cm[1]);
116    put_i32(&mut p, 20, mean_cm[2]);
117    put_u32(&mut p, 28, acc_01mm);
118    p[36] = valid as u8;
119    p[37] = active as u8;
120    ubx_frame(ubx::CLS_NAV, ubx::NAV_SVIN, &p)
121}
122
123/// NAV-RELPOSNED v1: heading and accuracy in 1e-5 deg, length in cm.
124pub fn nav_relposned(heading_e5: i32, acc_e5: u32, length_cm: i32, flags: u32) -> Vec<u8> {
125    let mut p = vec![0u8; 64];
126    p[0] = 0x01;
127    put_i32(&mut p, 20, length_cm);
128    put_i32(&mut p, 24, heading_e5);
129    put_u32(&mut p, 52, acc_e5);
130    put_u32(&mut p, 60, flags);
131    ubx_frame(ubx::CLS_NAV, ubx::NAV_RELPOSNED, &p)
132}
133
134/// A TIM-TP frame.
135pub fn tim_tp(tow_ms: u32, sub_ms: u32, week: u16, flags: u8) -> Vec<u8> {
136    let mut p = vec![0u8; 16];
137    put_u32(&mut p, 0, tow_ms);
138    put_u32(&mut p, 4, sub_ms);
139    put_u16(&mut p, 12, week);
140    p[14] = flags;
141    ubx_frame(ubx::CLS_TIM, ubx::TIM_TP, &p)
142}
143
144/// A MON-VER frame with hardware version `hw` and extension strings.
145pub fn mon_ver(hw: &str, extensions: &[&str]) -> Vec<u8> {
146    let mut p = vec![0u8; 40 + 30 * extensions.len()];
147    p[..9].copy_from_slice(b"EXT CORE ");
148    p[30..30 + hw.len()].copy_from_slice(hw.as_bytes());
149    for (i, e) in extensions.iter().enumerate() {
150        let o = 40 + 30 * i;
151        p[o..o + e.len()].copy_from_slice(e.as_bytes());
152    }
153    ubx_frame(ubx::CLS_MON, ubx::MON_VER, &p)
154}
155
156/// An ACK-ACK (`ok`) or ACK-NAK frame for `(class, id)`.
157pub fn ack(ok: bool, class: u8, id: u8) -> Vec<u8> {
158    let msg = if ok { ubx::ACK_ACK } else { ubx::ACK_NAK };
159    ubx_frame(ubx::CLS_ACK, msg, &[class, id])
160}
161
162/// Split one UBX frame back into (class, id, payload), checking the checksum.
163pub fn unframe(f: &[u8]) -> (u8, u8, Vec<u8>) {
164    let mut s = crate::stream::Splitter::new();
165    match s.push(f).pop() {
166        Some(crate::stream::Frame::Ubx { class, id, payload }) => (class, id, payload),
167        other => panic!("not one UBX frame: {other:?}"),
168    }
169}
ados_positioning/fixtures.rs

ados_positioning/
fixtures.rs
