sbf_tools/blocks/
external.rs

1//! External sensor blocks (ExtSensorMeas, ExtSensorStatus, ExtSensorSetup)
2
3use crate::error::{SbfError, SbfResult};
4use crate::header::SbfHeader;
5
6use super::block_ids;
7use super::dnu::f64_or_none;
8use super::SbfBlockParse;
9
10#[cfg(test)]
11use super::dnu::F64_DNU;
12
13// ============================================================================
14// ExtSensorMeas Block
15// ============================================================================
16
17/// Single measurement from an external sensor (ExtSensorMeas sub-block)
18#[derive(Debug, Clone)]
19pub struct ExtSensorMeasSet {
20    pub source: u8,
21    pub sensor_model: u8,
22    pub meas_type: u8,
23    pub obs_info: u8,
24    x: f64,
25    y: f64,
26    z: f64,
27}
28
29impl ExtSensorMeasSet {
30    pub fn x_m(&self) -> Option<f64> {
31        f64_or_none(self.x)
32    }
33    pub fn y_m(&self) -> Option<f64> {
34        f64_or_none(self.y)
35    }
36    pub fn z_m(&self) -> Option<f64> {
37        f64_or_none(self.z)
38    }
39}
40
41/// ExtSensorMeas block (Block ID 4050)
42///
43/// External sensor measurements (accelerometer, gyro, etc.).
44#[derive(Debug, Clone)]
45pub struct ExtSensorMeasBlock {
46    tow_ms: u32,
47    wnc: u16,
48    pub n: u8,
49    pub sb_length: u8,
50    pub meas_sets: Vec<ExtSensorMeasSet>,
51}
52
53impl ExtSensorMeasBlock {
54    pub fn tow_seconds(&self) -> f64 {
55        self.tow_ms as f64 * 0.001
56    }
57    pub fn tow_ms(&self) -> u32 {
58        self.tow_ms
59    }
60    pub fn wnc(&self) -> u16 {
61        self.wnc
62    }
63}
64
65impl SbfBlockParse for ExtSensorMeasBlock {
66    const BLOCK_ID: u16 = block_ids::EXT_SENSOR_MEAS;
67
68    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
69        const MIN_LEN: usize = 16;
70        if data.len() < MIN_LEN {
71            return Err(SbfError::ParseError("ExtSensorMeas too short".into()));
72        }
73
74        let n = data[12];
75        let sb_length = data[13];
76
77        if sb_length < 28 {
78            return Err(SbfError::ParseError(
79                "ExtSensorMeas SBLength too small for MeasSet".into(),
80            ));
81        }
82
83        let mut meas_sets = Vec::with_capacity(n as usize);
84        let mut offset = 14usize;
85
86        for _ in 0..n {
87            if offset + 28 > data.len() {
88                return Err(SbfError::ParseError(
89                    "ExtSensorMeas sub-block exceeds block length".into(),
90                ));
91            }
92
93            let source = data[offset];
94            let sensor_model = data[offset + 1];
95            let meas_type = data[offset + 2];
96            let obs_info = data[offset + 3];
97            let x = f64::from_le_bytes(data[offset + 4..offset + 12].try_into().unwrap());
98            let y = f64::from_le_bytes(data[offset + 12..offset + 20].try_into().unwrap());
99            let z = f64::from_le_bytes(data[offset + 20..offset + 28].try_into().unwrap());
100
101            meas_sets.push(ExtSensorMeasSet {
102                source,
103                sensor_model,
104                meas_type,
105                obs_info,
106                x,
107                y,
108                z,
109            });
110
111            offset += sb_length as usize;
112        }
113
114        Ok(Self {
115            tow_ms: header.tow_ms,
116            wnc: header.wnc,
117            n,
118            sb_length,
119            meas_sets,
120        })
121    }
122}
123
124// ============================================================================
125// ExtSensorStatus Block
126// ============================================================================
127
128/// ExtSensorStatus block (Block ID 4056)
129///
130/// External sensor status/health information.
131#[derive(Debug, Clone)]
132pub struct ExtSensorStatusBlock {
133    tow_ms: u32,
134    wnc: u16,
135    pub source: u8,
136    pub sensor_model: u8,
137    pub status_type: u8,
138    pub status_bits: Vec<u8>,
139}
140
141impl ExtSensorStatusBlock {
142    pub fn tow_seconds(&self) -> f64 {
143        self.tow_ms as f64 * 0.001
144    }
145    pub fn tow_ms(&self) -> u32 {
146        self.tow_ms
147    }
148    pub fn wnc(&self) -> u16 {
149        self.wnc
150    }
151}
152
153impl SbfBlockParse for ExtSensorStatusBlock {
154    const BLOCK_ID: u16 = block_ids::EXT_SENSOR_STATUS;
155
156    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
157        const MIN_LEN: usize = 15;
158        if data.len() < MIN_LEN {
159            return Err(SbfError::ParseError("ExtSensorStatus too short".into()));
160        }
161
162        let source = data[12];
163        let sensor_model = data[13];
164        let status_type = data[14];
165        let status_bits = data[15..].to_vec();
166
167        Ok(Self {
168            tow_ms: header.tow_ms,
169            wnc: header.wnc,
170            source,
171            sensor_model,
172            status_type,
173            status_bits,
174        })
175    }
176}
177
178// ============================================================================
179// ExtSensorSetup Block
180// ============================================================================
181
182/// Single sensor setup entry (ExtSensorSetup sub-block)
183#[derive(Debug, Clone)]
184pub struct ExtSensorSetupEntry {
185    pub source: u8,
186    pub sensor_model: u8,
187    pub meas_type: u16,
188}
189
190/// ExtSensorSetup block (Block ID 4057)
191///
192/// External sensor configuration/setup.
193#[derive(Debug, Clone)]
194pub struct ExtSensorSetupBlock {
195    tow_ms: u32,
196    wnc: u16,
197    pub n: u8,
198    pub sb_length: u8,
199    pub sensors: Vec<ExtSensorSetupEntry>,
200}
201
202impl ExtSensorSetupBlock {
203    pub fn tow_seconds(&self) -> f64 {
204        self.tow_ms as f64 * 0.001
205    }
206    pub fn tow_ms(&self) -> u32 {
207        self.tow_ms
208    }
209    pub fn wnc(&self) -> u16 {
210        self.wnc
211    }
212}
213
214impl SbfBlockParse for ExtSensorSetupBlock {
215    const BLOCK_ID: u16 = block_ids::EXT_SENSOR_SETUP;
216
217    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
218        const MIN_LEN: usize = 16;
219        if data.len() < MIN_LEN {
220            return Err(SbfError::ParseError("ExtSensorSetup too short".into()));
221        }
222
223        let n = data[12];
224        let sb_length = data[13];
225
226        if sb_length < 4 {
227            return Err(SbfError::ParseError(
228                "ExtSensorSetup SBLength too small for OneSensor".into(),
229            ));
230        }
231
232        let mut sensors = Vec::with_capacity(n as usize);
233        let mut offset = 14usize;
234
235        for _ in 0..n {
236            if offset + 4 > data.len() {
237                return Err(SbfError::ParseError(
238                    "ExtSensorSetup sub-block exceeds block length".into(),
239                ));
240            }
241
242            let source = data[offset];
243            let sensor_model = data[offset + 1];
244            let meas_type = u16::from_le_bytes([data[offset + 2], data[offset + 3]]);
245
246            sensors.push(ExtSensorSetupEntry {
247                source,
248                sensor_model,
249                meas_type,
250            });
251
252            offset += sb_length as usize;
253        }
254
255        Ok(Self {
256            tow_ms: header.tow_ms,
257            wnc: header.wnc,
258            n,
259            sb_length,
260            sensors,
261        })
262    }
263}
264
265#[cfg(test)]
266mod tests {
267    use super::*;
268    use crate::header::SbfHeader;
269
270    fn header_for(block_id: u16, tow_ms: u32, wnc: u16) -> SbfHeader {
271        SbfHeader {
272            crc: 0,
273            block_id,
274            block_rev: 0,
275            length: 100,
276            tow_ms,
277            wnc,
278        }
279    }
280
281    #[test]
282    fn test_ext_sensor_meas_parse() {
283        let mut data = vec![0u8; 70];
284        data[6..10].copy_from_slice(&5000u32.to_le_bytes());
285        data[10..12].copy_from_slice(&2400u16.to_le_bytes());
286        data[12] = 2; // N
287        data[13] = 28; // SBLength
288        data[14] = 1; // Source
289        data[15] = 2; // SensorModel
290        data[16] = 3; // Type
291        data[17] = 0; // ObsInfo
292        data[18..26].copy_from_slice(&1.5f64.to_le_bytes());
293        data[26..34].copy_from_slice(&2.5f64.to_le_bytes());
294        data[34..42].copy_from_slice(&3.5f64.to_le_bytes());
295        data[42] = 2; // Source
296        data[43] = 3; // SensorModel
297        data[44] = 4; // Type
298        data[45] = 0; // ObsInfo
299        data[46..54].copy_from_slice(&4.0f64.to_le_bytes());
300        data[54..62].copy_from_slice(&5.0f64.to_le_bytes());
301        data[62..70].copy_from_slice(&6.0f64.to_le_bytes());
302
303        let header = header_for(block_ids::EXT_SENSOR_MEAS, 5000, 2400);
304        let block = ExtSensorMeasBlock::parse(&header, &data).unwrap();
305        assert_eq!(block.tow_seconds(), 5.0);
306        assert_eq!(block.wnc(), 2400);
307        assert_eq!(block.n, 2);
308        assert_eq!(block.meas_sets.len(), 2);
309        assert_eq!(block.meas_sets[0].source, 1);
310        assert_eq!(block.meas_sets[0].x_m(), Some(1.5));
311        assert_eq!(block.meas_sets[0].y_m(), Some(2.5));
312        assert_eq!(block.meas_sets[0].z_m(), Some(3.5));
313        assert_eq!(block.meas_sets[1].x_m(), Some(4.0));
314    }
315
316    #[test]
317    fn test_ext_sensor_meas_dnu() {
318        let mut data = vec![0u8; 50];
319        data[6..10].copy_from_slice(&5000u32.to_le_bytes());
320        data[10..12].copy_from_slice(&2400u16.to_le_bytes());
321        data[12] = 1;
322        data[13] = 28;
323        data[18..26].copy_from_slice(&F64_DNU.to_le_bytes());
324
325        let header = header_for(block_ids::EXT_SENSOR_MEAS, 5000, 2400);
326        let block = ExtSensorMeasBlock::parse(&header, &data).unwrap();
327        assert!(block.meas_sets[0].x_m().is_none());
328    }
329
330    #[test]
331    fn test_ext_sensor_status_parse() {
332        let mut data = vec![0u8; 20];
333        data[6..10].copy_from_slice(&6000u32.to_le_bytes());
334        data[10..12].copy_from_slice(&2500u16.to_le_bytes());
335        data[12] = 1; // Source
336        data[13] = 2; // SensorModel
337        data[14] = 0; // StatusType
338        data[15..20].copy_from_slice(&[0x01, 0x02, 0x03, 0x04, 0x05]);
339
340        let header = header_for(block_ids::EXT_SENSOR_STATUS, 6000, 2500);
341        let block = ExtSensorStatusBlock::parse(&header, &data).unwrap();
342        assert_eq!(block.tow_seconds(), 6.0);
343        assert_eq!(block.source, 1);
344        assert_eq!(block.sensor_model, 2);
345        assert_eq!(block.status_bits.len(), 5);
346    }
347
348    #[test]
349    fn test_ext_sensor_setup_parse() {
350        let mut data = vec![0u8; 30];
351        data[6..10].copy_from_slice(&7000u32.to_le_bytes());
352        data[10..12].copy_from_slice(&2600u16.to_le_bytes());
353        data[12] = 2; // N
354        data[13] = 4; // SBLength
355        data[14] = 1;
356        data[15] = 2;
357        data[16..18].copy_from_slice(&0x0102u16.to_le_bytes());
358        data[18] = 2;
359        data[19] = 3;
360        data[20..22].copy_from_slice(&0x0304u16.to_le_bytes());
361
362        let header = header_for(block_ids::EXT_SENSOR_SETUP, 7000, 2600);
363        let block = ExtSensorSetupBlock::parse(&header, &data).unwrap();
364        assert_eq!(block.tow_seconds(), 7.0);
365        assert_eq!(block.n, 2);
366        assert_eq!(block.sensors.len(), 2);
367        assert_eq!(block.sensors[0].source, 1);
368        assert_eq!(block.sensors[0].meas_type, 0x0102);
369        assert_eq!(block.sensors[1].sensor_model, 3);
370    }
371}
sbf_tools/blocks/external.rs

sbf_tools/blocks/
external.rs
