ados_rtcm/
lib.rs

1#![doc = include_str!("../README.md")]
2#![warn(missing_docs)]
3
4/// Payload capacity of one `GPS_RTCM_DATA` message.
5pub const FRAGMENT_LEN: usize = 180;
6/// Fragments one message may span (two-bit fragment id).
7pub const MAX_FRAGMENTS: usize = 4;
8/// Longest message the fragments can carry.
9pub const MAX_MESSAGE_LEN: usize = FRAGMENT_LEN * MAX_FRAGMENTS;
10
11/// One `GPS_RTCM_DATA` message body.
12#[derive(Debug, Clone, PartialEq, Eq)]
13pub struct Fragment {
14    /// `GPS_RTCM_DATA.flags`: bit 0 fragmented, bits 1-2 fragment id, bits
15    /// 3-7 sequence id. Zero for an unfragmented message.
16    pub flags: u8,
17    /// Bytes of `data` in use.
18    pub len: u8,
19    /// Fragment bytes, zero padded to [`FRAGMENT_LEN`].
20    pub data: [u8; FRAGMENT_LEN],
21}
22
23/// A message too long to carry in four fragments.
24#[derive(Debug, Clone, Copy, PartialEq, Eq)]
25pub struct TooLong(
26    /// Length of the rejected message in bytes.
27    pub usize,
28);
29
30impl std::fmt::Display for TooLong {
31    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
32        write!(
33            f,
34            "RTCM message of {} bytes exceeds {} bytes",
35            self.0, MAX_MESSAGE_LEN
36        )
37    }
38}
39
40impl std::error::Error for TooLong {}
41
42/// The number of `GPS_RTCM_DATA` messages one RTCM message of `len` bytes
43/// becomes.
44pub fn message_fragments(len: usize) -> Result<usize, TooLong> {
45    if len > MAX_MESSAGE_LEN {
46        Err(TooLong(len))
47    } else if len <= FRAGMENT_LEN {
48        Ok(1)
49    } else if len.is_multiple_of(FRAGMENT_LEN) && len < MAX_MESSAGE_LEN {
50        // Full fragments only: a zero-length one marks the end.
51        Ok(len / FRAGMENT_LEN + 1)
52    } else {
53        Ok(len.div_ceil(FRAGMENT_LEN))
54    }
55}
56
57/// Splits RTCM messages into `GPS_RTCM_DATA` fragments, owning the 5-bit
58/// sequence id.
59#[derive(Debug, Default, Clone)]
60pub struct Fragmenter {
61    seq: u8,
62}
63
64impl Fragmenter {
65    /// Fragment one message. The sequence id advances only for a message
66    /// that is actually fragmented.
67    pub fn fragment(&mut self, msg: &[u8]) -> Result<Vec<Fragment>, TooLong> {
68        let count = message_fragments(msg.len())?;
69        if msg.len() <= FRAGMENT_LEN {
70            let mut data = [0u8; FRAGMENT_LEN];
71            data[..msg.len()].copy_from_slice(msg);
72            return Ok(vec![Fragment {
73                flags: 0,
74                len: msg.len() as u8,
75                data,
76            }]);
77        }
78        let seq = self.seq & 0x1f;
79        self.seq = (self.seq + 1) & 0x1f;
80        Ok((0..count)
81            .map(|id| {
82                let start = (id * FRAGMENT_LEN).min(msg.len());
83                let chunk = &msg[start..(start + FRAGMENT_LEN).min(msg.len())];
84                let mut data = [0u8; FRAGMENT_LEN];
85                data[..chunk.len()].copy_from_slice(chunk);
86                Fragment {
87                    flags: 1 | ((id as u8 & 0x3) << 1) | (seq << 3),
88                    len: chunk.len() as u8,
89                    data,
90                }
91            })
92            .collect())
93    }
94}
95
96/// Number of `GPS_RTCM_DATA` messages a byte stream produces after
97/// [`split_rtcm3`].
98pub fn fragment_count(bytes: &[u8]) -> Result<usize, TooLong> {
99    split_rtcm3(bytes)
100        .into_iter()
101        .map(|m| message_fragments(m.len()))
102        .sum()
103}
104
105/// RTCM3 preamble byte.
106const PREAMBLE: u8 = 0xD3;
107
108/// CRC-24Q (polynomial 0x1864CFB, initial value 0), the RTCM3 frame check.
109pub fn crc24q(bytes: &[u8]) -> u32 {
110    let mut crc: u32 = 0;
111    for &b in bytes {
112        crc ^= (b as u32) << 16;
113        for _ in 0..8 {
114            crc <<= 1;
115            if crc & 0x0100_0000 != 0 {
116                crc ^= 0x0186_4CFB;
117            }
118        }
119    }
120    crc & 0x00FF_FFFF
121}
122
123/// Wrap `payload` (at most 1023 bytes) in an RTCM3 frame: preamble, length,
124/// payload, CRC-24Q.
125pub fn encode_rtcm3(payload: &[u8]) -> Vec<u8> {
126    let len = payload.len().min(0x3FF);
127    let mut v = Vec::with_capacity(len + 6);
128    v.extend([PREAMBLE, (len >> 8) as u8 & 0x03, len as u8]);
129    v.extend_from_slice(&payload[..len]);
130    let crc = crc24q(&v);
131    v.extend(&crc.to_be_bytes()[1..]);
132    v
133}
134
135/// Scan `bytes` for whole CRC-valid RTCM3 frames (`0xD3`, 6 reserved bits,
136/// 10-bit length, payload, 24-bit CRC-24Q), calling `frame` with the range of
137/// each in order. Bytes before a preamble are skipped; a preamble whose frame
138/// fails its CRC is skipped by one byte so the scan resyncs on the next one.
139/// Returns where the incomplete tail starts: the earliest preamble whose frame
140/// runs past the end of `bytes` and that no later valid frame overlaps (it may
141/// complete with more bytes), else `bytes.len()`. The tail is shorter than
142/// one maximum frame (1029 bytes).
143fn scan(bytes: &[u8], mut frame: impl FnMut(std::ops::Range<usize>)) -> usize {
144    let mut tail: Option<usize> = None;
145    let mut i = 0;
146    while i < bytes.len() {
147        if bytes[i] != PREAMBLE {
148            i += 1;
149            continue;
150        }
151        if i + 3 > bytes.len() {
152            tail.get_or_insert(i);
153            break;
154        }
155        let len = (((bytes[i + 1] & 0x03) as usize) << 8) | bytes[i + 2] as usize;
156        let body_end = i + 3 + len;
157        let end = body_end + 3;
158        if end > bytes.len() {
159            tail.get_or_insert(i);
160            i += 1;
161            continue;
162        }
163        let want =
164            u32::from_be_bytes([0, bytes[body_end], bytes[body_end + 1], bytes[body_end + 2]]);
165        if crc24q(&bytes[i..body_end]) == want {
166            // A valid frame inside a pending candidate's span proves the
167            // candidate was a stray preamble.
168            tail = None;
169            frame(i..end);
170            i = end;
171        } else {
172            i += 1;
173        }
174    }
175    tail.unwrap_or(bytes.len())
176}
177
178/// Split one byte buffer into its whole CRC-valid RTCM3 frames (see
179/// [`Rtcm3Splitter`] for a stream that arrives in pieces). An incomplete
180/// frame at the end is dropped.
181pub fn split_rtcm3(bytes: &[u8]) -> Vec<&[u8]> {
182    let mut out = Vec::new();
183    scan(bytes, |r| out.push(&bytes[r]));
184    out
185}
186
187/// Splits an RTCM3 byte stream delivered in arbitrary pieces into whole
188/// CRC-valid frames, carrying a partial frame over to the next push. The
189/// carried tail is bounded by one maximum frame; junk and corrupt frames are
190/// skipped with the same resync as [`split_rtcm3`].
191#[derive(Debug, Default, Clone)]
192pub struct Rtcm3Splitter {
193    buf: Vec<u8>,
194}
195
196impl Rtcm3Splitter {
197    /// Append `bytes` and return every frame completed by them.
198    pub fn push(&mut self, bytes: &[u8]) -> Vec<Vec<u8>> {
199        self.buf.extend_from_slice(bytes);
200        let mut out = Vec::new();
201        let buf = &self.buf;
202        let tail = scan(buf, |r| out.push(buf[r].to_vec()));
203        self.buf.drain(..tail);
204        out
205    }
206
207    /// Bytes held for a frame not yet complete.
208    pub fn pending(&self) -> usize {
209        self.buf.len()
210    }
211}
212
213#[cfg(test)]
214mod tests;
ados_rtcm/lib.rs

ados_rtcm/
lib.rs
