sbf_tools/blocks/
extended.rs

1//! Additional SBF blocks: extra raw navigation pages, BeiDou/QZSS decoded nav, local/projected position.
2//!
3//! Layouts follow Septentrio SBF definitions (public reference headers such as PointOneNav `sbfdef.h`).
4
5use crate::error::{SbfError, SbfResult};
6use crate::header::SbfHeader;
7
8use super::block_ids;
9use super::dnu::{f32_or_none, f64_or_none};
10use super::navigation::GpsNavBlock;
11use super::SbfBlockParse;
12
13// --- Raw navigation (RxChannel + u32 NAVBits array) ---------------------------------
14
15/// GEORawL5 (4021) — SBAS L5 navigation message.
16#[derive(Debug, Clone)]
17pub struct GeoRawL5Block {
18    tow_ms: u32,
19    wnc: u16,
20    pub svid: u8,
21    pub crc_status: u8,
22    pub viterbi_count: u8,
23    pub source: u8,
24    pub freq_nr: u8,
25    pub rx_channel: u8,
26    /// Raw `NAVBits` payload (`16` × `u32` = 64 bytes).
27    pub nav_bits: [u8; 64],
28}
29
30impl GeoRawL5Block {
31    pub fn tow_ms(&self) -> u32 {
32        self.tow_ms
33    }
34    pub fn wnc(&self) -> u16 {
35        self.wnc
36    }
37    pub fn tow_seconds(&self) -> f64 {
38        self.tow_ms as f64 * 0.001
39    }
40    pub fn crc_ok(&self) -> bool {
41        self.crc_status != 0
42    }
43}
44
45impl SbfBlockParse for GeoRawL5Block {
46    const BLOCK_ID: u16 = block_ids::GEO_RAW_L5;
47
48    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
49        const MIN: usize = 82;
50        if data.len() < MIN {
51            return Err(SbfError::ParseError("GEORawL5 too short".into()));
52        }
53        let mut nav_bits = [0u8; 64];
54        nav_bits.copy_from_slice(&data[18..82]);
55        Ok(Self {
56            tow_ms: header.tow_ms,
57            wnc: header.wnc,
58            svid: data[12],
59            crc_status: data[13],
60            viterbi_count: data[14],
61            source: data[15],
62            freq_nr: data[16],
63            rx_channel: data[17],
64            nav_bits,
65        })
66    }
67}
68
69/// BDSRawB1C (4218) — BeiDou B1C navigation frame.
70#[derive(Debug, Clone)]
71pub struct BdsRawB1cBlock {
72    tow_ms: u32,
73    wnc: u16,
74    pub svid: u8,
75    pub crc_sf2: u8,
76    pub crc_sf3: u8,
77    pub source: u8,
78    pub reserved: u8,
79    pub rx_channel: u8,
80    /// `57` × `u32` = 228 bytes.
81    pub nav_bits: [u8; 228],
82}
83
84impl BdsRawB1cBlock {
85    pub fn tow_ms(&self) -> u32 {
86        self.tow_ms
87    }
88    pub fn wnc(&self) -> u16 {
89        self.wnc
90    }
91    pub fn tow_seconds(&self) -> f64 {
92        self.tow_ms as f64 * 0.001
93    }
94}
95
96impl SbfBlockParse for BdsRawB1cBlock {
97    const BLOCK_ID: u16 = block_ids::BDS_RAW_B1C;
98
99    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
100        const MIN: usize = 246;
101        if data.len() < MIN {
102            return Err(SbfError::ParseError("BDSRawB1C too short".into()));
103        }
104        let mut nav_bits = [0u8; 228];
105        nav_bits.copy_from_slice(&data[18..246]);
106        Ok(Self {
107            tow_ms: header.tow_ms,
108            wnc: header.wnc,
109            svid: data[12],
110            crc_sf2: data[13],
111            crc_sf3: data[14],
112            source: data[15],
113            reserved: data[16],
114            rx_channel: data[17],
115            nav_bits,
116        })
117    }
118}
119
120/// BDSRawB2a (4219) — BeiDou B2a navigation frame.
121#[derive(Debug, Clone)]
122pub struct BdsRawB2aBlock {
123    tow_ms: u32,
124    wnc: u16,
125    pub svid: u8,
126    pub crc_passed: u8,
127    pub viterbi_count: u8,
128    pub source: u8,
129    pub reserved: u8,
130    pub rx_channel: u8,
131    /// `18` × `u32` = 72 bytes.
132    pub nav_bits: [u8; 72],
133}
134
135impl BdsRawB2aBlock {
136    pub fn tow_ms(&self) -> u32 {
137        self.tow_ms
138    }
139    pub fn wnc(&self) -> u16 {
140        self.wnc
141    }
142    pub fn tow_seconds(&self) -> f64 {
143        self.tow_ms as f64 * 0.001
144    }
145    pub fn crc_ok(&self) -> bool {
146        self.crc_passed != 0
147    }
148}
149
150impl SbfBlockParse for BdsRawB2aBlock {
151    const BLOCK_ID: u16 = block_ids::BDS_RAW_B2A;
152
153    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
154        const MIN: usize = 90;
155        if data.len() < MIN {
156            return Err(SbfError::ParseError("BDSRawB2a too short".into()));
157        }
158        let mut nav_bits = [0u8; 72];
159        nav_bits.copy_from_slice(&data[18..90]);
160        Ok(Self {
161            tow_ms: header.tow_ms,
162            wnc: header.wnc,
163            svid: data[12],
164            crc_passed: data[13],
165            viterbi_count: data[14],
166            source: data[15],
167            reserved: data[16],
168            rx_channel: data[17],
169            nav_bits,
170        })
171    }
172}
173
174/// BDSRawB2b (4242) — BeiDou B2b navigation frame.
175#[derive(Debug, Clone)]
176pub struct BdsRawB2bBlock {
177    tow_ms: u32,
178    wnc: u16,
179    pub svid: u8,
180    pub crc_passed: u8,
181    pub reserved1: u8,
182    pub source: u8,
183    pub reserved2: u8,
184    pub rx_channel: u8,
185    /// `31` × `u32` = 124 bytes.
186    pub nav_bits: [u8; 124],
187}
188
189impl BdsRawB2bBlock {
190    pub fn tow_ms(&self) -> u32 {
191        self.tow_ms
192    }
193    pub fn wnc(&self) -> u16 {
194        self.wnc
195    }
196    pub fn tow_seconds(&self) -> f64 {
197        self.tow_ms as f64 * 0.001
198    }
199    pub fn crc_ok(&self) -> bool {
200        self.crc_passed != 0
201    }
202}
203
204impl SbfBlockParse for BdsRawB2bBlock {
205    const BLOCK_ID: u16 = block_ids::BDS_RAW_B2B;
206
207    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
208        const MIN: usize = 142;
209        if data.len() < MIN {
210            return Err(SbfError::ParseError("BDSRawB2b too short".into()));
211        }
212        let mut nav_bits = [0u8; 124];
213        nav_bits.copy_from_slice(&data[18..142]);
214        Ok(Self {
215            tow_ms: header.tow_ms,
216            wnc: header.wnc,
217            svid: data[12],
218            crc_passed: data[13],
219            reserved1: data[14],
220            source: data[15],
221            reserved2: data[16],
222            rx_channel: data[17],
223            nav_bits,
224        })
225    }
226}
227
228/// IRNSSRaw / NAVICRaw (4093) — NavIC/IRNSS subframe.
229#[derive(Debug, Clone)]
230pub struct IrnssRawBlock {
231    tow_ms: u32,
232    wnc: u16,
233    pub svid: u8,
234    pub crc_passed: u8,
235    pub viterbi_count: u8,
236    pub source: u8,
237    pub reserved: u8,
238    pub rx_channel: u8,
239    /// `10` × `u32` = 40 bytes.
240    pub nav_bits: [u8; 40],
241}
242
243impl IrnssRawBlock {
244    pub fn tow_ms(&self) -> u32 {
245        self.tow_ms
246    }
247    pub fn wnc(&self) -> u16 {
248        self.wnc
249    }
250    pub fn tow_seconds(&self) -> f64 {
251        self.tow_ms as f64 * 0.001
252    }
253    pub fn crc_ok(&self) -> bool {
254        self.crc_passed != 0
255    }
256}
257
258impl SbfBlockParse for IrnssRawBlock {
259    const BLOCK_ID: u16 = block_ids::NAVIC_RAW;
260
261    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
262        const MIN: usize = 58;
263        if data.len() < MIN {
264            return Err(SbfError::ParseError("IRNSSRaw too short".into()));
265        }
266        let mut nav_bits = [0u8; 40];
267        nav_bits.copy_from_slice(&data[18..58]);
268        Ok(Self {
269            tow_ms: header.tow_ms,
270            wnc: header.wnc,
271            svid: data[12],
272            crc_passed: data[13],
273            viterbi_count: data[14],
274            source: data[15],
275            reserved: data[16],
276            rx_channel: data[17],
277            nav_bits,
278        })
279    }
280}
281
282// --- Position --------------------------------------------------------------------
283
284/// PosLocal (4052) — position in a local datum.
285#[derive(Debug, Clone)]
286pub struct PosLocalBlock {
287    tow_ms: u32,
288    wnc: u16,
289    pub mode: u8,
290    pub error: u8,
291    pub latitude_rad: f64,
292    pub longitude_rad: f64,
293    pub height_m: f64,
294    pub datum: u8,
295}
296
297impl PosLocalBlock {
298    pub fn tow_ms(&self) -> u32 {
299        self.tow_ms
300    }
301    pub fn wnc(&self) -> u16 {
302        self.wnc
303    }
304    pub fn tow_seconds(&self) -> f64 {
305        self.tow_ms as f64 * 0.001
306    }
307}
308
309impl SbfBlockParse for PosLocalBlock {
310    const BLOCK_ID: u16 = block_ids::POS_LOCAL;
311
312    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
313        const MIN: usize = 42;
314        if data.len() < MIN {
315            return Err(SbfError::ParseError("PosLocal too short".into()));
316        }
317        Ok(Self {
318            tow_ms: header.tow_ms,
319            wnc: header.wnc,
320            mode: data[12],
321            error: data[13],
322            latitude_rad: f64::from_le_bytes(data[14..22].try_into().unwrap()),
323            longitude_rad: f64::from_le_bytes(data[22..30].try_into().unwrap()),
324            height_m: f64::from_le_bytes(data[30..38].try_into().unwrap()),
325            datum: data[38],
326        })
327    }
328}
329
330/// PosProjected (4094) — plane grid coordinates.
331#[derive(Debug, Clone)]
332pub struct PosProjectedBlock {
333    tow_ms: u32,
334    wnc: u16,
335    pub mode: u8,
336    pub error: u8,
337    pub northing_m: f64,
338    pub easting_m: f64,
339    pub height_m: f64,
340    pub datum: u8,
341}
342
343impl PosProjectedBlock {
344    pub fn tow_ms(&self) -> u32 {
345        self.tow_ms
346    }
347    pub fn wnc(&self) -> u16 {
348        self.wnc
349    }
350    pub fn tow_seconds(&self) -> f64 {
351        self.tow_ms as f64 * 0.001
352    }
353}
354
355impl SbfBlockParse for PosProjectedBlock {
356    const BLOCK_ID: u16 = block_ids::POS_PROJECTED;
357
358    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
359        const MIN: usize = 42;
360        if data.len() < MIN {
361            return Err(SbfError::ParseError("PosProjected too short".into()));
362        }
363        Ok(Self {
364            tow_ms: header.tow_ms,
365            wnc: header.wnc,
366            mode: data[12],
367            error: data[13],
368            northing_m: f64::from_le_bytes(data[14..22].try_into().unwrap()),
369            easting_m: f64::from_le_bytes(data[22..30].try_into().unwrap()),
370            height_m: f64::from_le_bytes(data[30..38].try_into().unwrap()),
371            datum: data[38],
372        })
373    }
374}
375
376// --- Decoded BeiDou ephemeris (cmpEph) --------------------------------------------
377
378/// BDSNav (4081) — BeiDou ephemeris and clock (`cmpEph`).
379#[derive(Debug, Clone)]
380pub struct BdsNavBlock {
381    tow_ms: u32,
382    wnc: u16,
383    pub prn: u8,
384    pub wn: u16,
385    pub ura: u8,
386    pub sat_h1: u8,
387    pub iodc: u8,
388    pub iode: u8,
389    pub t_gd1_s: f32,
390    pub t_gd2_s: f32,
391    pub t_oc: u32,
392    pub a_f2: f32,
393    pub a_f1: f32,
394    pub a_f0: f32,
395    pub c_rs: f32,
396    pub delta_n: f32,
397    pub m_0: f64,
398    pub c_uc: f32,
399    pub e: f64,
400    pub c_us: f32,
401    pub sqrt_a: f64,
402    pub t_oe: u32,
403    pub c_ic: f32,
404    pub omega_0: f64,
405    pub c_is: f32,
406    pub i_0: f64,
407    pub c_rc: f32,
408    pub omega: f64,
409    pub omega_dot: f32,
410    pub i_dot: f32,
411    pub wn_t_oc: u16,
412    pub wn_t_oe: u16,
413}
414
415impl BdsNavBlock {
416    pub fn tow_ms(&self) -> u32 {
417        self.tow_ms
418    }
419    pub fn wnc(&self) -> u16 {
420        self.wnc
421    }
422    pub fn tow_seconds(&self) -> f64 {
423        self.tow_ms as f64 * 0.001
424    }
425    pub fn t_gd1_s_opt(&self) -> Option<f32> {
426        f32_or_none(self.t_gd1_s)
427    }
428    pub fn t_gd2_s_opt(&self) -> Option<f32> {
429        f32_or_none(self.t_gd2_s)
430    }
431}
432
433impl SbfBlockParse for BdsNavBlock {
434    const BLOCK_ID: u16 = block_ids::BDS_NAV;
435
436    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
437        if data.len() < 138 {
438            return Err(SbfError::ParseError("BDSNav too short".into()));
439        }
440        Ok(Self {
441            tow_ms: header.tow_ms,
442            wnc: header.wnc,
443            prn: data[12],
444            wn: u16::from_le_bytes([data[14], data[15]]),
445            ura: data[16],
446            sat_h1: data[17],
447            iodc: data[18],
448            iode: data[19],
449            t_gd1_s: f32::from_le_bytes(data[22..26].try_into().unwrap()),
450            t_gd2_s: f32::from_le_bytes(data[26..30].try_into().unwrap()),
451            t_oc: u32::from_le_bytes(data[30..34].try_into().unwrap()),
452            a_f2: f32::from_le_bytes(data[34..38].try_into().unwrap()),
453            a_f1: f32::from_le_bytes(data[38..42].try_into().unwrap()),
454            a_f0: f32::from_le_bytes(data[42..46].try_into().unwrap()),
455            c_rs: f32::from_le_bytes(data[46..50].try_into().unwrap()),
456            delta_n: f32::from_le_bytes(data[50..54].try_into().unwrap()),
457            m_0: f64::from_le_bytes(data[54..62].try_into().unwrap()),
458            c_uc: f32::from_le_bytes(data[62..66].try_into().unwrap()),
459            e: f64::from_le_bytes(data[66..74].try_into().unwrap()),
460            c_us: f32::from_le_bytes(data[74..78].try_into().unwrap()),
461            sqrt_a: f64::from_le_bytes(data[78..86].try_into().unwrap()),
462            t_oe: u32::from_le_bytes(data[86..90].try_into().unwrap()),
463            c_ic: f32::from_le_bytes(data[90..94].try_into().unwrap()),
464            omega_0: f64::from_le_bytes(data[94..102].try_into().unwrap()),
465            c_is: f32::from_le_bytes(data[102..106].try_into().unwrap()),
466            i_0: f64::from_le_bytes(data[106..114].try_into().unwrap()),
467            c_rc: f32::from_le_bytes(data[114..118].try_into().unwrap()),
468            omega: f64::from_le_bytes(data[118..126].try_into().unwrap()),
469            omega_dot: f32::from_le_bytes(data[126..130].try_into().unwrap()),
470            i_dot: f32::from_le_bytes(data[130..134].try_into().unwrap()),
471            wn_t_oc: u16::from_le_bytes([data[134], data[135]]),
472            wn_t_oe: u16::from_le_bytes([data[136], data[137]]),
473        })
474    }
475}
476
477/// QZSNav (4095) — QZSS ephemeris and clock (same binary layout as [`GpsNavBlock`] / `GPSNav`).
478#[derive(Debug, Clone)]
479pub struct QzsNavBlock(pub GpsNavBlock);
480
481impl std::ops::Deref for QzsNavBlock {
482    type Target = GpsNavBlock;
483
484    fn deref(&self) -> &Self::Target {
485        &self.0
486    }
487}
488
489impl SbfBlockParse for QzsNavBlock {
490    const BLOCK_ID: u16 = block_ids::QZS_NAV;
491
492    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
493        Ok(QzsNavBlock(GpsNavBlock::parse(header, data)?))
494    }
495}
496
497/// BDSAlm (4119) — BeiDou almanac.
498#[derive(Debug, Clone)]
499pub struct BdsAlmBlock {
500    tow_ms: u32,
501    wnc: u16,
502    pub prn: u8,
503    pub wn_a: u8,
504    pub t_oa: u32,
505    pub sqrt_a: f32,
506    pub e: f32,
507    pub omega: f32,
508    pub m_0: f32,
509    pub omega_0: f32,
510    pub omega_dot: f32,
511    pub delta_i: f32,
512    pub a_f0: f32,
513    pub a_f1: f32,
514    pub health: u16,
515}
516
517impl BdsAlmBlock {
518    pub fn tow_ms(&self) -> u32 {
519        self.tow_ms
520    }
521    pub fn wnc(&self) -> u16 {
522        self.wnc
523    }
524    pub fn tow_seconds(&self) -> f64 {
525        self.tow_ms as f64 * 0.001
526    }
527}
528
529impl SbfBlockParse for BdsAlmBlock {
530    const BLOCK_ID: u16 = block_ids::BDS_ALM;
531
532    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
533        const MIN: usize = 56;
534        if data.len() < MIN {
535            return Err(SbfError::ParseError("BDSAlm too short".into()));
536        }
537        Ok(Self {
538            tow_ms: header.tow_ms,
539            wnc: header.wnc,
540            prn: data[12],
541            wn_a: data[13],
542            t_oa: u32::from_le_bytes(data[14..18].try_into().unwrap()),
543            sqrt_a: f32::from_le_bytes(data[18..22].try_into().unwrap()),
544            e: f32::from_le_bytes(data[22..26].try_into().unwrap()),
545            omega: f32::from_le_bytes(data[26..30].try_into().unwrap()),
546            m_0: f32::from_le_bytes(data[30..34].try_into().unwrap()),
547            omega_0: f32::from_le_bytes(data[34..38].try_into().unwrap()),
548            omega_dot: f32::from_le_bytes(data[38..42].try_into().unwrap()),
549            delta_i: f32::from_le_bytes(data[42..46].try_into().unwrap()),
550            a_f0: f32::from_le_bytes(data[46..50].try_into().unwrap()),
551            a_f1: f32::from_le_bytes(data[50..54].try_into().unwrap()),
552            health: u16::from_le_bytes(data[54..56].try_into().unwrap()),
553        })
554    }
555}
556
557/// QZSAlm (4116) — QZSS almanac.
558#[derive(Debug, Clone)]
559pub struct QzsAlmBlock {
560    tow_ms: u32,
561    wnc: u16,
562    pub prn: u8,
563    pub e: f32,
564    pub t_oa: u32,
565    pub delta_i: f32,
566    pub omega_dot: f32,
567    pub sqrt_a: f32,
568    pub omega_0: f32,
569    pub omega: f32,
570    pub m_0: f32,
571    pub a_f1: f32,
572    pub a_f0: f32,
573    pub wn_a: u8,
574    pub health8: u8,
575    pub health6: u8,
576}
577
578impl QzsAlmBlock {
579    pub fn tow_ms(&self) -> u32 {
580        self.tow_ms
581    }
582    pub fn wnc(&self) -> u16 {
583        self.wnc
584    }
585    pub fn tow_seconds(&self) -> f64 {
586        self.tow_ms as f64 * 0.001
587    }
588}
589
590impl SbfBlockParse for QzsAlmBlock {
591    const BLOCK_ID: u16 = block_ids::QZS_ALM;
592
593    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
594        const MIN: usize = 58;
595        if data.len() < MIN {
596            return Err(SbfError::ParseError("QZSAlm too short".into()));
597        }
598        // The public QZSAlm layout includes reserved bytes after `PRN` and `WN_a`.
599        Ok(Self {
600            tow_ms: header.tow_ms,
601            wnc: header.wnc,
602            prn: data[12],
603            e: f32::from_le_bytes(data[14..18].try_into().unwrap()),
604            t_oa: u32::from_le_bytes(data[18..22].try_into().unwrap()),
605            delta_i: f32::from_le_bytes(data[22..26].try_into().unwrap()),
606            omega_dot: f32::from_le_bytes(data[26..30].try_into().unwrap()),
607            sqrt_a: f32::from_le_bytes(data[30..34].try_into().unwrap()),
608            omega_0: f32::from_le_bytes(data[34..38].try_into().unwrap()),
609            omega: f32::from_le_bytes(data[38..42].try_into().unwrap()),
610            m_0: f32::from_le_bytes(data[42..46].try_into().unwrap()),
611            a_f1: f32::from_le_bytes(data[46..50].try_into().unwrap()),
612            a_f0: f32::from_le_bytes(data[50..54].try_into().unwrap()),
613            wn_a: data[54],
614            health8: data[56],
615            health6: data[57],
616        })
617    }
618}
619
620/// BDSCNav2 (4252) — BeiDou B-CNAV2 ephemeris from the B2a signal.
621#[derive(Debug, Clone)]
622pub struct BdsCNav2Block {
623    tow_ms: u32,
624    wnc: u16,
625    pub prn_idx: u8,
626    pub flags: u8,
627    pub t_oe: u32,
628    pub a: f64,
629    pub a_dot: f64,
630    pub delta_n0: f32,
631    pub delta_n0_dot: f32,
632    pub m_0: f64,
633    pub e: f64,
634    pub omega: f64,
635    pub omega_0: f64,
636    pub omega_dot: f32,
637    pub i_0: f64,
638    pub i_dot: f32,
639    pub c_is: f32,
640    pub c_ic: f32,
641    pub c_rs: f32,
642    pub c_rc: f32,
643    pub c_us: f32,
644    pub c_uc: f32,
645    pub t_oc: u32,
646    pub a_2: f32,
647    pub a_1: f32,
648    pub a_0: f64,
649    pub t_op: u32,
650    pub sisai_ocb: u8,
651    pub sisai_oc12: u8,
652    pub sisai_oe: u8,
653    pub sismai: u8,
654    pub health_if: u8,
655    pub iode: u8,
656    pub iodc: u16,
657    pub isc_b2ad: f32,
658    pub t_gd_b2ap: f32,
659    pub t_gd_b1cp: f32,
660}
661
662impl BdsCNav2Block {
663    pub fn tow_ms(&self) -> u32 {
664        self.tow_ms
665    }
666    pub fn wnc(&self) -> u16 {
667        self.wnc
668    }
669    pub fn tow_seconds(&self) -> f64 {
670        self.tow_ms as f64 * 0.001
671    }
672    pub fn satellite_type(&self) -> u8 {
673        self.flags & 0x03
674    }
675    pub fn is_healthy(&self) -> bool {
676        (self.health_if & 0xC0) == 0
677    }
678    pub fn isc_b2ad_s(&self) -> Option<f32> {
679        f32_or_none(self.isc_b2ad)
680    }
681    pub fn t_gd_b2ap_s(&self) -> Option<f32> {
682        f32_or_none(self.t_gd_b2ap)
683    }
684    pub fn t_gd_b1cp_s(&self) -> Option<f32> {
685        f32_or_none(self.t_gd_b1cp)
686    }
687}
688
689impl SbfBlockParse for BdsCNav2Block {
690    const BLOCK_ID: u16 = block_ids::BDS_CNAV2;
691
692    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
693        const MIN: usize = 158;
694        if data.len() < MIN {
695            return Err(SbfError::ParseError("BDSCNav2 too short".into()));
696        }
697        Ok(Self {
698            tow_ms: header.tow_ms,
699            wnc: header.wnc,
700            prn_idx: data[12],
701            flags: data[13],
702            t_oe: u32::from_le_bytes(data[14..18].try_into().unwrap()),
703            a: f64::from_le_bytes(data[18..26].try_into().unwrap()),
704            a_dot: f64::from_le_bytes(data[26..34].try_into().unwrap()),
705            delta_n0: f32::from_le_bytes(data[34..38].try_into().unwrap()),
706            delta_n0_dot: f32::from_le_bytes(data[38..42].try_into().unwrap()),
707            m_0: f64::from_le_bytes(data[42..50].try_into().unwrap()),
708            e: f64::from_le_bytes(data[50..58].try_into().unwrap()),
709            omega: f64::from_le_bytes(data[58..66].try_into().unwrap()),
710            omega_0: f64::from_le_bytes(data[66..74].try_into().unwrap()),
711            omega_dot: f32::from_le_bytes(data[74..78].try_into().unwrap()),
712            i_0: f64::from_le_bytes(data[78..86].try_into().unwrap()),
713            i_dot: f32::from_le_bytes(data[86..90].try_into().unwrap()),
714            c_is: f32::from_le_bytes(data[90..94].try_into().unwrap()),
715            c_ic: f32::from_le_bytes(data[94..98].try_into().unwrap()),
716            c_rs: f32::from_le_bytes(data[98..102].try_into().unwrap()),
717            c_rc: f32::from_le_bytes(data[102..106].try_into().unwrap()),
718            c_us: f32::from_le_bytes(data[106..110].try_into().unwrap()),
719            c_uc: f32::from_le_bytes(data[110..114].try_into().unwrap()),
720            t_oc: u32::from_le_bytes(data[114..118].try_into().unwrap()),
721            a_2: f32::from_le_bytes(data[118..122].try_into().unwrap()),
722            a_1: f32::from_le_bytes(data[122..126].try_into().unwrap()),
723            a_0: f64::from_le_bytes(data[126..134].try_into().unwrap()),
724            t_op: u32::from_le_bytes(data[134..138].try_into().unwrap()),
725            sisai_ocb: data[138],
726            sisai_oc12: data[139],
727            sisai_oe: data[140],
728            sismai: data[141],
729            health_if: data[142],
730            iode: data[143],
731            iodc: u16::from_le_bytes(data[144..146].try_into().unwrap()),
732            isc_b2ad: f32::from_le_bytes(data[146..150].try_into().unwrap()),
733            t_gd_b2ap: f32::from_le_bytes(data[150..154].try_into().unwrap()),
734            t_gd_b1cp: f32::from_le_bytes(data[154..158].try_into().unwrap()),
735        })
736    }
737}
738
739/// BDSCNav3 (4253) — BeiDou B-CNAV3 ephemeris from the B2b_I signal.
740#[derive(Debug, Clone)]
741pub struct BdsCNav3Block {
742    tow_ms: u32,
743    wnc: u16,
744    pub prn_idx: u8,
745    pub flags: u8,
746    pub t_oe: u32,
747    pub a: f64,
748    pub a_dot: f64,
749    pub delta_n0: f32,
750    pub delta_n0_dot: f32,
751    pub m_0: f64,
752    pub e: f64,
753    pub omega: f64,
754    pub omega_0: f64,
755    pub omega_dot: f32,
756    pub i_0: f64,
757    pub i_dot: f32,
758    pub c_is: f32,
759    pub c_ic: f32,
760    pub c_rs: f32,
761    pub c_rc: f32,
762    pub c_us: f32,
763    pub c_uc: f32,
764    pub t_oc: u32,
765    pub a_2: f32,
766    pub a_1: f32,
767    pub a_0: f64,
768    pub t_op: u32,
769    pub sisai_ocb: u8,
770    pub sisai_oc12: u8,
771    pub sisai_oe: u8,
772    pub sismai: u8,
773    pub health_if: u8,
774    pub reserved: [u8; 3],
775    pub t_gd_b2bi: f32,
776}
777
778impl BdsCNav3Block {
779    pub fn tow_ms(&self) -> u32 {
780        self.tow_ms
781    }
782    pub fn wnc(&self) -> u16 {
783        self.wnc
784    }
785    pub fn tow_seconds(&self) -> f64 {
786        self.tow_ms as f64 * 0.001
787    }
788    pub fn satellite_type(&self) -> u8 {
789        self.flags & 0x03
790    }
791    pub fn is_healthy(&self) -> bool {
792        (self.health_if & 0xC0) == 0
793    }
794    pub fn t_gd_b2bi_s(&self) -> Option<f32> {
795        f32_or_none(self.t_gd_b2bi)
796    }
797}
798
799impl SbfBlockParse for BdsCNav3Block {
800    const BLOCK_ID: u16 = block_ids::BDS_CNAV3;
801
802    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
803        const MIN: usize = 150;
804        if data.len() < MIN {
805            return Err(SbfError::ParseError("BDSCNav3 too short".into()));
806        }
807        Ok(Self {
808            tow_ms: header.tow_ms,
809            wnc: header.wnc,
810            prn_idx: data[12],
811            flags: data[13],
812            t_oe: u32::from_le_bytes(data[14..18].try_into().unwrap()),
813            a: f64::from_le_bytes(data[18..26].try_into().unwrap()),
814            a_dot: f64::from_le_bytes(data[26..34].try_into().unwrap()),
815            delta_n0: f32::from_le_bytes(data[34..38].try_into().unwrap()),
816            delta_n0_dot: f32::from_le_bytes(data[38..42].try_into().unwrap()),
817            m_0: f64::from_le_bytes(data[42..50].try_into().unwrap()),
818            e: f64::from_le_bytes(data[50..58].try_into().unwrap()),
819            omega: f64::from_le_bytes(data[58..66].try_into().unwrap()),
820            omega_0: f64::from_le_bytes(data[66..74].try_into().unwrap()),
821            omega_dot: f32::from_le_bytes(data[74..78].try_into().unwrap()),
822            i_0: f64::from_le_bytes(data[78..86].try_into().unwrap()),
823            i_dot: f32::from_le_bytes(data[86..90].try_into().unwrap()),
824            c_is: f32::from_le_bytes(data[90..94].try_into().unwrap()),
825            c_ic: f32::from_le_bytes(data[94..98].try_into().unwrap()),
826            c_rs: f32::from_le_bytes(data[98..102].try_into().unwrap()),
827            c_rc: f32::from_le_bytes(data[102..106].try_into().unwrap()),
828            c_us: f32::from_le_bytes(data[106..110].try_into().unwrap()),
829            c_uc: f32::from_le_bytes(data[110..114].try_into().unwrap()),
830            t_oc: u32::from_le_bytes(data[114..118].try_into().unwrap()),
831            a_2: f32::from_le_bytes(data[118..122].try_into().unwrap()),
832            a_1: f32::from_le_bytes(data[122..126].try_into().unwrap()),
833            a_0: f64::from_le_bytes(data[126..134].try_into().unwrap()),
834            t_op: u32::from_le_bytes(data[134..138].try_into().unwrap()),
835            sisai_ocb: data[138],
836            sisai_oc12: data[139],
837            sisai_oe: data[140],
838            sismai: data[141],
839            health_if: data[142],
840            reserved: data[143..146].try_into().unwrap(),
841            t_gd_b2bi: f32::from_le_bytes(data[146..150].try_into().unwrap()),
842        })
843    }
844}
845
846/// BDSUtc (4121) — BDT-UTC parameters (`cmpUtc`).
847#[derive(Debug, Clone)]
848pub struct BdsUtcBlock {
849    tow_ms: u32,
850    wnc: u16,
851    pub prn: u8,
852    pub a_1: f32,
853    pub a_0: f64,
854    pub delta_t_ls: i8,
855    pub wn_lsf: u8,
856    pub dn: u8,
857    pub delta_t_lsf: i8,
858}
859
860impl BdsUtcBlock {
861    pub fn tow_ms(&self) -> u32 {
862        self.tow_ms
863    }
864    pub fn wnc(&self) -> u16 {
865        self.wnc
866    }
867    pub fn tow_seconds(&self) -> f64 {
868        self.tow_ms as f64 * 0.001
869    }
870    pub fn a_1_opt(&self) -> Option<f32> {
871        f32_or_none(self.a_1)
872    }
873    pub fn a_0_opt(&self) -> Option<f64> {
874        f64_or_none(self.a_0)
875    }
876}
877
878impl SbfBlockParse for BdsUtcBlock {
879    const BLOCK_ID: u16 = block_ids::BDS_UTC;
880
881    fn parse(header: &SbfHeader, data: &[u8]) -> SbfResult<Self> {
882        if data.len() < 30 {
883            return Err(SbfError::ParseError("BDSUtc too short".into()));
884        }
885        Ok(Self {
886            tow_ms: header.tow_ms,
887            wnc: header.wnc,
888            prn: data[12],
889            a_1: f32::from_le_bytes(data[14..18].try_into().unwrap()),
890            a_0: f64::from_le_bytes(data[18..26].try_into().unwrap()),
891            delta_t_ls: data[26] as i8,
892            wn_lsf: data[27],
893            dn: data[28],
894            delta_t_lsf: data[29] as i8,
895        })
896    }
897}
898
899#[cfg(test)]
900mod tests {
901    use super::*;
902
903    #[test]
904    fn pos_local_roundtrip_minimal() {
905        let mut d = vec![0u8; 44];
906        d[6..10].copy_from_slice(&1000u32.to_le_bytes());
907        d[10..12].copy_from_slice(&200u16.to_le_bytes());
908        d[12] = 1;
909        d[13] = 0;
910        d[14..22].copy_from_slice(&1.0f64.to_le_bytes());
911        d[22..30].copy_from_slice(&2.0f64.to_le_bytes());
912        d[30..38].copy_from_slice(&3.0f64.to_le_bytes());
913        d[38] = 5;
914        let h = SbfHeader {
915            crc: 0,
916            block_id: block_ids::POS_LOCAL,
917            block_rev: 0,
918            length: 46,
919            tow_ms: 1000,
920            wnc: 200,
921        };
922        let p = PosLocalBlock::parse(&h, &d).unwrap();
923        assert_eq!(p.latitude_rad, 1.0);
924        assert_eq!(p.datum, 5);
925    }
926
927    #[test]
928    fn bds_alm_parse_minimal() {
929        let mut d = vec![0u8; 56];
930        d[12] = 12;
931        d[13] = 34;
932        d[14..18].copy_from_slice(&5678u32.to_le_bytes());
933        d[18..22].copy_from_slice(&5153.5f32.to_le_bytes());
934        d[54..56].copy_from_slice(&0x0123u16.to_le_bytes());
935        let h = SbfHeader {
936            crc: 0,
937            block_id: block_ids::BDS_ALM,
938            block_rev: 0,
939            length: (d.len() + 2) as u16,
940            tow_ms: 1000,
941            wnc: 200,
942        };
943        let block = BdsAlmBlock::parse(&h, &d).unwrap();
944        assert_eq!(block.prn, 12);
945        assert_eq!(block.wn_a, 34);
946        assert_eq!(block.t_oa, 5678);
947        assert_eq!(block.health, 0x0123);
948    }
949
950    #[test]
951    fn qzs_alm_parse_minimal_respects_reserved_bytes() {
952        let mut d = vec![0u8; 58];
953        d[12] = 3;
954        d[13] = 0x7E;
955        d[14..18].copy_from_slice(&0.25f32.to_le_bytes());
956        d[18..22].copy_from_slice(&3456u32.to_le_bytes());
957        d[54] = 77;
958        d[55] = 0x5A;
959        d[56] = 0xAA;
960        d[57] = 0x55;
961        let h = SbfHeader {
962            crc: 0,
963            block_id: block_ids::QZS_ALM,
964            block_rev: 0,
965            length: (d.len() + 2) as u16,
966            tow_ms: 2000,
967            wnc: 300,
968        };
969        let block = QzsAlmBlock::parse(&h, &d).unwrap();
970        assert_eq!(block.prn, 3);
971        assert!((block.e - 0.25).abs() < 1e-6);
972        assert_eq!(block.t_oa, 3456);
973        assert_eq!(block.wn_a, 77);
974        assert_eq!(block.health8, 0xAA);
975        assert_eq!(block.health6, 0x55);
976    }
977
978    #[test]
979    fn bds_cnav2_parse_minimal() {
980        let mut d = vec![0u8; 158];
981        d[12] = 21;
982        d[13] = 3;
983        d[14..18].copy_from_slice(&7200u32.to_le_bytes());
984        d[18..26].copy_from_slice(&42_164_000.0f64.to_le_bytes());
985        d[134..138].copy_from_slice(&8000u32.to_le_bytes());
986        d[142] = 0;
987        d[143] = 44;
988        d[144..146].copy_from_slice(&0x1122u16.to_le_bytes());
989        d[146..150].copy_from_slice(&1.25f32.to_le_bytes());
990        d[150..154].copy_from_slice(&2.5f32.to_le_bytes());
991        d[154..158].copy_from_slice(&3.75f32.to_le_bytes());
992        let h = SbfHeader {
993            crc: 0,
994            block_id: block_ids::BDS_CNAV2,
995            block_rev: 0,
996            length: (d.len() + 2) as u16,
997            tow_ms: 3000,
998            wnc: 400,
999        };
1000        let block = BdsCNav2Block::parse(&h, &d).unwrap();
1001        assert_eq!(block.prn_idx, 21);
1002        assert_eq!(block.satellite_type(), 3);
1003        assert_eq!(block.t_oe, 7200);
1004        assert_eq!(block.t_op, 8000);
1005        assert_eq!(block.iode, 44);
1006        assert_eq!(block.iodc, 0x1122);
1007        assert_eq!(block.isc_b2ad_s(), Some(1.25));
1008        assert_eq!(block.t_gd_b2ap_s(), Some(2.5));
1009        assert_eq!(block.t_gd_b1cp_s(), Some(3.75));
1010    }
1011
1012    #[test]
1013    fn bds_cnav3_parse_minimal() {
1014        let mut d = vec![0u8; 150];
1015        d[12] = 7;
1016        d[13] = 2;
1017        d[14..18].copy_from_slice(&900u32.to_le_bytes());
1018        d[138] = 9;
1019        d[142] = 0;
1020        d[143..146].copy_from_slice(&[1, 2, 3]);
1021        d[146..150].copy_from_slice(&(-0.5f32).to_le_bytes());
1022        let h = SbfHeader {
1023            crc: 0,
1024            block_id: block_ids::BDS_CNAV3,
1025            block_rev: 0,
1026            length: (d.len() + 2) as u16,
1027            tow_ms: 4000,
1028            wnc: 500,
1029        };
1030        let block = BdsCNav3Block::parse(&h, &d).unwrap();
1031        assert_eq!(block.prn_idx, 7);
1032        assert_eq!(block.satellite_type(), 2);
1033        assert_eq!(block.t_oe, 900);
1034        assert_eq!(block.sisai_ocb, 9);
1035        assert_eq!(block.reserved, [1, 2, 3]);
1036        assert_eq!(block.t_gd_b2bi_s(), Some(-0.5));
1037    }
1038}
sbf_tools/blocks/extended.rs

sbf_tools/blocks/
extended.rs
