sbf_tools/blocks/
ins.rs

1//! INS (Integrated Navigation) blocks
2
3use crate::error::{SbfError, SbfResult};
4use crate::header::SbfHeader;
5use crate::types::{PvtError, PvtMode};
6
7use super::block_ids;
8use super::dnu::{F32_DNU, F64_DNU, I16_DNU, I32_DNU, U16_DNU};
9use super::SbfBlockParse;
10
11// ============================================================================
12// IntPVCart Block
13// ============================================================================
14
15/// IntPVCart block (Block ID 4060)
16///
17/// INS position and velocity in Cartesian (ECEF) coordinates.
18#[derive(Debug, Clone)]
19#[allow(dead_code)]
20pub struct IntPvCartBlock {
21    tow_ms: u32,
22    wnc: u16,
23    mode: u8,
24    error: u8,
25    info: u16,
26    nr_sv: u8,
27    nr_ant: u8,
28    gnss_pvt_mode: u8,
29    datum: u8,
30    gnss_age_raw: u16,
31    x_m: f64,
32    y_m: f64,
33    z_m: f64,
34    vx_mps: f32,
35    vy_mps: f32,
36    vz_mps: f32,
37    cog_deg: f32,
38}
39
40impl IntPvCartBlock {
41    pub fn tow_seconds(&self) -> f64 {
42        self.tow_ms as f64 * 0.001
43    }
44    pub fn tow_ms(&self) -> u32 {
45        self.tow_ms
46    }
47    pub fn wnc(&self) -> u16 {
48        self.wnc
49    }
50    pub fn mode(&self) -> PvtMode {
51        PvtMode::from_mode_byte(self.mode)
52    }
53    pub fn mode_raw(&self) -> u8 {
54        self.mode
55    }
56    pub fn error(&self) -> PvtError {
57        PvtError::from_error_byte(self.error)
58    }
59    pub fn error_raw(&self) -> u8 {
60        self.error
61    }
62    pub fn info(&self) -> u16 {
63        self.info
64    }
65    pub fn nr_sv(&self) -> u8 {
66        self.nr_sv
67    }
68    pub fn nr_ant(&self) -> u8 {
69        self.nr_ant
70    }
71    pub fn gnss_pvt_mode(&self) -> u8 {
72        self.gnss_pvt_mode
73    }
74    pub fn datum(&self) -> u8 {
75        self.datum
76    }
77    /// GNSS age in seconds (raw × 0.01)
78    pub fn gnss_age_seconds(&self) -> Option<f32> {
79        if self.gnss_age_raw == U16_DNU {
80            None
81        } else {
82            Some(self.gnss_age_raw as f32 * 0.01)
83        }
84    }
85    pub fn gnss_age_raw(&self) -> u16 {
86        self.gnss_age_raw
87    }
88    pub fn x_m(&self) -> Option<f64> {
89        if self.x_m == F64_DNU {
90            None
91        } else {
92            Some(self.x_m)
93        }
94    }
95    pub fn y_m(&self) -> Option<f64> {
96        if self.y_m == F64_DNU {
97            None
98        } else {
99            Some(self.y_m)
100        }
101    }
102    pub fn z_m(&self) -> Option<f64> {
103        if self.z_m == F64_DNU {
104            None
105        } else {
106            Some(self.z_m)
107        }
108    }
109    pub fn velocity_x_mps(&self) -> Option<f32> {
110        if self.vx_mps == F32_DNU {
111            None
112        } else {
113            Some(self.vx_mps)
114        }
115    }
116    pub fn velocity_y_mps(&self) -> Option<f32> {
117        if self.vy_mps == F32_DNU {
118            None
119        } else {
120            Some(self.vy_mps)
121        }
122    }
123    pub fn velocity_z_mps(&self) -> Option<f32> {
124        if self.vz_mps == F32_DNU {
125            None
126        } else {
127            Some(self.vz_mps)
128        }
129    }
130    pub fn course_over_ground_deg(&self) -> Option<f32> {
131        if self.cog_deg == F32_DNU {
132            None
133        } else {
134            Some(self.cog_deg)
135        }
136    }
137}
138
139impl SbfBlockParse for IntPvCartBlock {
140    const BLOCK_ID: u16 = block_ids::INT_PV_CART;
141
142    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
143        // Block-specific: Mode, Error, Info, NrSV, NrAnt, GNSSPVTMode, Datum, GNSSage,
144        // X, Y, Z (f64 each), Vx, Vy, Vz, COG (f32 each)
145        // 12 + 2 + 2 + 1 + 1 + 1 + 1 + 2 + 24 + 16 = 62 bytes min
146        const MIN_LEN: usize = 62;
147        if data.len() < MIN_LEN {
148            return Err(SbfError::ParseError("IntPVCart too short".into()));
149        }
150
151        let mode = data[12];
152        let error = data[13];
153        let info = u16::from_le_bytes([data[14], data[15]]);
154        let nr_sv = data[16];
155        let nr_ant = data[17];
156        let gnss_pvt_mode = data[18];
157        let datum = data[19];
158        let gnss_age_raw = u16::from_le_bytes([data[20], data[21]]);
159        let x_m = f64::from_le_bytes(data[22..30].try_into().unwrap());
160        let y_m = f64::from_le_bytes(data[30..38].try_into().unwrap());
161        let z_m = f64::from_le_bytes(data[38..46].try_into().unwrap());
162        let vx_mps = f32::from_le_bytes(data[46..50].try_into().unwrap());
163        let vy_mps = f32::from_le_bytes(data[50..54].try_into().unwrap());
164        let vz_mps = f32::from_le_bytes(data[54..58].try_into().unwrap());
165        let cog_deg = f32::from_le_bytes(data[58..62].try_into().unwrap());
166
167        Ok(Self {
168            tow_ms: header.tow_ms,
169            wnc: header.wnc,
170            mode,
171            error,
172            info,
173            nr_sv,
174            nr_ant,
175            gnss_pvt_mode,
176            datum,
177            gnss_age_raw,
178            x_m,
179            y_m,
180            z_m,
181            vx_mps,
182            vy_mps,
183            vz_mps,
184            cog_deg,
185        })
186    }
187}
188
189// ============================================================================
190// IntPVGeod Block
191// ============================================================================
192
193/// IntPVGeod block (Block ID 4061)
194///
195/// INS position and velocity in geodetic coordinates.
196#[derive(Debug, Clone)]
197#[allow(dead_code)]
198pub struct IntPvGeodBlock {
199    tow_ms: u32,
200    wnc: u16,
201    mode: u8,
202    error: u8,
203    info: u16,
204    nr_sv: u8,
205    nr_ant: u8,
206    gnss_pvt_mode: u8,
207    datum: u8,
208    gnss_age_raw: u16,
209    lat_rad: f64,
210    long_rad: f64,
211    alt_m: f64,
212    vn_mps: f32,
213    ve_mps: f32,
214    vu_mps: f32,
215    cog_deg: f32,
216}
217
218impl IntPvGeodBlock {
219    pub fn tow_seconds(&self) -> f64 {
220        self.tow_ms as f64 * 0.001
221    }
222    pub fn tow_ms(&self) -> u32 {
223        self.tow_ms
224    }
225    pub fn wnc(&self) -> u16 {
226        self.wnc
227    }
228    pub fn mode(&self) -> PvtMode {
229        PvtMode::from_mode_byte(self.mode)
230    }
231    pub fn mode_raw(&self) -> u8 {
232        self.mode
233    }
234    pub fn error(&self) -> PvtError {
235        PvtError::from_error_byte(self.error)
236    }
237    pub fn error_raw(&self) -> u8 {
238        self.error
239    }
240    pub fn info(&self) -> u16 {
241        self.info
242    }
243    pub fn nr_sv(&self) -> u8 {
244        self.nr_sv
245    }
246    pub fn nr_ant(&self) -> u8 {
247        self.nr_ant
248    }
249    pub fn gnss_pvt_mode(&self) -> u8 {
250        self.gnss_pvt_mode
251    }
252    pub fn datum(&self) -> u8 {
253        self.datum
254    }
255    pub fn gnss_age_seconds(&self) -> Option<f32> {
256        if self.gnss_age_raw == U16_DNU {
257            None
258        } else {
259            Some(self.gnss_age_raw as f32 * 0.01)
260        }
261    }
262    pub fn gnss_age_raw(&self) -> u16 {
263        self.gnss_age_raw
264    }
265    pub fn latitude_deg(&self) -> Option<f64> {
266        if self.lat_rad == F64_DNU {
267            None
268        } else {
269            Some(self.lat_rad.to_degrees())
270        }
271    }
272    pub fn longitude_deg(&self) -> Option<f64> {
273        if self.long_rad == F64_DNU {
274            None
275        } else {
276            Some(self.long_rad.to_degrees())
277        }
278    }
279    pub fn altitude_m(&self) -> Option<f64> {
280        if self.alt_m == F64_DNU {
281            None
282        } else {
283            Some(self.alt_m)
284        }
285    }
286    pub fn velocity_north_mps(&self) -> Option<f32> {
287        if self.vn_mps == F32_DNU {
288            None
289        } else {
290            Some(self.vn_mps)
291        }
292    }
293    pub fn velocity_east_mps(&self) -> Option<f32> {
294        if self.ve_mps == F32_DNU {
295            None
296        } else {
297            Some(self.ve_mps)
298        }
299    }
300    pub fn velocity_up_mps(&self) -> Option<f32> {
301        if self.vu_mps == F32_DNU {
302            None
303        } else {
304            Some(self.vu_mps)
305        }
306    }
307    pub fn course_over_ground_deg(&self) -> Option<f32> {
308        if self.cog_deg == F32_DNU {
309            None
310        } else {
311            Some(self.cog_deg)
312        }
313    }
314}
315
316impl SbfBlockParse for IntPvGeodBlock {
317    const BLOCK_ID: u16 = block_ids::INT_PV_GEOD;
318
319    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
320        const MIN_LEN: usize = 62;
321        if data.len() < MIN_LEN {
322            return Err(SbfError::ParseError("IntPVGeod too short".into()));
323        }
324
325        let mode = data[12];
326        let error = data[13];
327        let info = u16::from_le_bytes([data[14], data[15]]);
328        let nr_sv = data[16];
329        let nr_ant = data[17];
330        let gnss_pvt_mode = data[18];
331        let datum = data[19];
332        let gnss_age_raw = u16::from_le_bytes([data[20], data[21]]);
333        let lat_rad = f64::from_le_bytes(data[22..30].try_into().unwrap());
334        let long_rad = f64::from_le_bytes(data[30..38].try_into().unwrap());
335        let alt_m = f64::from_le_bytes(data[38..46].try_into().unwrap());
336        let vn_mps = f32::from_le_bytes(data[46..50].try_into().unwrap());
337        let ve_mps = f32::from_le_bytes(data[50..54].try_into().unwrap());
338        let vu_mps = f32::from_le_bytes(data[54..58].try_into().unwrap());
339        let cog_deg = f32::from_le_bytes(data[58..62].try_into().unwrap());
340
341        Ok(Self {
342            tow_ms: header.tow_ms,
343            wnc: header.wnc,
344            mode,
345            error,
346            info,
347            nr_sv,
348            nr_ant,
349            gnss_pvt_mode,
350            datum,
351            gnss_age_raw,
352            lat_rad,
353            long_rad,
354            alt_m,
355            vn_mps,
356            ve_mps,
357            vu_mps,
358            cog_deg,
359        })
360    }
361}
362
363// ============================================================================
364// IntPVAAGeod Block
365// ============================================================================
366
367/// IntPVAAGeod block (Block ID 4045)
368///
369/// INS position, velocity, and acceleration in geodetic coordinates.
370/// Uses scaled integers for compact representation.
371#[derive(Debug, Clone)]
372#[allow(dead_code)]
373pub struct IntPvaaGeodBlock {
374    tow_ms: u32,
375    wnc: u16,
376    mode: u8,
377    error: u8,
378    info: u16,
379    gnss_pvt_mode: u8,
380    datum: u8,
381    gnss_age_raw: u8,
382    nr_sv_ant: u8,
383    pos_fine: u8,
384    lat_raw: i32,
385    long_raw: i32,
386    alt_raw: i32,
387    vn_raw: i32,
388    ve_raw: i32,
389    vu_raw: i32,
390    ax_raw: i16,
391    ay_raw: i16,
392    az_raw: i16,
393    heading_raw: u16,
394    pitch_raw: i16,
395    roll_raw: i16,
396}
397
398impl IntPvaaGeodBlock {
399    pub fn tow_seconds(&self) -> f64 {
400        self.tow_ms as f64 * 0.001
401    }
402    pub fn tow_ms(&self) -> u32 {
403        self.tow_ms
404    }
405    pub fn wnc(&self) -> u16 {
406        self.wnc
407    }
408    pub fn mode(&self) -> PvtMode {
409        PvtMode::from_mode_byte(self.mode)
410    }
411    pub fn mode_raw(&self) -> u8 {
412        self.mode
413    }
414    pub fn error(&self) -> PvtError {
415        PvtError::from_error_byte(self.error)
416    }
417    pub fn error_raw(&self) -> u8 {
418        self.error
419    }
420    pub fn info(&self) -> u16 {
421        self.info
422    }
423    pub fn gnss_pvt_mode(&self) -> u8 {
424        self.gnss_pvt_mode
425    }
426    pub fn datum(&self) -> u8 {
427        self.datum
428    }
429    /// GNSS age in seconds (raw × 0.1)
430    pub fn gnss_age_seconds(&self) -> Option<f32> {
431        if self.gnss_age_raw == 255 {
432            None
433        } else {
434            Some(self.gnss_age_raw as f32 * 0.1)
435        }
436    }
437    pub fn gnss_age_raw(&self) -> u8 {
438        self.gnss_age_raw
439    }
440    pub fn nr_sv_ant(&self) -> u8 {
441        self.nr_sv_ant
442    }
443    pub fn pos_fine(&self) -> u8 {
444        self.pos_fine
445    }
446    /// NrSV = NrSVAnt >> 4
447    pub fn nr_sv(&self) -> u8 {
448        self.nr_sv_ant >> 4
449    }
450    /// NrAnt = NrSVAnt & 0x0F
451    pub fn nr_ant(&self) -> u8 {
452        self.nr_sv_ant & 0x0F
453    }
454    /// Latitude in degrees (Lat × 1e-7)
455    pub fn latitude_deg(&self) -> Option<f64> {
456        if self.lat_raw == I32_DNU {
457            None
458        } else {
459            Some(self.lat_raw as f64 * 1e-7)
460        }
461    }
462    /// Longitude in degrees (Long × 1e-7)
463    pub fn longitude_deg(&self) -> Option<f64> {
464        if self.long_raw == I32_DNU {
465            None
466        } else {
467            Some(self.long_raw as f64 * 1e-7)
468        }
469    }
470    /// Altitude in meters (Alt × 1e-3)
471    pub fn altitude_m(&self) -> Option<f64> {
472        if self.alt_raw == I32_DNU {
473            None
474        } else {
475            Some(self.alt_raw as f64 * 1e-3)
476        }
477    }
478    /// North velocity in m/s (Vn × 1e-3)
479    pub fn velocity_north_mps(&self) -> Option<f64> {
480        if self.vn_raw == I32_DNU {
481            None
482        } else {
483            Some(self.vn_raw as f64 * 1e-3)
484        }
485    }
486    /// East velocity in m/s (Ve × 1e-3)
487    pub fn velocity_east_mps(&self) -> Option<f64> {
488        if self.ve_raw == I32_DNU {
489            None
490        } else {
491            Some(self.ve_raw as f64 * 1e-3)
492        }
493    }
494    /// Up velocity in m/s (Vu × 1e-3)
495    pub fn velocity_up_mps(&self) -> Option<f64> {
496        if self.vu_raw == I32_DNU {
497            None
498        } else {
499            Some(self.vu_raw as f64 * 1e-3)
500        }
501    }
502    /// X acceleration in m/s² (Ax × 0.01)
503    pub fn acceleration_x_mps2(&self) -> Option<f64> {
504        if self.ax_raw == I16_DNU {
505            None
506        } else {
507            Some(self.ax_raw as f64 * 0.01)
508        }
509    }
510    /// Y acceleration in m/s² (Ay × 0.01)
511    pub fn acceleration_y_mps2(&self) -> Option<f64> {
512        if self.ay_raw == I16_DNU {
513            None
514        } else {
515            Some(self.ay_raw as f64 * 0.01)
516        }
517    }
518    /// Z acceleration in m/s² (Az × 0.01)
519    pub fn acceleration_z_mps2(&self) -> Option<f64> {
520        if self.az_raw == I16_DNU {
521            None
522        } else {
523            Some(self.az_raw as f64 * 0.01)
524        }
525    }
526    /// Heading in degrees (× 0.01)
527    pub fn heading_deg(&self) -> Option<f64> {
528        if self.heading_raw == U16_DNU {
529            None
530        } else {
531            Some(self.heading_raw as f64 * 0.01)
532        }
533    }
534    /// Pitch in degrees (× 0.01)
535    pub fn pitch_deg(&self) -> Option<f64> {
536        if self.pitch_raw == I16_DNU {
537            None
538        } else {
539            Some(self.pitch_raw as f64 * 0.01)
540        }
541    }
542    /// Roll in degrees (× 0.01)
543    pub fn roll_deg(&self) -> Option<f64> {
544        if self.roll_raw == I16_DNU {
545            None
546        } else {
547            Some(self.roll_raw as f64 * 0.01)
548        }
549    }
550}
551
552impl SbfBlockParse for IntPvaaGeodBlock {
553    const BLOCK_ID: u16 = block_ids::INT_PVA_AGEOD;
554
555    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
556        // Mode:1 Error:1 Info:2 GNSSPVTMode:1 Datum:1 GNSSage:1 NrSVAnt:1 PosFine:1
557        // Lat:4 Long:4 Alt:4 Vn:4 Ve:4 Vu:4 Ax:2 Ay:2 Az:2 Heading:2 Pitch:2 Roll:2
558        const MIN_LEN: usize = 57;
559        if data.len() < MIN_LEN {
560            return Err(SbfError::ParseError("IntPVAAGeod too short".into()));
561        }
562
563        let mode = data[12];
564        let error = data[13];
565        let info = u16::from_le_bytes([data[14], data[15]]);
566        let gnss_pvt_mode = data[16];
567        let datum = data[17];
568        let gnss_age_raw = data[18];
569        let nr_sv_ant = data[19];
570        let pos_fine = data[20];
571        let lat_raw = i32::from_le_bytes(data[21..25].try_into().unwrap());
572        let long_raw = i32::from_le_bytes(data[25..29].try_into().unwrap());
573        let alt_raw = i32::from_le_bytes(data[29..33].try_into().unwrap());
574        let vn_raw = i32::from_le_bytes(data[33..37].try_into().unwrap());
575        let ve_raw = i32::from_le_bytes(data[37..41].try_into().unwrap());
576        let vu_raw = i32::from_le_bytes(data[41..45].try_into().unwrap());
577        let ax_raw = i16::from_le_bytes(data[45..47].try_into().unwrap());
578        let ay_raw = i16::from_le_bytes(data[47..49].try_into().unwrap());
579        let az_raw = i16::from_le_bytes(data[49..51].try_into().unwrap());
580        let heading_raw = u16::from_le_bytes([data[51], data[52]]);
581        let pitch_raw = i16::from_le_bytes(data[53..55].try_into().unwrap());
582        let roll_raw = i16::from_le_bytes(data[55..57].try_into().unwrap());
583
584        Ok(Self {
585            tow_ms: header.tow_ms,
586            wnc: header.wnc,
587            mode,
588            error,
589            info,
590            gnss_pvt_mode,
591            datum,
592            gnss_age_raw,
593            nr_sv_ant,
594            pos_fine,
595            lat_raw,
596            long_raw,
597            alt_raw,
598            vn_raw,
599            ve_raw,
600            vu_raw,
601            ax_raw,
602            ay_raw,
603            az_raw,
604            heading_raw,
605            pitch_raw,
606            roll_raw,
607        })
608    }
609}
610
611// ============================================================================
612// IntAttEuler Block
613// ============================================================================
614
615/// IntAttEuler block (Block ID 4070)
616///
617/// INS attitude in Euler angles (heading, pitch, roll).
618#[derive(Debug, Clone)]
619#[allow(dead_code)]
620pub struct IntAttEulerBlock {
621    tow_ms: u32,
622    wnc: u16,
623    mode: u8,
624    error: u8,
625    info: u16,
626    nr_sv: u8,
627    nr_ant: u8,
628    datum: u8,
629    gnss_age_raw: u16,
630    heading_deg: f32,
631    pitch_deg: f32,
632    roll_deg: f32,
633    pitch_dot_dps: f32,
634    roll_dot_dps: f32,
635    heading_dot_dps: f32,
636}
637
638impl IntAttEulerBlock {
639    pub fn tow_seconds(&self) -> f64 {
640        self.tow_ms as f64 * 0.001
641    }
642    pub fn tow_ms(&self) -> u32 {
643        self.tow_ms
644    }
645    pub fn wnc(&self) -> u16 {
646        self.wnc
647    }
648    pub fn mode(&self) -> PvtMode {
649        PvtMode::from_mode_byte(self.mode)
650    }
651    pub fn mode_raw(&self) -> u8 {
652        self.mode
653    }
654    pub fn error(&self) -> PvtError {
655        PvtError::from_error_byte(self.error)
656    }
657    pub fn error_raw(&self) -> u8 {
658        self.error
659    }
660    pub fn info(&self) -> u16 {
661        self.info
662    }
663    pub fn nr_sv(&self) -> u8 {
664        self.nr_sv
665    }
666    pub fn nr_ant(&self) -> u8 {
667        self.nr_ant
668    }
669    pub fn datum(&self) -> u8 {
670        self.datum
671    }
672    pub fn gnss_age_seconds(&self) -> Option<f32> {
673        if self.gnss_age_raw == U16_DNU {
674            None
675        } else {
676            Some(self.gnss_age_raw as f32 * 0.01)
677        }
678    }
679    pub fn gnss_age_raw(&self) -> u16 {
680        self.gnss_age_raw
681    }
682    pub fn heading_deg(&self) -> Option<f32> {
683        if self.heading_deg == F32_DNU {
684            None
685        } else {
686            Some(self.heading_deg)
687        }
688    }
689    pub fn pitch_deg(&self) -> Option<f32> {
690        if self.pitch_deg == F32_DNU {
691            None
692        } else {
693            Some(self.pitch_deg)
694        }
695    }
696    pub fn roll_deg(&self) -> Option<f32> {
697        if self.roll_deg == F32_DNU {
698            None
699        } else {
700            Some(self.roll_deg)
701        }
702    }
703    pub fn pitch_rate_dps(&self) -> Option<f32> {
704        if self.pitch_dot_dps == F32_DNU {
705            None
706        } else {
707            Some(self.pitch_dot_dps)
708        }
709    }
710    pub fn roll_rate_dps(&self) -> Option<f32> {
711        if self.roll_dot_dps == F32_DNU {
712            None
713        } else {
714            Some(self.roll_dot_dps)
715        }
716    }
717    pub fn heading_rate_dps(&self) -> Option<f32> {
718        if self.heading_dot_dps == F32_DNU {
719            None
720        } else {
721            Some(self.heading_dot_dps)
722        }
723    }
724}
725
726impl SbfBlockParse for IntAttEulerBlock {
727    const BLOCK_ID: u16 = block_ids::INT_ATT_EULER;
728
729    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
730        const MIN_LEN: usize = 45;
731        if data.len() < MIN_LEN {
732            return Err(SbfError::ParseError("IntAttEuler too short".into()));
733        }
734
735        let mode = data[12];
736        let error = data[13];
737        let info = u16::from_le_bytes([data[14], data[15]]);
738        let nr_sv = data[16];
739        let nr_ant = data[17];
740        let datum = data[18];
741        let gnss_age_raw = u16::from_le_bytes([data[19], data[20]]);
742        let heading_deg = f32::from_le_bytes(data[21..25].try_into().unwrap());
743        let pitch_deg = f32::from_le_bytes(data[25..29].try_into().unwrap());
744        let roll_deg = f32::from_le_bytes(data[29..33].try_into().unwrap());
745        let pitch_dot_dps = f32::from_le_bytes(data[33..37].try_into().unwrap());
746        let roll_dot_dps = f32::from_le_bytes(data[37..41].try_into().unwrap());
747        let heading_dot_dps = f32::from_le_bytes(data[41..45].try_into().unwrap());
748
749        Ok(Self {
750            tow_ms: header.tow_ms,
751            wnc: header.wnc,
752            mode,
753            error,
754            info,
755            nr_sv,
756            nr_ant,
757            datum,
758            gnss_age_raw,
759            heading_deg,
760            pitch_deg,
761            roll_deg,
762            pitch_dot_dps,
763            roll_dot_dps,
764            heading_dot_dps,
765        })
766    }
767}
768
769// ============================================================================
770// IntPosCovCart Block
771// ============================================================================
772
773/// IntPosCovCart block (Block ID 4062)
774///
775/// INS position covariance matrix in Cartesian (ECEF) coordinates.
776#[derive(Debug, Clone)]
777#[allow(dead_code)]
778pub struct IntPosCovCartBlock {
779    tow_ms: u32,
780    wnc: u16,
781    mode: u8,
782    error: u8,
783    cov_xx: f32,
784    cov_yy: f32,
785    cov_zz: f32,
786    cov_xy: f32,
787    cov_xz: f32,
788    cov_yz: f32,
789}
790
791impl IntPosCovCartBlock {
792    pub fn tow_seconds(&self) -> f64 {
793        self.tow_ms as f64 * 0.001
794    }
795    pub fn tow_ms(&self) -> u32 {
796        self.tow_ms
797    }
798    pub fn wnc(&self) -> u16 {
799        self.wnc
800    }
801    pub fn mode(&self) -> PvtMode {
802        PvtMode::from_mode_byte(self.mode)
803    }
804    pub fn mode_raw(&self) -> u8 {
805        self.mode
806    }
807    pub fn error(&self) -> PvtError {
808        PvtError::from_error_byte(self.error)
809    }
810    pub fn error_raw(&self) -> u8 {
811        self.error
812    }
813
814    pub fn cov_xx(&self) -> Option<f32> {
815        if self.cov_xx == F32_DNU {
816            None
817        } else {
818            Some(self.cov_xx)
819        }
820    }
821    pub fn cov_yy(&self) -> Option<f32> {
822        if self.cov_yy == F32_DNU {
823            None
824        } else {
825            Some(self.cov_yy)
826        }
827    }
828    pub fn cov_zz(&self) -> Option<f32> {
829        if self.cov_zz == F32_DNU {
830            None
831        } else {
832            Some(self.cov_zz)
833        }
834    }
835    pub fn cov_xy(&self) -> Option<f32> {
836        if self.cov_xy == F32_DNU {
837            None
838        } else {
839            Some(self.cov_xy)
840        }
841    }
842    pub fn cov_xz(&self) -> Option<f32> {
843        if self.cov_xz == F32_DNU {
844            None
845        } else {
846            Some(self.cov_xz)
847        }
848    }
849    pub fn cov_yz(&self) -> Option<f32> {
850        if self.cov_yz == F32_DNU {
851            None
852        } else {
853            Some(self.cov_yz)
854        }
855    }
856
857    pub fn x_std_m(&self) -> Option<f32> {
858        if self.cov_xx == F32_DNU || self.cov_xx < 0.0 {
859            None
860        } else {
861            Some(self.cov_xx.sqrt())
862        }
863    }
864    pub fn y_std_m(&self) -> Option<f32> {
865        if self.cov_yy == F32_DNU || self.cov_yy < 0.0 {
866            None
867        } else {
868            Some(self.cov_yy.sqrt())
869        }
870    }
871    pub fn z_std_m(&self) -> Option<f32> {
872        if self.cov_zz == F32_DNU || self.cov_zz < 0.0 {
873            None
874        } else {
875            Some(self.cov_zz.sqrt())
876        }
877    }
878}
879
880// ============================================================================
881// IntVelCovCart Block
882// ============================================================================
883
884/// IntVelCovCart block (Block ID 4063)
885///
886/// INS velocity covariance matrix in Cartesian (ECEF) coordinates.
887#[derive(Debug, Clone)]
888#[allow(dead_code)]
889pub struct IntVelCovCartBlock {
890    tow_ms: u32,
891    wnc: u16,
892    mode: u8,
893    error: u8,
894    cov_vx_vx: f32,
895    cov_vy_vy: f32,
896    cov_vz_vz: f32,
897    cov_vx_vy: f32,
898    cov_vx_vz: f32,
899    cov_vy_vz: f32,
900}
901
902impl IntVelCovCartBlock {
903    pub fn tow_seconds(&self) -> f64 {
904        self.tow_ms as f64 * 0.001
905    }
906    pub fn tow_ms(&self) -> u32 {
907        self.tow_ms
908    }
909    pub fn wnc(&self) -> u16 {
910        self.wnc
911    }
912    pub fn mode(&self) -> PvtMode {
913        PvtMode::from_mode_byte(self.mode)
914    }
915    pub fn mode_raw(&self) -> u8 {
916        self.mode
917    }
918    pub fn error(&self) -> PvtError {
919        PvtError::from_error_byte(self.error)
920    }
921
922    pub fn cov_vx_vx(&self) -> Option<f32> {
923        if self.cov_vx_vx == F32_DNU {
924            None
925        } else {
926            Some(self.cov_vx_vx)
927        }
928    }
929    pub fn cov_vy_vy(&self) -> Option<f32> {
930        if self.cov_vy_vy == F32_DNU {
931            None
932        } else {
933            Some(self.cov_vy_vy)
934        }
935    }
936    pub fn cov_vz_vz(&self) -> Option<f32> {
937        if self.cov_vz_vz == F32_DNU {
938            None
939        } else {
940            Some(self.cov_vz_vz)
941        }
942    }
943    pub fn cov_vx_vy(&self) -> Option<f32> {
944        if self.cov_vx_vy == F32_DNU {
945            None
946        } else {
947            Some(self.cov_vx_vy)
948        }
949    }
950    pub fn cov_vx_vz(&self) -> Option<f32> {
951        if self.cov_vx_vz == F32_DNU {
952            None
953        } else {
954            Some(self.cov_vx_vz)
955        }
956    }
957    pub fn cov_vy_vz(&self) -> Option<f32> {
958        if self.cov_vy_vz == F32_DNU {
959            None
960        } else {
961            Some(self.cov_vy_vz)
962        }
963    }
964
965    pub fn vx_std_mps(&self) -> Option<f32> {
966        if self.cov_vx_vx == F32_DNU || self.cov_vx_vx < 0.0 {
967            None
968        } else {
969            Some(self.cov_vx_vx.sqrt())
970        }
971    }
972    pub fn vy_std_mps(&self) -> Option<f32> {
973        if self.cov_vy_vy == F32_DNU || self.cov_vy_vy < 0.0 {
974            None
975        } else {
976            Some(self.cov_vy_vy.sqrt())
977        }
978    }
979    pub fn vz_std_mps(&self) -> Option<f32> {
980        if self.cov_vz_vz == F32_DNU || self.cov_vz_vz < 0.0 {
981            None
982        } else {
983            Some(self.cov_vz_vz.sqrt())
984        }
985    }
986}
987
988impl SbfBlockParse for IntVelCovCartBlock {
989    const BLOCK_ID: u16 = block_ids::INT_VEL_COV_CART;
990
991    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
992        const MIN_LEN: usize = 38;
993        if data.len() < MIN_LEN {
994            return Err(SbfError::ParseError("IntVelCovCart too short".into()));
995        }
996
997        let mode = data[12];
998        let error = data[13];
999        let cov_vx_vx = f32::from_le_bytes(data[14..18].try_into().unwrap());
1000        let cov_vy_vy = f32::from_le_bytes(data[18..22].try_into().unwrap());
1001        let cov_vz_vz = f32::from_le_bytes(data[22..26].try_into().unwrap());
1002        let cov_vx_vy = f32::from_le_bytes(data[26..30].try_into().unwrap());
1003        let cov_vx_vz = f32::from_le_bytes(data[30..34].try_into().unwrap());
1004        let cov_vy_vz = f32::from_le_bytes(data[34..38].try_into().unwrap());
1005
1006        Ok(Self {
1007            tow_ms: header.tow_ms,
1008            wnc: header.wnc,
1009            mode,
1010            error,
1011            cov_vx_vx,
1012            cov_vy_vy,
1013            cov_vz_vz,
1014            cov_vx_vy,
1015            cov_vx_vz,
1016            cov_vy_vz,
1017        })
1018    }
1019}
1020
1021// ============================================================================
1022// IntPosCovGeod Block
1023// ============================================================================
1024
1025/// IntPosCovGeod block (Block ID 4064)
1026///
1027/// INS position covariance matrix in geodetic coordinates.
1028#[derive(Debug, Clone)]
1029#[allow(dead_code)]
1030pub struct IntPosCovGeodBlock {
1031    tow_ms: u32,
1032    wnc: u16,
1033    mode: u8,
1034    error: u8,
1035    cov_lat_lat: f32,
1036    cov_lon_lon: f32,
1037    cov_alt_alt: f32,
1038    cov_lat_lon: f32,
1039    cov_lat_alt: f32,
1040    cov_lon_alt: f32,
1041}
1042
1043impl IntPosCovGeodBlock {
1044    pub fn tow_seconds(&self) -> f64 {
1045        self.tow_ms as f64 * 0.001
1046    }
1047    pub fn tow_ms(&self) -> u32 {
1048        self.tow_ms
1049    }
1050    pub fn wnc(&self) -> u16 {
1051        self.wnc
1052    }
1053    pub fn mode(&self) -> PvtMode {
1054        PvtMode::from_mode_byte(self.mode)
1055    }
1056    pub fn mode_raw(&self) -> u8 {
1057        self.mode
1058    }
1059    pub fn error(&self) -> PvtError {
1060        PvtError::from_error_byte(self.error)
1061    }
1062
1063    pub fn cov_lat_lat(&self) -> Option<f32> {
1064        if self.cov_lat_lat == F32_DNU {
1065            None
1066        } else {
1067            Some(self.cov_lat_lat)
1068        }
1069    }
1070    pub fn cov_lon_lon(&self) -> Option<f32> {
1071        if self.cov_lon_lon == F32_DNU {
1072            None
1073        } else {
1074            Some(self.cov_lon_lon)
1075        }
1076    }
1077    pub fn cov_alt_alt(&self) -> Option<f32> {
1078        if self.cov_alt_alt == F32_DNU {
1079            None
1080        } else {
1081            Some(self.cov_alt_alt)
1082        }
1083    }
1084    pub fn cov_lat_lon(&self) -> Option<f32> {
1085        if self.cov_lat_lon == F32_DNU {
1086            None
1087        } else {
1088            Some(self.cov_lat_lon)
1089        }
1090    }
1091    pub fn cov_lat_alt(&self) -> Option<f32> {
1092        if self.cov_lat_alt == F32_DNU {
1093            None
1094        } else {
1095            Some(self.cov_lat_alt)
1096        }
1097    }
1098    pub fn cov_lon_alt(&self) -> Option<f32> {
1099        if self.cov_lon_alt == F32_DNU {
1100            None
1101        } else {
1102            Some(self.cov_lon_alt)
1103        }
1104    }
1105
1106    pub fn lat_std_m(&self) -> Option<f32> {
1107        if self.cov_lat_lat == F32_DNU || self.cov_lat_lat < 0.0 {
1108            None
1109        } else {
1110            Some(self.cov_lat_lat.sqrt())
1111        }
1112    }
1113    pub fn lon_std_m(&self) -> Option<f32> {
1114        if self.cov_lon_lon == F32_DNU || self.cov_lon_lon < 0.0 {
1115            None
1116        } else {
1117            Some(self.cov_lon_lon.sqrt())
1118        }
1119    }
1120    pub fn alt_std_m(&self) -> Option<f32> {
1121        if self.cov_alt_alt == F32_DNU || self.cov_alt_alt < 0.0 {
1122            None
1123        } else {
1124            Some(self.cov_alt_alt.sqrt())
1125        }
1126    }
1127}
1128
1129impl SbfBlockParse for IntPosCovGeodBlock {
1130    const BLOCK_ID: u16 = block_ids::INT_POS_COV_GEOD;
1131
1132    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
1133        const MIN_LEN: usize = 38;
1134        if data.len() < MIN_LEN {
1135            return Err(SbfError::ParseError("IntPosCovGeod too short".into()));
1136        }
1137
1138        let mode = data[12];
1139        let error = data[13];
1140        let cov_lat_lat = f32::from_le_bytes(data[14..18].try_into().unwrap());
1141        let cov_lon_lon = f32::from_le_bytes(data[18..22].try_into().unwrap());
1142        let cov_alt_alt = f32::from_le_bytes(data[22..26].try_into().unwrap());
1143        let cov_lat_lon = f32::from_le_bytes(data[26..30].try_into().unwrap());
1144        let cov_lat_alt = f32::from_le_bytes(data[30..34].try_into().unwrap());
1145        let cov_lon_alt = f32::from_le_bytes(data[34..38].try_into().unwrap());
1146
1147        Ok(Self {
1148            tow_ms: header.tow_ms,
1149            wnc: header.wnc,
1150            mode,
1151            error,
1152            cov_lat_lat,
1153            cov_lon_lon,
1154            cov_alt_alt,
1155            cov_lat_lon,
1156            cov_lat_alt,
1157            cov_lon_alt,
1158        })
1159    }
1160}
1161
1162// ============================================================================
1163// IntVelCovGeod Block
1164// ============================================================================
1165
1166/// IntVelCovGeod block (Block ID 4065)
1167///
1168/// INS velocity covariance matrix in geodetic coordinates.
1169#[derive(Debug, Clone)]
1170#[allow(dead_code)]
1171pub struct IntVelCovGeodBlock {
1172    tow_ms: u32,
1173    wnc: u16,
1174    mode: u8,
1175    error: u8,
1176    cov_vn_vn: f32,
1177    cov_ve_ve: f32,
1178    cov_vu_vu: f32,
1179    cov_vn_ve: f32,
1180    cov_vn_vu: f32,
1181    cov_ve_vu: f32,
1182}
1183
1184impl IntVelCovGeodBlock {
1185    pub fn tow_seconds(&self) -> f64 {
1186        self.tow_ms as f64 * 0.001
1187    }
1188    pub fn tow_ms(&self) -> u32 {
1189        self.tow_ms
1190    }
1191    pub fn wnc(&self) -> u16 {
1192        self.wnc
1193    }
1194    pub fn mode(&self) -> PvtMode {
1195        PvtMode::from_mode_byte(self.mode)
1196    }
1197    pub fn mode_raw(&self) -> u8 {
1198        self.mode
1199    }
1200    pub fn error(&self) -> PvtError {
1201        PvtError::from_error_byte(self.error)
1202    }
1203
1204    pub fn cov_vn_vn(&self) -> Option<f32> {
1205        if self.cov_vn_vn == F32_DNU {
1206            None
1207        } else {
1208            Some(self.cov_vn_vn)
1209        }
1210    }
1211    pub fn cov_ve_ve(&self) -> Option<f32> {
1212        if self.cov_ve_ve == F32_DNU {
1213            None
1214        } else {
1215            Some(self.cov_ve_ve)
1216        }
1217    }
1218    pub fn cov_vu_vu(&self) -> Option<f32> {
1219        if self.cov_vu_vu == F32_DNU {
1220            None
1221        } else {
1222            Some(self.cov_vu_vu)
1223        }
1224    }
1225    pub fn cov_vn_ve(&self) -> Option<f32> {
1226        if self.cov_vn_ve == F32_DNU {
1227            None
1228        } else {
1229            Some(self.cov_vn_ve)
1230        }
1231    }
1232    pub fn cov_vn_vu(&self) -> Option<f32> {
1233        if self.cov_vn_vu == F32_DNU {
1234            None
1235        } else {
1236            Some(self.cov_vn_vu)
1237        }
1238    }
1239    pub fn cov_ve_vu(&self) -> Option<f32> {
1240        if self.cov_ve_vu == F32_DNU {
1241            None
1242        } else {
1243            Some(self.cov_ve_vu)
1244        }
1245    }
1246
1247    pub fn vn_std_mps(&self) -> Option<f32> {
1248        if self.cov_vn_vn == F32_DNU || self.cov_vn_vn < 0.0 {
1249            None
1250        } else {
1251            Some(self.cov_vn_vn.sqrt())
1252        }
1253    }
1254    pub fn ve_std_mps(&self) -> Option<f32> {
1255        if self.cov_ve_ve == F32_DNU || self.cov_ve_ve < 0.0 {
1256            None
1257        } else {
1258            Some(self.cov_ve_ve.sqrt())
1259        }
1260    }
1261    pub fn vu_std_mps(&self) -> Option<f32> {
1262        if self.cov_vu_vu == F32_DNU || self.cov_vu_vu < 0.0 {
1263            None
1264        } else {
1265            Some(self.cov_vu_vu.sqrt())
1266        }
1267    }
1268}
1269
1270impl SbfBlockParse for IntVelCovGeodBlock {
1271    const BLOCK_ID: u16 = block_ids::INT_VEL_COV_GEOD;
1272
1273    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
1274        const MIN_LEN: usize = 38;
1275        if data.len() < MIN_LEN {
1276            return Err(SbfError::ParseError("IntVelCovGeod too short".into()));
1277        }
1278
1279        let mode = data[12];
1280        let error = data[13];
1281        let cov_vn_vn = f32::from_le_bytes(data[14..18].try_into().unwrap());
1282        let cov_ve_ve = f32::from_le_bytes(data[18..22].try_into().unwrap());
1283        let cov_vu_vu = f32::from_le_bytes(data[22..26].try_into().unwrap());
1284        let cov_vn_ve = f32::from_le_bytes(data[26..30].try_into().unwrap());
1285        let cov_vn_vu = f32::from_le_bytes(data[30..34].try_into().unwrap());
1286        let cov_ve_vu = f32::from_le_bytes(data[34..38].try_into().unwrap());
1287
1288        Ok(Self {
1289            tow_ms: header.tow_ms,
1290            wnc: header.wnc,
1291            mode,
1292            error,
1293            cov_vn_vn,
1294            cov_ve_ve,
1295            cov_vu_vu,
1296            cov_vn_ve,
1297            cov_vn_vu,
1298            cov_ve_vu,
1299        })
1300    }
1301}
1302
1303// ============================================================================
1304// IntAttCovEuler Block
1305// ============================================================================
1306
1307/// IntAttCovEuler block (Block ID 4072)
1308///
1309/// INS attitude covariance matrix in Euler angles.
1310#[derive(Debug, Clone)]
1311#[allow(dead_code)]
1312pub struct IntAttCovEulerBlock {
1313    tow_ms: u32,
1314    wnc: u16,
1315    mode: u8,
1316    error: u8,
1317    cov_head_head: f32,
1318    cov_pitch_pitch: f32,
1319    cov_roll_roll: f32,
1320    cov_head_pitch: f32,
1321    cov_head_roll: f32,
1322    cov_pitch_roll: f32,
1323}
1324
1325impl IntAttCovEulerBlock {
1326    pub fn tow_seconds(&self) -> f64 {
1327        self.tow_ms as f64 * 0.001
1328    }
1329    pub fn tow_ms(&self) -> u32 {
1330        self.tow_ms
1331    }
1332    pub fn wnc(&self) -> u16 {
1333        self.wnc
1334    }
1335    pub fn mode(&self) -> PvtMode {
1336        PvtMode::from_mode_byte(self.mode)
1337    }
1338    pub fn mode_raw(&self) -> u8 {
1339        self.mode
1340    }
1341    pub fn error(&self) -> PvtError {
1342        PvtError::from_error_byte(self.error)
1343    }
1344
1345    pub fn cov_head_head(&self) -> Option<f32> {
1346        if self.cov_head_head == F32_DNU {
1347            None
1348        } else {
1349            Some(self.cov_head_head)
1350        }
1351    }
1352    pub fn cov_pitch_pitch(&self) -> Option<f32> {
1353        if self.cov_pitch_pitch == F32_DNU {
1354            None
1355        } else {
1356            Some(self.cov_pitch_pitch)
1357        }
1358    }
1359    pub fn cov_roll_roll(&self) -> Option<f32> {
1360        if self.cov_roll_roll == F32_DNU {
1361            None
1362        } else {
1363            Some(self.cov_roll_roll)
1364        }
1365    }
1366    pub fn cov_head_pitch(&self) -> Option<f32> {
1367        if self.cov_head_pitch == F32_DNU {
1368            None
1369        } else {
1370            Some(self.cov_head_pitch)
1371        }
1372    }
1373    pub fn cov_head_roll(&self) -> Option<f32> {
1374        if self.cov_head_roll == F32_DNU {
1375            None
1376        } else {
1377            Some(self.cov_head_roll)
1378        }
1379    }
1380    pub fn cov_pitch_roll(&self) -> Option<f32> {
1381        if self.cov_pitch_roll == F32_DNU {
1382            None
1383        } else {
1384            Some(self.cov_pitch_roll)
1385        }
1386    }
1387
1388    pub fn heading_std_deg(&self) -> Option<f32> {
1389        if self.cov_head_head == F32_DNU || self.cov_head_head < 0.0 {
1390            None
1391        } else {
1392            Some(self.cov_head_head.sqrt())
1393        }
1394    }
1395    pub fn pitch_std_deg(&self) -> Option<f32> {
1396        if self.cov_pitch_pitch == F32_DNU || self.cov_pitch_pitch < 0.0 {
1397            None
1398        } else {
1399            Some(self.cov_pitch_pitch.sqrt())
1400        }
1401    }
1402    pub fn roll_std_deg(&self) -> Option<f32> {
1403        if self.cov_roll_roll == F32_DNU || self.cov_roll_roll < 0.0 {
1404            None
1405        } else {
1406            Some(self.cov_roll_roll.sqrt())
1407        }
1408    }
1409}
1410
1411impl SbfBlockParse for IntAttCovEulerBlock {
1412    const BLOCK_ID: u16 = block_ids::INT_ATT_COV_EULER;
1413
1414    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
1415        const MIN_LEN: usize = 38;
1416        if data.len() < MIN_LEN {
1417            return Err(SbfError::ParseError("IntAttCovEuler too short".into()));
1418        }
1419
1420        let mode = data[12];
1421        let error = data[13];
1422        let cov_head_head = f32::from_le_bytes(data[14..18].try_into().unwrap());
1423        let cov_pitch_pitch = f32::from_le_bytes(data[18..22].try_into().unwrap());
1424        let cov_roll_roll = f32::from_le_bytes(data[22..26].try_into().unwrap());
1425        let cov_head_pitch = f32::from_le_bytes(data[26..30].try_into().unwrap());
1426        let cov_head_roll = f32::from_le_bytes(data[30..34].try_into().unwrap());
1427        let cov_pitch_roll = f32::from_le_bytes(data[34..38].try_into().unwrap());
1428
1429        Ok(Self {
1430            tow_ms: header.tow_ms,
1431            wnc: header.wnc,
1432            mode,
1433            error,
1434            cov_head_head,
1435            cov_pitch_pitch,
1436            cov_roll_roll,
1437            cov_head_pitch,
1438            cov_head_roll,
1439            cov_pitch_roll,
1440        })
1441    }
1442}
1443
1444impl SbfBlockParse for IntPosCovCartBlock {
1445    const BLOCK_ID: u16 = block_ids::INT_POS_COV_CART;
1446
1447    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
1448        const MIN_LEN: usize = 38;
1449        if data.len() < MIN_LEN {
1450            return Err(SbfError::ParseError("IntPosCovCart too short".into()));
1451        }
1452
1453        let mode = data[12];
1454        let error = data[13];
1455        let cov_xx = f32::from_le_bytes(data[14..18].try_into().unwrap());
1456        let cov_yy = f32::from_le_bytes(data[18..22].try_into().unwrap());
1457        let cov_zz = f32::from_le_bytes(data[22..26].try_into().unwrap());
1458        let cov_xy = f32::from_le_bytes(data[26..30].try_into().unwrap());
1459        let cov_xz = f32::from_le_bytes(data[30..34].try_into().unwrap());
1460        let cov_yz = f32::from_le_bytes(data[34..38].try_into().unwrap());
1461
1462        Ok(Self {
1463            tow_ms: header.tow_ms,
1464            wnc: header.wnc,
1465            mode,
1466            error,
1467            cov_xx,
1468            cov_yy,
1469            cov_zz,
1470            cov_xy,
1471            cov_xz,
1472            cov_yz,
1473        })
1474    }
1475}
1476
1477#[cfg(test)]
1478mod tests {
1479    use super::*;
1480    use crate::header::SbfHeader;
1481
1482    fn header_for(block_id: u16, tow_ms: u32, wnc: u16) -> SbfHeader {
1483        SbfHeader {
1484            crc: 0,
1485            block_id,
1486            block_rev: 0,
1487            length: 70,
1488            tow_ms,
1489            wnc,
1490        }
1491    }
1492
1493    #[test]
1494    fn test_int_pv_cart_parse() {
1495        let mut data = vec![0u8; 70];
1496        data[0..6].copy_from_slice(&[0, 0, 0, 0, 0, 0]);
1497        data[6..10].copy_from_slice(&1000u32.to_le_bytes());
1498        data[10..12].copy_from_slice(&2000u16.to_le_bytes());
1499        data[12] = 4; // Mode: RTK Fixed
1500        data[13] = 0; // Error: none
1501        data[14..16].copy_from_slice(&0u16.to_le_bytes());
1502        data[16] = 12; // NrSV
1503        data[17] = 1; // NrAnt
1504        data[18] = 4;
1505        data[19] = 0;
1506        data[20..22].copy_from_slice(&100u16.to_le_bytes()); // GNSSage 1.0s
1507                                                             // X, Y, Z - use valid ECEF values (not DNU)
1508        data[22..30].copy_from_slice(&1234567.0f64.to_le_bytes());
1509        data[30..38].copy_from_slice(&2345678.0f64.to_le_bytes());
1510        data[38..46].copy_from_slice(&3456789.0f64.to_le_bytes());
1511        data[46..50].copy_from_slice(&0.1f32.to_le_bytes());
1512        data[50..54].copy_from_slice(&0.2f32.to_le_bytes());
1513        data[54..58].copy_from_slice(&0.3f32.to_le_bytes());
1514        data[58..62].copy_from_slice(&45.0f32.to_le_bytes());
1515
1516        let header = header_for(block_ids::INT_PV_CART, 1000, 2000);
1517        let block = IntPvCartBlock::parse(&header, &data).unwrap();
1518        assert_eq!(block.tow_seconds(), 1.0);
1519        assert_eq!(block.wnc(), 2000);
1520        assert_eq!(block.nr_sv(), 12);
1521        assert_eq!(block.gnss_age_seconds(), Some(1.0));
1522    }
1523
1524    #[test]
1525    fn test_int_pv_cart_dnu() {
1526        let mut data = vec![0u8; 70];
1527        data[6..10].copy_from_slice(&1000u32.to_le_bytes());
1528        data[10..12].copy_from_slice(&2000u16.to_le_bytes());
1529        data[20..22].copy_from_slice(&U16_DNU.to_le_bytes());
1530        data[22..30].copy_from_slice(&F64_DNU.to_le_bytes());
1531        data[46..50].copy_from_slice(&F32_DNU.to_le_bytes());
1532
1533        let header = header_for(block_ids::INT_PV_CART, 1000, 2000);
1534        let block = IntPvCartBlock::parse(&header, &data).unwrap();
1535        assert!(block.gnss_age_seconds().is_none());
1536        assert!(block.x_m().is_none());
1537        assert!(block.velocity_x_mps().is_none());
1538    }
1539
1540    #[test]
1541    fn test_int_pv_geod_parse() {
1542        let mut data = vec![0u8; 70];
1543        data[6..10].copy_from_slice(&2000u32.to_le_bytes());
1544        data[10..12].copy_from_slice(&2100u16.to_le_bytes());
1545        data[12] = 4;
1546        data[13] = 0;
1547        data[14..16].copy_from_slice(&0u16.to_le_bytes());
1548        data[16] = 10;
1549        data[17] = 1;
1550        data[18] = 4;
1551        data[19] = 0;
1552        data[20..22].copy_from_slice(&50u16.to_le_bytes());
1553        let lat_rad = 0.5f64;
1554        let long_rad = 0.3f64;
1555        data[22..30].copy_from_slice(&lat_rad.to_le_bytes());
1556        data[30..38].copy_from_slice(&long_rad.to_le_bytes());
1557        data[38..46].copy_from_slice(&100.0f64.to_le_bytes());
1558        data[46..50].copy_from_slice(&0.5f32.to_le_bytes());
1559        data[50..54].copy_from_slice(&0.2f32.to_le_bytes());
1560        data[54..58].copy_from_slice(&0.1f32.to_le_bytes());
1561        data[58..62].copy_from_slice(&90.0f32.to_le_bytes());
1562
1563        let header = header_for(block_ids::INT_PV_GEOD, 2000, 2100);
1564        let block = IntPvGeodBlock::parse(&header, &data).unwrap();
1565        assert_eq!(block.tow_seconds(), 2.0);
1566        assert_eq!(block.wnc(), 2100);
1567        assert_eq!(block.nr_sv(), 10);
1568        assert_eq!(block.gnss_age_seconds(), Some(0.5));
1569        assert!((block.latitude_deg().unwrap() - lat_rad.to_degrees()).abs() < 1e-6);
1570        assert!((block.longitude_deg().unwrap() - long_rad.to_degrees()).abs() < 1e-6);
1571        assert_eq!(block.altitude_m(), Some(100.0));
1572        assert_eq!(block.course_over_ground_deg(), Some(90.0));
1573    }
1574
1575    #[test]
1576    fn test_int_pvaa_geod_parse() {
1577        let mut data = vec![0u8; 72];
1578        data[6..10].copy_from_slice(&3000u32.to_le_bytes());
1579        data[10..12].copy_from_slice(&2200u16.to_le_bytes());
1580        data[12] = 4; // mode
1581        data[13] = 0; // error
1582        data[14..16].copy_from_slice(&0u16.to_le_bytes());
1583        data[16] = 4;
1584        data[17] = 0;
1585        data[18] = 5; // GNSSage 0.5s
1586        data[19] = 0x81; // NrSV=8, NrAnt=1
1587        data[20] = 0; // PosFine
1588                      // Lat 1e-7 deg: -33.87° = -338700000
1589        data[21..25].copy_from_slice(&(-338700000i32).to_le_bytes());
1590        // Long 1e-7 deg: 151.21° = 1512100000
1591        data[25..29].copy_from_slice(&1512100000i32.to_le_bytes());
1592        // Alt 1e-3 m: 50.5m = 50500
1593        data[29..33].copy_from_slice(&50500i32.to_le_bytes());
1594        // Vn, Ve, Vu 1e-3 m/s: 1.5, 0.2, 0.1
1595        data[33..37].copy_from_slice(&1500i32.to_le_bytes());
1596        data[37..41].copy_from_slice(&200i32.to_le_bytes());
1597        data[41..45].copy_from_slice(&100i32.to_le_bytes());
1598        // Ax, Ay, Az 0.01 m/s²: 0.1, 0, 9.8
1599        data[45..47].copy_from_slice(&10i16.to_le_bytes());
1600        data[47..49].copy_from_slice(&0i16.to_le_bytes());
1601        data[49..51].copy_from_slice(&980i16.to_le_bytes());
1602        // Heading 0.01 deg: 45° = 4500
1603        data[51..53].copy_from_slice(&4500u16.to_le_bytes());
1604        // Pitch, Roll 0.01 deg: 2°, -1°
1605        data[53..55].copy_from_slice(&200i16.to_le_bytes());
1606        data[55..57].copy_from_slice(&(-100i16).to_le_bytes());
1607
1608        let header = SbfHeader {
1609            crc: 0,
1610            block_id: block_ids::INT_PVA_AGEOD,
1611            block_rev: 0,
1612            length: 72,
1613            tow_ms: 3000,
1614            wnc: 2200,
1615        };
1616        let block = IntPvaaGeodBlock::parse(&header, &data).unwrap();
1617        assert_eq!(block.tow_seconds(), 3.0);
1618        assert_eq!(block.wnc(), 2200);
1619        assert_eq!(block.nr_sv(), 8);
1620        assert_eq!(block.nr_ant(), 1);
1621        assert_eq!(block.gnss_age_seconds(), Some(0.5));
1622        assert!((block.latitude_deg().unwrap() - (-33.87)).abs() < 1e-6);
1623        assert!((block.longitude_deg().unwrap() - 151.21).abs() < 1e-6);
1624        assert!((block.altitude_m().unwrap() - 50.5).abs() < 1e-6);
1625        assert!((block.velocity_north_mps().unwrap() - 1.5).abs() < 1e-6);
1626        assert!((block.velocity_east_mps().unwrap() - 0.2).abs() < 1e-6);
1627        assert!((block.velocity_up_mps().unwrap() - 0.1).abs() < 1e-6);
1628        assert!((block.acceleration_z_mps2().unwrap() - 9.8).abs() < 0.01);
1629        assert!((block.heading_deg().unwrap() - 45.0).abs() < 1e-6);
1630        assert!((block.pitch_deg().unwrap() - 2.0).abs() < 1e-6);
1631        assert!((block.roll_deg().unwrap() - (-1.0)).abs() < 1e-6);
1632    }
1633
1634    #[test]
1635    fn test_int_pvaa_geod_dnu() {
1636        let mut data = vec![0u8; 72];
1637        data[6..10].copy_from_slice(&1000u32.to_le_bytes());
1638        data[10..12].copy_from_slice(&2000u16.to_le_bytes());
1639        data[18] = 255; // GNSSage DNU
1640        data[21..25].copy_from_slice(&I32_DNU.to_le_bytes());
1641        data[45..47].copy_from_slice(&I16_DNU.to_le_bytes());
1642        data[51..53].copy_from_slice(&U16_DNU.to_le_bytes());
1643
1644        let header = SbfHeader {
1645            crc: 0,
1646            block_id: block_ids::INT_PVA_AGEOD,
1647            block_rev: 0,
1648            length: 72,
1649            tow_ms: 1000,
1650            wnc: 2000,
1651        };
1652        let block = IntPvaaGeodBlock::parse(&header, &data).unwrap();
1653        assert!(block.gnss_age_seconds().is_none());
1654        assert!(block.latitude_deg().is_none());
1655        assert!(block.acceleration_x_mps2().is_none());
1656        assert!(block.heading_deg().is_none());
1657    }
1658
1659    #[test]
1660    fn test_int_pv_geod_dnu() {
1661        let mut data = vec![0u8; 70];
1662        data[6..10].copy_from_slice(&2000u32.to_le_bytes());
1663        data[10..12].copy_from_slice(&2100u16.to_le_bytes());
1664        data[20..22].copy_from_slice(&U16_DNU.to_le_bytes());
1665        data[22..30].copy_from_slice(&F64_DNU.to_le_bytes());
1666        data[46..50].copy_from_slice(&F32_DNU.to_le_bytes());
1667
1668        let header = header_for(block_ids::INT_PV_GEOD, 2000, 2100);
1669        let block = IntPvGeodBlock::parse(&header, &data).unwrap();
1670        assert!(block.gnss_age_seconds().is_none());
1671        assert!(block.latitude_deg().is_none());
1672        assert!(block.velocity_north_mps().is_none());
1673    }
1674
1675    #[test]
1676    fn test_int_att_euler_parse() {
1677        let mut data = vec![0u8; 60];
1678        data[6..10].copy_from_slice(&3000u32.to_le_bytes());
1679        data[10..12].copy_from_slice(&2200u16.to_le_bytes());
1680        data[12] = 4;
1681        data[13] = 0;
1682        data[14..16].copy_from_slice(&0u16.to_le_bytes());
1683        data[16] = 8;
1684        data[17] = 1;
1685        data[18] = 0;
1686        data[19..21].copy_from_slice(&25u16.to_le_bytes());
1687        data[21..25].copy_from_slice(&180.0f32.to_le_bytes());
1688        data[25..29].copy_from_slice(&5.0f32.to_le_bytes());
1689        data[29..33].copy_from_slice(&(-2.0f32).to_le_bytes());
1690        data[33..37].copy_from_slice(&0.1f32.to_le_bytes());
1691        data[37..41].copy_from_slice(&0.05f32.to_le_bytes());
1692        data[41..45].copy_from_slice(&1.5f32.to_le_bytes());
1693
1694        let header = header_for(block_ids::INT_ATT_EULER, 3000, 2200);
1695        let block = IntAttEulerBlock::parse(&header, &data).unwrap();
1696        assert_eq!(block.tow_seconds(), 3.0);
1697        assert_eq!(block.nr_sv(), 8);
1698        assert_eq!(block.gnss_age_raw(), 25);
1699        assert_eq!(block.gnss_age_seconds(), Some(0.25));
1700        assert_eq!(block.heading_deg(), Some(180.0));
1701        assert_eq!(block.pitch_deg(), Some(5.0));
1702        assert_eq!(block.roll_deg(), Some(-2.0));
1703        assert_eq!(block.heading_rate_dps(), Some(1.5));
1704    }
1705
1706    #[test]
1707    fn test_int_att_euler_dnu() {
1708        let mut data = vec![0u8; 60];
1709        data[6..10].copy_from_slice(&3000u32.to_le_bytes());
1710        data[10..12].copy_from_slice(&2200u16.to_le_bytes());
1711        data[19..21].copy_from_slice(&U16_DNU.to_le_bytes());
1712        data[21..25].copy_from_slice(&F32_DNU.to_le_bytes());
1713
1714        let header = header_for(block_ids::INT_ATT_EULER, 3000, 2200);
1715        let block = IntAttEulerBlock::parse(&header, &data).unwrap();
1716        assert_eq!(block.gnss_age_raw(), U16_DNU);
1717        assert!(block.gnss_age_seconds().is_none());
1718        assert!(block.heading_deg().is_none());
1719    }
1720
1721    #[test]
1722    fn test_int_pos_cov_cart_parse() {
1723        let mut data = vec![0u8; 50];
1724        data[6..10].copy_from_slice(&4000u32.to_le_bytes());
1725        data[10..12].copy_from_slice(&2300u16.to_le_bytes());
1726        data[12] = 4;
1727        data[13] = 0;
1728        data[14..18].copy_from_slice(&1.0f32.to_le_bytes());
1729        data[18..22].copy_from_slice(&2.0f32.to_le_bytes());
1730        data[22..26].copy_from_slice(&3.0f32.to_le_bytes());
1731        data[26..30].copy_from_slice(&0.1f32.to_le_bytes());
1732        data[30..34].copy_from_slice(&0.2f32.to_le_bytes());
1733        data[34..38].copy_from_slice(&0.3f32.to_le_bytes());
1734
1735        let header = header_for(block_ids::INT_POS_COV_CART, 4000, 2300);
1736        let block = IntPosCovCartBlock::parse(&header, &data).unwrap();
1737        assert_eq!(block.tow_seconds(), 4.0);
1738        assert_eq!(block.cov_xx(), Some(1.0));
1739        assert_eq!(block.cov_yy(), Some(2.0));
1740        assert_eq!(block.x_std_m(), Some(1.0));
1741        assert!((block.y_std_m().unwrap() - 2.0_f32.sqrt()).abs() < 1e-5);
1742    }
1743
1744    #[test]
1745    fn test_int_pos_cov_cart_dnu() {
1746        let mut data = vec![0u8; 50];
1747        data[6..10].copy_from_slice(&4000u32.to_le_bytes());
1748        data[10..12].copy_from_slice(&2300u16.to_le_bytes());
1749        data[14..18].copy_from_slice(&F32_DNU.to_le_bytes());
1750
1751        let header = header_for(block_ids::INT_POS_COV_CART, 4000, 2300);
1752        let block = IntPosCovCartBlock::parse(&header, &data).unwrap();
1753        assert!(block.cov_xx().is_none());
1754        assert!(block.x_std_m().is_none());
1755    }
1756
1757    #[test]
1758    fn test_int_vel_cov_cart_parse() {
1759        let mut data = vec![0u8; 50];
1760        data[6..10].copy_from_slice(&4100u32.to_le_bytes());
1761        data[10..12].copy_from_slice(&2300u16.to_le_bytes());
1762        data[12] = 4;
1763        data[13] = 0;
1764        data[14..18].copy_from_slice(&0.01f32.to_le_bytes());
1765        data[18..22].copy_from_slice(&0.02f32.to_le_bytes());
1766        data[22..26].copy_from_slice(&0.03f32.to_le_bytes());
1767        data[26..30].copy_from_slice(&0.001f32.to_le_bytes());
1768        data[30..34].copy_from_slice(&0.002f32.to_le_bytes());
1769        data[34..38].copy_from_slice(&0.003f32.to_le_bytes());
1770
1771        let header = header_for(block_ids::INT_VEL_COV_CART, 4100, 2300);
1772        let block = IntVelCovCartBlock::parse(&header, &data).unwrap();
1773        assert_eq!(block.tow_seconds(), 4.1);
1774        assert_eq!(block.cov_vx_vx(), Some(0.01));
1775        assert_eq!(block.vx_std_mps(), Some(0.1));
1776    }
1777
1778    #[test]
1779    fn test_int_vel_cov_cart_dnu() {
1780        let mut data = vec![0u8; 50];
1781        data[6..10].copy_from_slice(&4100u32.to_le_bytes());
1782        data[10..12].copy_from_slice(&2300u16.to_le_bytes());
1783        data[14..18].copy_from_slice(&F32_DNU.to_le_bytes());
1784
1785        let header = header_for(block_ids::INT_VEL_COV_CART, 4100, 2300);
1786        let block = IntVelCovCartBlock::parse(&header, &data).unwrap();
1787        assert!(block.cov_vx_vx().is_none());
1788        assert!(block.vx_std_mps().is_none());
1789    }
1790
1791    #[test]
1792    fn test_int_pos_cov_geod_parse() {
1793        let mut data = vec![0u8; 50];
1794        data[6..10].copy_from_slice(&4200u32.to_le_bytes());
1795        data[10..12].copy_from_slice(&2300u16.to_le_bytes());
1796        data[12] = 4;
1797        data[13] = 0;
1798        data[14..18].copy_from_slice(&1.0f32.to_le_bytes());
1799        data[18..22].copy_from_slice(&2.0f32.to_le_bytes());
1800        data[22..26].copy_from_slice(&3.0f32.to_le_bytes());
1801        data[26..30].copy_from_slice(&0.1f32.to_le_bytes());
1802        data[30..34].copy_from_slice(&0.2f32.to_le_bytes());
1803        data[34..38].copy_from_slice(&0.3f32.to_le_bytes());
1804
1805        let header = header_for(block_ids::INT_POS_COV_GEOD, 4200, 2300);
1806        let block = IntPosCovGeodBlock::parse(&header, &data).unwrap();
1807        assert_eq!(block.tow_seconds(), 4.2);
1808        assert_eq!(block.cov_lat_lat(), Some(1.0));
1809        assert_eq!(block.lat_std_m(), Some(1.0));
1810    }
1811
1812    #[test]
1813    fn test_int_vel_cov_geod_parse() {
1814        let mut data = vec![0u8; 50];
1815        data[6..10].copy_from_slice(&4300u32.to_le_bytes());
1816        data[10..12].copy_from_slice(&2300u16.to_le_bytes());
1817        data[12] = 4;
1818        data[13] = 0;
1819        data[14..18].copy_from_slice(&0.04f32.to_le_bytes());
1820        data[18..22].copy_from_slice(&0.09f32.to_le_bytes());
1821        data[22..26].copy_from_slice(&0.16f32.to_le_bytes());
1822        data[26..30].copy_from_slice(&0.01f32.to_le_bytes());
1823        data[30..34].copy_from_slice(&0.02f32.to_le_bytes());
1824        data[34..38].copy_from_slice(&0.03f32.to_le_bytes());
1825
1826        let header = header_for(block_ids::INT_VEL_COV_GEOD, 4300, 2300);
1827        let block = IntVelCovGeodBlock::parse(&header, &data).unwrap();
1828        assert_eq!(block.tow_seconds(), 4.3);
1829        assert_eq!(block.cov_vn_vn(), Some(0.04));
1830        assert_eq!(block.vn_std_mps(), Some(0.2));
1831    }
1832
1833    #[test]
1834    fn test_int_att_cov_euler_parse() {
1835        let mut data = vec![0u8; 50];
1836        data[6..10].copy_from_slice(&4400u32.to_le_bytes());
1837        data[10..12].copy_from_slice(&2300u16.to_le_bytes());
1838        data[12] = 4;
1839        data[13] = 0;
1840        data[14..18].copy_from_slice(&4.0f32.to_le_bytes());
1841        data[18..22].copy_from_slice(&1.0f32.to_le_bytes());
1842        data[22..26].copy_from_slice(&1.0f32.to_le_bytes());
1843        data[26..30].copy_from_slice(&0.5f32.to_le_bytes());
1844        data[30..34].copy_from_slice(&0.5f32.to_le_bytes());
1845        data[34..38].copy_from_slice(&0.25f32.to_le_bytes());
1846
1847        let header = header_for(block_ids::INT_ATT_COV_EULER, 4400, 2300);
1848        let block = IntAttCovEulerBlock::parse(&header, &data).unwrap();
1849        assert_eq!(block.tow_seconds(), 4.4);
1850        assert_eq!(block.cov_head_head(), Some(4.0));
1851        assert_eq!(block.heading_std_deg(), Some(2.0));
1852    }
1853
1854    #[test]
1855    fn test_int_att_cov_euler_dnu() {
1856        let mut data = vec![0u8; 50];
1857        data[6..10].copy_from_slice(&4400u32.to_le_bytes());
1858        data[10..12].copy_from_slice(&2300u16.to_le_bytes());
1859        data[14..18].copy_from_slice(&F32_DNU.to_le_bytes());
1860
1861        let header = header_for(block_ids::INT_ATT_COV_EULER, 4400, 2300);
1862        let block = IntAttCovEulerBlock::parse(&header, &data).unwrap();
1863        assert!(block.cov_head_head().is_none());
1864        assert!(block.heading_std_deg().is_none());
1865    }
1866}
sbf_tools/blocks/ins.rs

sbf_tools/blocks/
ins.rs
