ados_positioning/
stream.rs

1//! Splits one receiver byte stream into UBX, NMEA and RTCM3 frames.
2//!
3//! Receivers interleave protocols on one port, and a read can end anywhere,
4//! so the splitter buffers across pushes. Each candidate frame is accepted
5//! only when its checksum holds; on a mismatch, or on bytes that start no
6//! known frame, one byte is dropped and the scan resumes (resync).
7
8/// Largest UBX payload accepted; anything longer is treated as garbage.
9const UBX_MAX_PAYLOAD: usize = 8192;
10/// NMEA 0183 caps a sentence at 82 characters; allow some slack.
11const NMEA_MAX: usize = 120;
12/// Buffer cap: a stream that never forms a frame cannot grow memory.
13const BUF_MAX: usize = 64 * 1024;
14
15/// One whole protocol frame.
16#[derive(Debug, Clone, PartialEq, Eq)]
17pub enum Frame {
18    /// UBX message: class, id, payload.
19    Ubx {
20        /// Message class.
21        class: u8,
22        /// Message id.
23        id: u8,
24        /// Payload without header or checksum.
25        payload: Vec<u8>,
26    },
27    /// NMEA sentence without the trailing CR LF (`$GNGGA,...*hh`).
28    Nmea(String),
29    /// RTCM3 frame, header to CRC inclusive.
30    Rtcm(Vec<u8>),
31}
32
33/// The UBX 8-bit Fletcher checksum over class, id, length and payload.
34pub fn ubx_checksum(bytes: &[u8]) -> [u8; 2] {
35    let (mut a, mut b) = (0u8, 0u8);
36    for &x in bytes {
37        a = a.wrapping_add(x);
38        b = b.wrapping_add(a);
39    }
40    [a, b]
41}
42
43/// CRC-24Q (the RTCM3 / Qualcomm polynomial 0x1864CFB).
44pub fn crc24q(bytes: &[u8]) -> u32 {
45    let mut crc: u32 = 0;
46    for &b in bytes {
47        crc ^= (b as u32) << 16;
48        for _ in 0..8 {
49            crc <<= 1;
50            if crc & 0x0100_0000 != 0 {
51                crc ^= 0x0186_4CFB;
52            }
53        }
54    }
55    crc & 0x00FF_FFFF
56}
57
58enum Step {
59    Emit(Frame, usize),
60    Skip,
61    Wait,
62}
63
64/// The stream splitter.
65#[derive(Debug, Default)]
66pub struct Splitter {
67    buf: Vec<u8>,
68    garbage: u64,
69}
70
71impl Splitter {
72    /// An empty splitter.
73    pub fn new() -> Self {
74        Self::default()
75    }
76
77    /// Bytes discarded while resynchronising.
78    pub fn garbage(&self) -> u64 {
79        self.garbage
80    }
81
82    /// Append `bytes` and return every frame completed so far.
83    pub fn push(&mut self, bytes: &[u8]) -> Vec<Frame> {
84        self.buf.extend_from_slice(bytes);
85        let mut out = Vec::new();
86        let mut pos = 0;
87        while pos < self.buf.len() {
88            match scan(&self.buf[pos..]) {
89                Step::Emit(frame, used) => {
90                    out.push(frame);
91                    pos += used;
92                }
93                Step::Skip => {
94                    // Jump to the next byte that can start a frame.
95                    let next = self.buf[pos + 1..]
96                        .iter()
97                        .position(|&b| matches!(b, 0xB5 | b'$' | 0xD3))
98                        .map_or(self.buf.len(), |i| pos + 1 + i);
99                    self.garbage += (next - pos) as u64;
100                    pos = next;
101                }
102                Step::Wait => break,
103            }
104        }
105        self.buf.drain(..pos);
106        if self.buf.len() > BUF_MAX {
107            self.garbage += self.buf.len() as u64;
108            self.buf.clear();
109        }
110        out
111    }
112}
113
114fn scan(b: &[u8]) -> Step {
115    match b[0] {
116        0xB5 => scan_ubx(b),
117        b'$' => scan_nmea(b),
118        0xD3 => scan_rtcm(b),
119        _ => Step::Skip,
120    }
121}
122
123fn scan_ubx(b: &[u8]) -> Step {
124    if b.len() < 2 {
125        return Step::Wait;
126    }
127    if b[1] != 0x62 {
128        return Step::Skip;
129    }
130    if b.len() < 6 {
131        return Step::Wait;
132    }
133    let len = u16::from_le_bytes([b[4], b[5]]) as usize;
134    if len > UBX_MAX_PAYLOAD {
135        return Step::Skip;
136    }
137    let total = 8 + len;
138    if b.len() < total {
139        return Step::Wait;
140    }
141    if ubx_checksum(&b[2..6 + len]) != [b[6 + len], b[7 + len]] {
142        return Step::Skip;
143    }
144    Step::Emit(
145        Frame::Ubx {
146            class: b[2],
147            id: b[3],
148            payload: b[6..6 + len].to_vec(),
149        },
150        total,
151    )
152}
153
154fn scan_nmea(b: &[u8]) -> Step {
155    let window = &b[..b.len().min(NMEA_MAX)];
156    let Some(nl) = window.iter().position(|&c| c == b'\n') else {
157        return if b.len() < NMEA_MAX {
158            Step::Wait
159        } else {
160            Step::Skip
161        };
162    };
163    let line = &b[..nl];
164    let line = line.strip_suffix(b"\r").unwrap_or(line);
165    match nmea_valid(line) {
166        Some(s) => Step::Emit(Frame::Nmea(s), nl + 1),
167        None => Step::Skip,
168    }
169}
170
171/// Validate `$body*hh` and return it as text.
172fn nmea_valid(line: &[u8]) -> Option<String> {
173    if line.len() < 4 || line[0] != b'$' {
174        return None;
175    }
176    let star = line.len().checked_sub(3)?;
177    if line[star] != b'*' {
178        return None;
179    }
180    let want = u8::from_str_radix(std::str::from_utf8(&line[star + 1..]).ok()?, 16).ok()?;
181    let body = &line[1..star];
182    if !body.iter().all(|c| (0x20..0x7F).contains(c)) {
183        return None;
184    }
185    let got = body.iter().fold(0u8, |acc, &c| acc ^ c);
186    (got == want).then(|| String::from_utf8_lossy(line).into_owned())
187}
188
189fn scan_rtcm(b: &[u8]) -> Step {
190    if b.len() < 3 {
191        return Step::Wait;
192    }
193    if b[1] & 0xFC != 0 {
194        return Step::Skip;
195    }
196    let len = (((b[1] & 0x03) as usize) << 8) | b[2] as usize;
197    let total = 3 + len + 3;
198    if b.len() < total {
199        return Step::Wait;
200    }
201    let crc = crc24q(&b[..3 + len]);
202    let got = ((b[3 + len] as u32) << 16) | ((b[4 + len] as u32) << 8) | b[5 + len] as u32;
203    if crc != got {
204        return Step::Skip;
205    }
206    Step::Emit(Frame::Rtcm(b[..total].to_vec()), total)
207}
208
209/// Build a UBX frame with a correct checksum.
210pub fn ubx_frame(class: u8, id: u8, payload: &[u8]) -> Vec<u8> {
211    let mut out = Vec::with_capacity(8 + payload.len());
212    out.extend_from_slice(&[0xB5, 0x62, class, id]);
213    out.extend_from_slice(&(payload.len() as u16).to_le_bytes());
214    out.extend_from_slice(payload);
215    let ck = ubx_checksum(&out[2..]);
216    out.extend_from_slice(&ck);
217    out
218}
219
220/// The RTCM3 message number of a whole frame (first 12 payload bits).
221pub fn rtcm_msg_type(frame: &[u8]) -> Option<u16> {
222    (frame.len() >= 5).then(|| ((frame[3] as u16) << 4) | (frame[4] as u16 >> 4))
223}
224
225#[cfg(test)]
226mod tests;
ados_positioning/stream.rs

ados_positioning/
stream.rs
