libsbf/
lib.rs

1//! A no_std parser for the SBF (Septentrio Binary Format) using the
2//! [sans-io](https://sans-io.readthedocs.io/) philosophy.
3//!
4//! ## `std` BufReader Iterator
5//! There is also a `std` API that exposes an `SbfReader` that uses a
6//! BufReader. The `SbfReader` implements an `Iterator` that will give
7//! you `libsbf::Messages`. To enable this do `cargo add libsbf -F std`
8
9#![cfg_attr(docsrs, feature(doc_cfg))]
10#![cfg_attr(not(feature = "std"), no_std)]
11use binrw::binrw;
12
13extern crate alloc;
14use alloc::vec::Vec;
15
16pub mod parser;
17
18#[cfg(feature = "std")]
19#[cfg_attr(docsrs, doc(cfg(feature = "std")))]
20pub mod reader;
21
22const DO_NOT_USE_I2: i16 = -32768;
23const DO_NOT_USE_U1: u8  = 255;
24const DO_NOT_USE_U2: u16 = 65535;
25const DO_NOT_USE_U4: u32 = 4294967295;
26const DO_NOT_USE_F4: f32 = -2e10;
27const DO_NOT_USE_F8: f64 = -2e10;
28
29
30
31#[binrw]
32#[derive(Debug)]
33struct Id {
34    pub bytes: u16,
35}
36
37impl Id {
38    fn message_type(&self) -> MessageKind {
39        MessageKind::from(self.block_number())
40    }
41
42    fn block_number(&self) -> u16 {
43        // NOTE: Bits 0-12 are the actual Block Number
44        self.bytes & 0x1FFF
45    }
46
47    fn _block_rev_number(&self) -> u16 {
48        // NOTE: Bits 13-15 are the Block Revision Number
49        self.bytes & 0xE000
50    }
51}
52
53#[binrw]
54#[derive(Debug)]
55struct Header {
56    pub crc: u16,
57    pub block_id: Id,
58    pub length: u16,
59}
60
61macro_rules! define_messages {
62    ( $( $variant:ident => $code:expr ),+ $(,)? ) => {
63        /// Core enum that just represents the message kind.
64        #[derive(Debug, Copy, Clone, PartialEq, Eq)]
65        enum MessageKind {
66            $( $variant, )+
67            Unsupported,
68        }
69
70        impl From<u16> for MessageKind {
71            fn from(block_number: u16) -> Self {
72                match block_number {
73                    $( $code => MessageKind::$variant, )+
74                    _ => MessageKind::Unsupported,
75                }
76            }
77        }
78
79        /// Detailed enum that holds the associated payload.
80        #[derive(Debug)]
81        pub enum Messages {
82            $( $variant($variant), )+
83            Unsupported,
84        }
85    };
86}
87
88define_messages!(
89    INSNavGeod => 4226,
90    AttEuler => 5938,
91    ExtSensorMeas => 4050,
92    QualityInd => 4082,
93);
94
95// Attitude Euler Block 5938
96#[binrw]
97#[derive(Debug)]
98pub struct AttEuler {
99    #[br(map = |x| if x == DO_NOT_USE_U4 { None } else { Some(x) })]
100    pub tow: Option<u32>,
101    #[br(map = |x| if x == DO_NOT_USE_U2 { None } else { Some(x) })]
102    pub wnc: Option<u16>,
103    #[br(map = |x| if x == DO_NOT_USE_U1 { None } else { Some(x) })]
104    pub nrsv: Option<u8>,
105    // TODO: create Error enum
106    pub error: u8,
107    pub mode: u16,
108    _reserved: u16,
109
110    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
111    pub heading: Option<f32>,
112    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
113    pub pitch: Option<f32>,
114    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
115    pub roll: Option<f32>,
116
117    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
118    pub pitch_dot: Option<f32>,
119    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
120    pub roll_dot: Option<f32>,
121    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
122    pub heading_dot: Option<f32>,
123}
124
125// External Sensor Measurement Block 4050
126#[binrw]
127#[derive(Debug)]
128pub struct ExtSensorMeas {
129    #[br(map = |x| if x == DO_NOT_USE_U4 { None } else { Some(x) })]
130    pub tow: Option<u32>,
131    #[br(map = |x| if x == DO_NOT_USE_U2 { None } else { Some(x) })]
132    pub wnc: Option<u16>,
133    pub n: u8,
134    pub sb_length: u8,
135    #[br(count = n)]
136    pub ext_sensor_meas_set: Vec<ExtSensorMeasSet>,
137}
138
139#[repr(u8)]
140pub enum ExtSensorMeasSetType {
141    Acceleration = 0,
142    AngularRate = 1,
143    _Reserved = 2,
144    Info = 3,
145    Velocity = 4,
146    ZeroVelocityFlag = 20,
147}
148
149#[binrw]
150#[derive(Debug)]
151pub struct ExtSensorMeasSet {
152    pub source: u8,
153    pub sensor_model: u8,
154    pub type_: u8,
155    pub obs_info: u8,
156
157    #[br(if(type_ == ExtSensorMeasSetType::Acceleration as u8))]
158    pub acc: Option<ExtSensorMeasAcceleration>,
159    #[br(if(type_ == ExtSensorMeasSetType::AngularRate as u8))]
160    pub ang_rate: Option<ExtSensorMeasAngularRate>,
161    #[br(if(type_ == ExtSensorMeasSetType::Info as u8))]
162    pub info: Option<ExtSensorMeasInfo>,
163    #[br(if(type_ == ExtSensorMeasSetType::Velocity as u8))]
164    pub vel: Option<ExtSensorMeasVelocity>,
165    #[br(if(type_ == ExtSensorMeasSetType::ZeroVelocityFlag as u8))]
166    pub zero_vel_flag: Option<ExtSensorMeasZeroVelocityFlag>,
167}
168
169#[binrw]
170#[derive(Debug)]
171pub struct ExtSensorMeasAcceleration {
172    #[br(map = |x| if x == DO_NOT_USE_F8 { None } else { Some(x) })]
173    pub ax: Option<f64>,
174    #[br(map = |x| if x == DO_NOT_USE_F8 { None } else { Some(x) })]
175    pub ay: Option<f64>,
176    #[br(map = |x| if x == DO_NOT_USE_F8 { None } else { Some(x) })]
177    pub az: Option<f64>,
178}
179
180#[binrw]
181#[derive(Debug)]
182pub struct ExtSensorMeasAngularRate {
183    #[br(map = |x| if x == DO_NOT_USE_F8 { None } else { Some(x) })]
184    pub wx: Option<f64>,
185    #[br(map = |x| if x == DO_NOT_USE_F8 { None } else { Some(x) })]
186    pub wy: Option<f64>,
187    #[br(map = |x| if x == DO_NOT_USE_F8 { None } else { Some(x) })]
188    pub wz: Option<f64>,
189}
190
191#[binrw]
192#[derive(Debug)]
193pub struct ExtSensorMeasVelocity {
194    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
195    pub vx: Option<f32>,
196    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
197    pub vy: Option<f32>,
198    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
199    pub vz: Option<f32>,
200    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
201    pub stdx: Option<f32>,
202    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
203    pub stdy: Option<f32>,
204    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
205    pub stdz: Option<f32>,
206}
207
208#[binrw]
209#[derive(Debug)]
210pub struct ExtSensorMeasInfo {
211    #[br(map = |x| if x == DO_NOT_USE_I2 { None } else { Some(x) })]
212    pub sensor_temp: Option<i16>,
213    pub _reserved: [u8; 22],
214}
215
216#[binrw]
217#[derive(Debug)]
218pub struct ExtSensorMeasZeroVelocityFlag {
219    pub zero_v_flag: f64,
220    pub _reserved: [u8; 16],
221}
222
223// INS Nav Geod Block 4226
224#[binrw]
225#[derive(Debug)]
226pub struct INSNavGeod {
227    #[br(map = |x| if x == DO_NOT_USE_U4 { None } else { Some(x) })]
228    pub tow: Option<u32>,
229    #[br(map = |x| if x == DO_NOT_USE_U2 { None } else { Some(x) })]
230    pub wnc: Option<u16>,
231    // TODO: create GNSSMode type for future telemetry info
232    pub gnss_mode: u8,
233    // TODO: create Error enum
234    pub error: u8,
235    // TODO: unpack this if we want more info
236    pub info: u16,
237    #[br(map = |x| if x == DO_NOT_USE_U2 { None } else { Some(x) })]
238    pub gnss_age: Option<u16>,
239    #[br(map = |x| if x == DO_NOT_USE_F8 { None } else { Some(x) })]
240    pub latitude: Option<f64>,
241    #[br(map = |x| if x == DO_NOT_USE_F8 { None } else { Some(x) })]
242    pub longitude: Option<f64>,
243    #[br(map = |x| if x == DO_NOT_USE_F8 { None } else { Some(x) })]
244    pub height: Option<f64>,
245    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
246    pub undulation: Option<f32>,
247    #[br(map = |x| if x == DO_NOT_USE_U2 { None } else { Some(x) })]
248    pub accuracy: Option<u16>,
249    #[br(map = |x| if x == DO_NOT_USE_U2 { None } else { Some(x) })]
250    pub latency: Option<u16>,
251    // TODO: create a Datum enum
252    #[br(map = |x| if x == DO_NOT_USE_U1 { None } else { Some(x) })]
253    pub datum: Option<u8>,
254    _reserved: u8,
255    // TODO: unpack into an SBList type so we know what INSNav Sub Blocks we can parse
256    pub sb_list: u16,
257
258    #[br(if((sb_list >> 0) & 1 == 1))]
259    pub pos_std_dev: Option<INSNavGeodPosStdDev>,
260    #[br(if((sb_list >> 1) & 1 == 1))]
261    pub att: Option<INSNavGeodAtt>,
262    #[br(if((sb_list >> 2) & 1 == 1))]
263    pub att_std_dev: Option<INSNavGeodAttStdDev>,
264    #[br(if((sb_list >> 3) & 1 == 1))]
265    pub vel: Option<INSNavGeodVel>,
266    #[br(if((sb_list >> 4) & 1 == 1))]
267    pub vel_std_dev: Option<INSNavGeodVelStdDev>,
268    #[br(if((sb_list >> 5) & 1 == 1))]
269    pub pos_cov: Option<INSNavGeodPosCov>,
270    #[br(if((sb_list >> 6) & 1 == 1))]
271    pub att_cov: Option<INSNavGeodAttCov>,
272    #[br(if((sb_list >> 7) & 1 == 1))]
273    pub vel_cov: Option<INSNavGeodVelCov>,
274}
275
276#[binrw]
277#[derive(Debug, PartialEq)]
278pub struct QualityInd {
279    #[br(map = |x| if x == DO_NOT_USE_U4 { None } else { Some(x) })]
280    pub tow: Option<u32>,
281    #[br(map = |x| if x == DO_NOT_USE_U2 { None } else { Some(x) })]
282    pub wnc: Option<u16>,
283    pub n: u8,
284    pub reserved: u8,
285    #[br( if(n > 0))]
286    pub indicator_1: Option<u16>,
287    #[br(if(n > 1))]
288    pub indicator_2: Option<u16>,
289    #[br(if(n > 2))]
290    pub indicator_3: Option<u16>,
291    #[br(if(n > 3))]
292    pub indicator_4: Option<u16>,
293    #[br(if(n > 4))]
294    pub indicator_5: Option<u16>,
295    #[br(if(n > 5))]
296    pub indicator_6: Option<u16>,
297    #[br(if(n > 6))]
298    pub indicator_7: Option<u16>,
299    #[br( if(n > 7))]
300    pub indicator_8: Option<u16>,
301}
302
303#[binrw]
304#[derive(Debug)]
305pub struct INSNavGeodPosStdDev {
306    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
307    pub longitude_std_dev: Option<f32>,
308    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
309    pub latitude_std_dev: Option<f32>,
310    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
311    pub height_std_dev: Option<f32>,
312}
313
314#[binrw]
315#[derive(Debug)]
316pub struct INSNavGeodAtt {
317    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
318    pub heading: Option<f32>,
319    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
320    pub pitch: Option<f32>,
321    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
322    pub roll: Option<f32>,
323}
324
325#[binrw]
326#[derive(Debug)]
327pub struct INSNavGeodAttStdDev {
328    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
329    pub heading_std_dev: Option<f32>,
330    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
331    pub pitch_std_dev: Option<f32>,
332    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
333    pub roll_std_dev: Option<f32>,
334}
335
336#[binrw]
337#[derive(Debug)]
338pub struct INSNavGeodVel {
339    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
340    pub ve: Option<f32>,
341    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
342    pub vn: Option<f32>,
343    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
344    pub vu: Option<f32>,
345}
346
347#[binrw]
348#[derive(Debug)]
349pub struct INSNavGeodVelStdDev {
350    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
351    pub ve_std_dev: Option<f32>,
352    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
353    pub vn_std_dev: Option<f32>,
354    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
355    pub vu_std_dev: Option<f32>,
356}
357
358#[binrw]
359#[derive(Debug)]
360pub struct INSNavGeodPosCov {
361    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
362    pub latitude_longitude_cov: Option<f32>,
363    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
364    pub latitude_height_cov: Option<f32>,
365    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
366    pub longitude_height_cov: Option<f32>,
367}
368
369#[binrw]
370#[derive(Debug)]
371pub struct INSNavGeodVelCov {
372    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
373    pub ve_vn_cov: Option<f32>,
374    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
375    pub ve_vu_cov: Option<f32>,
376    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
377    pub vn_vu_cov: Option<f32>,
378}
379
380#[binrw]
381#[derive(Debug)]
382pub struct INSNavGeodAttCov {
383    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
384    pub heading_pitch_cov: Option<f32>,
385    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
386    pub heading_roll_cov: Option<f32>,
387    #[br(map = |x| if x == DO_NOT_USE_F4 { None } else { Some(x) })]
388    pub pitch_roll_cov: Option<f32>,
389}
390
391pub fn is_sync(bytes: &[u8; 2]) -> bool {
392    bytes == b"$@"
393}
libsbf/lib.rs

libsbf/
lib.rs
