sbf_tools/blocks/
attitude.rs

1//! Attitude blocks (AttEuler, AttCovEuler, AuxAntPositions, EndOfAtt)
2
3use crate::error::{SbfError, SbfResult};
4use crate::header::SbfHeader;
5
6use super::block_ids;
7use super::dnu::{f32_or_none, f64_or_none, u8_or_none, F32_DNU};
8use super::SbfBlockParse;
9
10#[cfg(test)]
11use super::dnu::F64_DNU;
12
13// ============================================================================
14// AttEuler Block
15// ============================================================================
16
17/// AttEuler block (Block ID 5938)
18///
19/// Attitude solution in Euler angles.
20#[derive(Debug, Clone)]
21pub struct AttEulerBlock {
22    tow_ms: u32,
23    wnc: u16,
24    nr_sv: u8,
25    error: u8,
26    mode: u16,
27    datum: u8,
28    heading_deg: f32,
29    pitch_deg: f32,
30    roll_deg: f32,
31    pitch_rate_dps: f32,
32    roll_rate_dps: f32,
33    heading_rate_dps: f32,
34}
35
36impl AttEulerBlock {
37    pub fn tow_seconds(&self) -> f64 {
38        self.tow_ms as f64 * 0.001
39    }
40    pub fn tow_ms(&self) -> u32 {
41        self.tow_ms
42    }
43    pub fn wnc(&self) -> u16 {
44        self.wnc
45    }
46
47    /// Number of satellites included in attitude calculations.
48    ///
49    /// Returns `0` when the SBF `NrSV` field is not available (`255`). Use
50    /// [`Self::num_satellites_opt`] to distinguish unavailable from a real zero.
51    pub fn num_satellites(&self) -> u8 {
52        u8_or_none(self.nr_sv).unwrap_or(0)
53    }
54    /// Number of satellites included in attitude calculations, or `None` when unavailable.
55    pub fn num_satellites_opt(&self) -> Option<u8> {
56        u8_or_none(self.nr_sv)
57    }
58    /// Raw `NrSV` field from the SBF block.
59    pub fn num_satellites_raw(&self) -> u8 {
60        self.nr_sv
61    }
62    pub fn error_raw(&self) -> u8 {
63        self.error
64    }
65    pub fn mode_raw(&self) -> u16 {
66        self.mode
67    }
68    pub fn datum(&self) -> u8 {
69        self.datum
70    }
71
72    pub fn heading_deg(&self) -> Option<f32> {
73        f32_or_none(self.heading_deg)
74    }
75    pub fn pitch_deg(&self) -> Option<f32> {
76        f32_or_none(self.pitch_deg)
77    }
78    pub fn roll_deg(&self) -> Option<f32> {
79        f32_or_none(self.roll_deg)
80    }
81    pub fn pitch_rate_dps(&self) -> Option<f32> {
82        f32_or_none(self.pitch_rate_dps)
83    }
84    pub fn roll_rate_dps(&self) -> Option<f32> {
85        f32_or_none(self.roll_rate_dps)
86    }
87    pub fn heading_rate_dps(&self) -> Option<f32> {
88        f32_or_none(self.heading_rate_dps)
89    }
90}
91
92impl SbfBlockParse for AttEulerBlock {
93    const BLOCK_ID: u16 = block_ids::ATT_EULER;
94
95    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
96        if data.len() < 42 {
97            return Err(SbfError::ParseError("AttEuler too short".into()));
98        }
99
100        // Offsets:
101        // 12: NrSV
102        // 13: Error
103        // 14-15: Mode (u16)
104        // 16: Datum
105        // 17: Reserved
106        // 18-21: Heading (f32)
107        // 22-25: Pitch (f32)
108        // 26-29: Roll (f32)
109        // 30-33: PitchDot (f32)
110        // 34-37: RollDot (f32)
111        // 38-41: HeadingDot (f32)
112
113        let nr_sv = data[12];
114        let error = data[13];
115        let mode = u16::from_le_bytes([data[14], data[15]]);
116        let datum = data[16];
117
118        let heading_deg = f32::from_le_bytes(data[18..22].try_into().unwrap());
119        let pitch_deg = f32::from_le_bytes(data[22..26].try_into().unwrap());
120        let roll_deg = f32::from_le_bytes(data[26..30].try_into().unwrap());
121        let pitch_rate_dps = f32::from_le_bytes(data[30..34].try_into().unwrap());
122        let roll_rate_dps = f32::from_le_bytes(data[34..38].try_into().unwrap());
123        let heading_rate_dps = f32::from_le_bytes(data[38..42].try_into().unwrap());
124
125        Ok(Self {
126            tow_ms: header.tow_ms,
127            wnc: header.wnc,
128            nr_sv,
129            error,
130            mode,
131            datum,
132            heading_deg,
133            pitch_deg,
134            roll_deg,
135            pitch_rate_dps,
136            roll_rate_dps,
137            heading_rate_dps,
138        })
139    }
140}
141
142// ============================================================================
143// AttCovEuler Block
144// ============================================================================
145
146/// AttCovEuler block (Block ID 5939)
147///
148/// Attitude covariance matrix for Euler angles.
149#[derive(Debug, Clone)]
150pub struct AttCovEulerBlock {
151    tow_ms: u32,
152    wnc: u16,
153    error: u8,
154    /// Heading variance (deg^2)
155    pub cov_head_head: f32,
156    /// Pitch variance (deg^2)
157    pub cov_pitch_pitch: f32,
158    /// Roll variance (deg^2)
159    pub cov_roll_roll: f32,
160    /// Heading/Pitch covariance (deg^2)
161    pub cov_head_pitch: f32,
162    /// Heading/Roll covariance (deg^2)
163    pub cov_head_roll: f32,
164    /// Pitch/Roll covariance (deg^2)
165    pub cov_pitch_roll: f32,
166}
167
168impl AttCovEulerBlock {
169    pub fn tow_seconds(&self) -> f64 {
170        self.tow_ms as f64 * 0.001
171    }
172    pub fn tow_ms(&self) -> u32 {
173        self.tow_ms
174    }
175    pub fn wnc(&self) -> u16 {
176        self.wnc
177    }
178
179    pub fn error_raw(&self) -> u8 {
180        self.error
181    }
182
183    pub fn heading_std_deg(&self) -> Option<f32> {
184        if self.cov_head_head == F32_DNU || self.cov_head_head < 0.0 {
185            None
186        } else {
187            Some(self.cov_head_head.sqrt())
188        }
189    }
190    pub fn pitch_std_deg(&self) -> Option<f32> {
191        if self.cov_pitch_pitch == F32_DNU || self.cov_pitch_pitch < 0.0 {
192            None
193        } else {
194            Some(self.cov_pitch_pitch.sqrt())
195        }
196    }
197    pub fn roll_std_deg(&self) -> Option<f32> {
198        if self.cov_roll_roll == F32_DNU || self.cov_roll_roll < 0.0 {
199            None
200        } else {
201            Some(self.cov_roll_roll.sqrt())
202        }
203    }
204}
205
206impl SbfBlockParse for AttCovEulerBlock {
207    const BLOCK_ID: u16 = block_ids::ATT_COV_EULER;
208
209    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
210        if data.len() < 38 {
211            return Err(SbfError::ParseError("AttCovEuler too short".into()));
212        }
213
214        // Offsets:
215        // 12: Reserved
216        // 13: Error
217        // 14-17: Cov_HeadHead (f32)
218        // 18-21: Cov_PitchPitch (f32)
219        // 22-25: Cov_RollRoll (f32)
220        // 26-29: Cov_HeadPitch (f32)
221        // 30-33: Cov_HeadRoll (f32)
222        // 34-37: Cov_PitchRoll (f32)
223
224        let error = data[13];
225        let cov_head_head = f32::from_le_bytes(data[14..18].try_into().unwrap());
226        let cov_pitch_pitch = f32::from_le_bytes(data[18..22].try_into().unwrap());
227        let cov_roll_roll = f32::from_le_bytes(data[22..26].try_into().unwrap());
228        let cov_head_pitch = f32::from_le_bytes(data[26..30].try_into().unwrap());
229        let cov_head_roll = f32::from_le_bytes(data[30..34].try_into().unwrap());
230        let cov_pitch_roll = f32::from_le_bytes(data[34..38].try_into().unwrap());
231
232        Ok(Self {
233            tow_ms: header.tow_ms,
234            wnc: header.wnc,
235            error,
236            cov_head_head,
237            cov_pitch_pitch,
238            cov_roll_roll,
239            cov_head_pitch,
240            cov_head_roll,
241            cov_pitch_roll,
242        })
243    }
244}
245
246// ============================================================================
247// AuxAntPositions Block
248// ============================================================================
249
250// TODO(spec-audit): AuxAntPositions (5942) is not documented in the mosaic-X5
251// FW v4.15.1 guide (attitude section and block index only list AttEuler,
252// AttCovEuler and EndOfAtt). Keep this legacy parser unchanged until an
253// official layout/revision table is available.
254
255/// Auxiliary antenna position info
256#[derive(Debug, Clone)]
257pub struct AuxAntPosition {
258    pub nr_sv: u8,
259    pub error: u8,
260    pub ambiguity_type: u8,
261    pub aux_ant_id: u8,
262    d_east_m: f64,
263    d_north_m: f64,
264    d_up_m: f64,
265    east_vel_mps: f64,
266    north_vel_mps: f64,
267    up_vel_mps: f64,
268}
269
270impl AuxAntPosition {
271    pub fn d_east_m(&self) -> Option<f64> {
272        f64_or_none(self.d_east_m)
273    }
274    pub fn d_north_m(&self) -> Option<f64> {
275        f64_or_none(self.d_north_m)
276    }
277    pub fn d_up_m(&self) -> Option<f64> {
278        f64_or_none(self.d_up_m)
279    }
280    pub fn velocity_east_mps(&self) -> Option<f64> {
281        f64_or_none(self.east_vel_mps)
282    }
283    pub fn velocity_north_mps(&self) -> Option<f64> {
284        f64_or_none(self.north_vel_mps)
285    }
286    pub fn velocity_up_mps(&self) -> Option<f64> {
287        f64_or_none(self.up_vel_mps)
288    }
289}
290
291/// AuxAntPositions block (Block ID 5942)
292#[derive(Debug, Clone)]
293pub struct AuxAntPositionsBlock {
294    tow_ms: u32,
295    wnc: u16,
296    pub positions: Vec<AuxAntPosition>,
297}
298
299impl AuxAntPositionsBlock {
300    pub fn tow_seconds(&self) -> f64 {
301        self.tow_ms as f64 * 0.001
302    }
303    pub fn tow_ms(&self) -> u32 {
304        self.tow_ms
305    }
306    pub fn wnc(&self) -> u16 {
307        self.wnc
308    }
309
310    pub fn num_positions(&self) -> usize {
311        self.positions.len()
312    }
313}
314
315impl SbfBlockParse for AuxAntPositionsBlock {
316    const BLOCK_ID: u16 = block_ids::AUX_ANT_POSITIONS;
317
318    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
319        if data.len() < 14 {
320            return Err(SbfError::ParseError("AuxAntPositions too short".into()));
321        }
322
323        let n = data[12] as usize;
324        let sb_length = data[13] as usize;
325
326        if sb_length < 52 {
327            return Err(SbfError::ParseError(
328                "AuxAntPositions SBLength too small".into(),
329            ));
330        }
331
332        let mut positions = Vec::new();
333        let mut offset = 14;
334
335        for _ in 0..n {
336            if offset + sb_length > data.len() {
337                break;
338            }
339
340            let nr_sv = data[offset];
341            let error = data[offset + 1];
342            let ambiguity_type = data[offset + 2];
343            let aux_ant_id = data[offset + 3];
344
345            let d_east_m = f64::from_le_bytes(data[offset + 4..offset + 12].try_into().unwrap());
346            let d_north_m = f64::from_le_bytes(data[offset + 12..offset + 20].try_into().unwrap());
347            let d_up_m = f64::from_le_bytes(data[offset + 20..offset + 28].try_into().unwrap());
348            let east_vel_mps =
349                f64::from_le_bytes(data[offset + 28..offset + 36].try_into().unwrap());
350            let north_vel_mps =
351                f64::from_le_bytes(data[offset + 36..offset + 44].try_into().unwrap());
352            let up_vel_mps = f64::from_le_bytes(data[offset + 44..offset + 52].try_into().unwrap());
353
354            positions.push(AuxAntPosition {
355                nr_sv,
356                error,
357                ambiguity_type,
358                aux_ant_id,
359                d_east_m,
360                d_north_m,
361                d_up_m,
362                east_vel_mps,
363                north_vel_mps,
364                up_vel_mps,
365            });
366
367            offset += sb_length;
368        }
369
370        Ok(Self {
371            tow_ms: header.tow_ms,
372            wnc: header.wnc,
373            positions,
374        })
375    }
376}
377
378// ============================================================================
379// EndOfAtt Block
380// ============================================================================
381
382/// EndOfAtt block (Block ID 5943)
383#[derive(Debug, Clone)]
384pub struct EndOfAttBlock {
385    tow_ms: u32,
386    wnc: u16,
387}
388
389impl EndOfAttBlock {
390    pub fn tow_seconds(&self) -> f64 {
391        self.tow_ms as f64 * 0.001
392    }
393    pub fn tow_ms(&self) -> u32 {
394        self.tow_ms
395    }
396    pub fn wnc(&self) -> u16 {
397        self.wnc
398    }
399}
400
401impl SbfBlockParse for EndOfAttBlock {
402    const BLOCK_ID: u16 = block_ids::END_OF_ATT;
403
404    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
405        if data.len() < 12 {
406            return Err(SbfError::ParseError("EndOfAtt too short".into()));
407        }
408
409        Ok(Self {
410            tow_ms: header.tow_ms,
411            wnc: header.wnc,
412        })
413    }
414}
415
416#[cfg(test)]
417mod tests {
418    use super::*;
419    use crate::header::SbfHeader;
420
421    fn header_for(block_id: u16, data_len: usize, tow_ms: u32, wnc: u16) -> SbfHeader {
422        SbfHeader {
423            crc: 0,
424            block_id,
425            block_rev: 0,
426            length: (data_len + 2) as u16,
427            tow_ms,
428            wnc,
429        }
430    }
431
432    #[test]
433    fn test_att_euler_scaled_accessors() {
434        let block = AttEulerBlock {
435            tow_ms: 1000,
436            wnc: 2000,
437            nr_sv: 12,
438            error: 0,
439            mode: 3,
440            datum: 1,
441            heading_deg: 45.0,
442            pitch_deg: -2.0,
443            roll_deg: 1.5,
444            pitch_rate_dps: 0.1,
445            roll_rate_dps: 0.2,
446            heading_rate_dps: -0.3,
447        };
448
449        assert!((block.tow_seconds() - 1.0).abs() < 1e-6);
450        assert_eq!(block.num_satellites(), 12);
451        assert_eq!(block.mode_raw(), 3);
452        assert!((block.heading_deg().unwrap() - 45.0).abs() < 1e-6);
453        assert!((block.roll_rate_dps().unwrap() - 0.2).abs() < 1e-6);
454    }
455
456    #[test]
457    fn test_att_euler_dnu_handling() {
458        let block = AttEulerBlock {
459            tow_ms: 0,
460            wnc: 0,
461            nr_sv: 255,
462            error: 0,
463            mode: 0,
464            datum: 0,
465            heading_deg: F32_DNU,
466            pitch_deg: F32_DNU,
467            roll_deg: 1.0,
468            pitch_rate_dps: F32_DNU,
469            roll_rate_dps: 0.0,
470            heading_rate_dps: F32_DNU,
471        };
472
473        assert_eq!(block.num_satellites_raw(), 255);
474        assert_eq!(block.num_satellites_opt(), None);
475        assert_eq!(block.num_satellites(), 0);
476        assert!(block.heading_deg().is_none());
477        assert!(block.pitch_deg().is_none());
478        assert!(block.roll_deg().is_some());
479        assert!(block.pitch_rate_dps().is_none());
480        assert!(block.heading_rate_dps().is_none());
481    }
482
483    #[test]
484    fn test_att_euler_parse() {
485        let mut data = vec![0u8; 42];
486        data[12] = 8; // NrSV
487        data[13] = 1; // Error
488        data[14..16].copy_from_slice(&500_u16.to_le_bytes());
489        data[16] = 2; // Datum
490        data[18..22].copy_from_slice(&10.5_f32.to_le_bytes());
491        data[22..26].copy_from_slice(&(-1.25_f32).to_le_bytes());
492        data[26..30].copy_from_slice(&0.75_f32.to_le_bytes());
493        data[30..34].copy_from_slice(&0.1_f32.to_le_bytes());
494        data[34..38].copy_from_slice(&0.2_f32.to_le_bytes());
495        data[38..42].copy_from_slice(&0.3_f32.to_le_bytes());
496
497        let header = header_for(block_ids::ATT_EULER, data.len(), 123456, 2048);
498        let block = AttEulerBlock::parse(&header, &data).unwrap();
499
500        assert_eq!(block.num_satellites(), 8);
501        assert_eq!(block.num_satellites_opt(), Some(8));
502        assert_eq!(block.num_satellites_raw(), 8);
503        assert_eq!(block.error_raw(), 1);
504        assert_eq!(block.mode_raw(), 500);
505        assert!((block.heading_deg().unwrap() - 10.5).abs() < 1e-6);
506        assert!((block.pitch_rate_dps().unwrap() - 0.1).abs() < 1e-6);
507    }
508
509    #[test]
510    fn test_att_cov_euler_std_accessors() {
511        let block = AttCovEulerBlock {
512            tow_ms: 2000,
513            wnc: 100,
514            error: 0,
515            cov_head_head: 4.0,
516            cov_pitch_pitch: 9.0,
517            cov_roll_roll: 16.0,
518            cov_head_pitch: 0.0,
519            cov_head_roll: 0.0,
520            cov_pitch_roll: 0.0,
521        };
522
523        assert!((block.heading_std_deg().unwrap() - 2.0).abs() < 1e-6);
524        assert!((block.pitch_std_deg().unwrap() - 3.0).abs() < 1e-6);
525        assert!((block.roll_std_deg().unwrap() - 4.0).abs() < 1e-6);
526        assert!((block.tow_seconds() - 2.0).abs() < 1e-6);
527    }
528
529    #[test]
530    fn test_att_cov_euler_dnu_handling() {
531        let block = AttCovEulerBlock {
532            tow_ms: 0,
533            wnc: 0,
534            error: 0,
535            cov_head_head: F32_DNU,
536            cov_pitch_pitch: -1.0,
537            cov_roll_roll: 1.0,
538            cov_head_pitch: 0.0,
539            cov_head_roll: 0.0,
540            cov_pitch_roll: 0.0,
541        };
542
543        assert!(block.heading_std_deg().is_none());
544        assert!(block.pitch_std_deg().is_none());
545        assert!(block.roll_std_deg().is_some());
546    }
547
548    #[test]
549    fn test_att_cov_euler_parse() {
550        let mut data = vec![0u8; 38];
551        data[13] = 2; // Error
552        data[14..18].copy_from_slice(&1.0_f32.to_le_bytes());
553        data[18..22].copy_from_slice(&4.0_f32.to_le_bytes());
554        data[22..26].copy_from_slice(&9.0_f32.to_le_bytes());
555        data[26..30].copy_from_slice(&0.1_f32.to_le_bytes());
556        data[30..34].copy_from_slice(&0.2_f32.to_le_bytes());
557        data[34..38].copy_from_slice(&0.3_f32.to_le_bytes());
558
559        let header = header_for(block_ids::ATT_COV_EULER, data.len(), 654321, 1024);
560        let block = AttCovEulerBlock::parse(&header, &data).unwrap();
561
562        assert_eq!(block.error_raw(), 2);
563        assert!((block.heading_std_deg().unwrap() - 1.0).abs() < 1e-6);
564    }
565
566    #[test]
567    fn test_aux_ant_positions_accessors() {
568        let info = AuxAntPosition {
569            nr_sv: 7,
570            error: 0,
571            ambiguity_type: 1,
572            aux_ant_id: 2,
573            d_east_m: 1.5,
574            d_north_m: -2.5,
575            d_up_m: 0.5,
576            east_vel_mps: 0.1,
577            north_vel_mps: -0.2,
578            up_vel_mps: 0.0,
579        };
580
581        assert_eq!(info.nr_sv, 7);
582        assert!((info.d_east_m().unwrap() - 1.5).abs() < 1e-6);
583        assert!((info.velocity_north_mps().unwrap() + 0.2).abs() < 1e-6);
584    }
585
586    #[test]
587    fn test_aux_ant_positions_dnu_handling() {
588        let info = AuxAntPosition {
589            nr_sv: 0,
590            error: 0,
591            ambiguity_type: 0,
592            aux_ant_id: 0,
593            d_east_m: F64_DNU,
594            d_north_m: 1.0,
595            d_up_m: F64_DNU,
596            east_vel_mps: F64_DNU,
597            north_vel_mps: 0.0,
598            up_vel_mps: F64_DNU,
599        };
600
601        assert!(info.d_east_m().is_none());
602        assert!(info.d_up_m().is_none());
603        assert!(info.velocity_east_mps().is_none());
604        assert!(info.velocity_up_mps().is_none());
605        assert!(info.velocity_north_mps().is_some());
606    }
607
608    #[test]
609    fn test_aux_ant_positions_parse() {
610        let mut data = vec![0u8; 14 + 52];
611        data[12] = 1; // N
612        data[13] = 52; // SBLength
613
614        let offset = 14;
615        data[offset] = 5; // NrSV
616        data[offset + 1] = 1; // Error
617        data[offset + 2] = 2; // AmbiguityType
618        data[offset + 3] = 3; // AuxAntID
619        data[offset + 4..offset + 12].copy_from_slice(&1.0_f64.to_le_bytes());
620        data[offset + 12..offset + 20].copy_from_slice(&2.0_f64.to_le_bytes());
621        data[offset + 20..offset + 28].copy_from_slice(&3.0_f64.to_le_bytes());
622        data[offset + 28..offset + 36].copy_from_slice(&0.1_f64.to_le_bytes());
623        data[offset + 36..offset + 44].copy_from_slice(&0.2_f64.to_le_bytes());
624        data[offset + 44..offset + 52].copy_from_slice(&0.3_f64.to_le_bytes());
625
626        let header = header_for(block_ids::AUX_ANT_POSITIONS, data.len(), 9999, 55);
627        let block = AuxAntPositionsBlock::parse(&header, &data).unwrap();
628
629        assert_eq!(block.num_positions(), 1);
630        let entry = &block.positions[0];
631        assert_eq!(entry.aux_ant_id, 3);
632        assert!((entry.d_north_m().unwrap() - 2.0).abs() < 1e-6);
633    }
634
635    #[test]
636    fn test_end_of_att_parse() {
637        let data = vec![0u8; 14];
638        let header = header_for(block_ids::END_OF_ATT, data.len(), 2500, 42);
639        let block = EndOfAttBlock::parse(&header, &data).unwrap();
640
641        assert!((block.tow_seconds() - 2.5).abs() < 1e-6);
642        assert_eq!(block.wnc(), 42);
643    }
644}
sbf_tools/blocks/attitude.rs

sbf_tools/blocks/
attitude.rs
