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

sbf_tools/blocks/
attitude.rs
