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

sbf_tools/blocks/
extended.rs
