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

sbf_tools/blocks/
ins.rs
