sidereon_core/sbas/
message.rs

1use crate::error::{Error, Result};
2use crate::rtcm::bits::{BitReader, BitWriter};
3use crate::rtcm::crc::crc24q_bits;
4
5pub type SbasMessageType = u8;
6
7const FRAMED_LEN: usize = 32;
8const BODY_LEN: usize = 29;
9const HEADER_BITS: usize = 14;
10const DATA_BITS: usize = 212;
11const BODY_BITS: usize = HEADER_BITS + DATA_BITS;
12const CRC_BITS: usize = 24;
13const FRAMED_BITS: usize = BODY_BITS + CRC_BITS;
14const PREAMBLES: [u8; 3] = [0x53, 0x9A, 0xC6];
15
16#[derive(Clone, Debug, PartialEq, Eq, Default)]
17pub struct SpareBits(pub Vec<(u64, u8)>);
18
19impl SpareBits {
20    pub fn new() -> Self {
21        Self(Vec::new())
22    }
23
24    pub fn push(&mut self, value: u64, width: u8) {
25        self.0.push((value, width));
26    }
27}
28
29#[derive(Clone, Debug, PartialEq, Eq)]
30pub enum SbasMessage {
31    DoNotUse(SbasDoNotUse),
32    PrnMask(SbasPrnMask),
33    FastCorrections(SbasFastCorrections),
34    Integrity(SbasIntegrity),
35    FastDegradation(SbasFastDegradation),
36    GeoNav(SbasGeoNav),
37    NetworkTime(SbasNetworkTime),
38    GeoAlmanac(SbasGeoAlmanac),
39    IgpMask(SbasIgpMask),
40    MixedCorrections(SbasMixedCorrections),
41    LongTermCorrections(SbasLongTermCorrections),
42    IonoDelays(SbasIonoDelays),
43    Unsupported(SbasUnsupported),
44}
45
46#[derive(Clone, Debug, PartialEq, Eq)]
47pub struct SbasDoNotUse {
48    pub preamble: u8,
49    pub data: Vec<u8>,
50}
51
52#[derive(Clone, Debug, PartialEq, Eq)]
53pub struct SbasPrnMask {
54    pub preamble: u8,
55    pub iodp: u8,
56    pub mask: [bool; 210],
57    pub reserved: SpareBits,
58}
59
60#[derive(Clone, Debug, PartialEq, Eq)]
61pub struct SbasFastCorrections {
62    pub preamble: u8,
63    pub message_type: SbasMessageType,
64    pub iodf: u8,
65    pub iodp: u8,
66    pub prc: [i16; 13],
67    pub udrei: [u8; 13],
68    pub reserved: SpareBits,
69}
70
71#[derive(Clone, Debug, PartialEq, Eq)]
72pub struct SbasIntegrity {
73    pub preamble: u8,
74    pub iodf: [u8; 4],
75    pub udrei: [u8; 51],
76    pub reserved: SpareBits,
77}
78
79#[derive(Clone, Debug, PartialEq, Eq)]
80pub struct SbasFastDegradation {
81    pub preamble: u8,
82    pub system_latency_s: u8,
83    pub iodp: u8,
84    pub ai: [u8; 51],
85    pub reserved: SpareBits,
86}
87
88#[derive(Clone, Debug, PartialEq, Eq)]
89pub struct SbasGeoNav {
90    pub preamble: u8,
91    pub time_of_day_s: u16,
92    pub ura: u8,
93    pub x_m: i32,
94    pub y_m: i32,
95    pub z_m: i32,
96    pub x_rate_m_s: i32,
97    pub y_rate_m_s: i32,
98    pub z_rate_m_s: i32,
99    pub x_accel_m_s2: i16,
100    pub y_accel_m_s2: i16,
101    pub z_accel_m_s2: i16,
102    pub a_gf0_s: i16,
103    pub a_gf1_s_s: i16,
104    pub reserved: SpareBits,
105}
106
107#[derive(Clone, Debug, PartialEq, Eq)]
108pub struct SbasNetworkTime {
109    pub preamble: u8,
110    pub data: Vec<u8>,
111}
112
113#[derive(Clone, Debug, PartialEq, Eq)]
114pub struct SbasGeoAlmanac {
115    pub preamble: u8,
116    pub data: Vec<u8>,
117}
118
119#[derive(Clone, Debug, PartialEq, Eq)]
120pub struct SbasMixedCorrections {
121    pub preamble: u8,
122    pub fast: SbasMixedFastCorrections,
123    pub long_term: SbasLongTermHalf,
124}
125
126#[derive(Clone, Debug, PartialEq, Eq)]
127pub struct SbasMixedFastCorrections {
128    pub iodf: u8,
129    pub iodp: u8,
130    pub block_id: u8,
131    pub prc: [i16; 6],
132    pub udrei: [u8; 6],
133    pub reserved: SpareBits,
134}
135
136#[derive(Clone, Debug, PartialEq, Eq)]
137pub struct SbasLongTermCorrections {
138    pub preamble: u8,
139    pub halves: [SbasLongTermHalf; 2],
140}
141
142#[derive(Clone, Debug, PartialEq, Eq)]
143pub struct SbasLongTermHalf {
144    pub velocity_code: bool,
145    pub iodp: u8,
146    pub records: Vec<SbasLongTermRecord>,
147    pub reserved: SpareBits,
148}
149
150#[derive(Clone, Debug, PartialEq, Eq)]
151pub struct SbasLongTermRecord {
152    pub monitored_index: u8,
153    pub iode: u8,
154    pub delta_x: i32,
155    pub delta_y: i32,
156    pub delta_z: i32,
157    pub delta_x_rate: i32,
158    pub delta_y_rate: i32,
159    pub delta_z_rate: i32,
160    pub delta_a_f0: i32,
161    pub delta_a_f1: i32,
162    pub time_of_day_s: Option<u32>,
163}
164
165#[derive(Clone, Debug, PartialEq, Eq)]
166pub struct SbasIgpMask {
167    pub preamble: u8,
168    pub band_number: u8,
169    pub iodi: u8,
170    pub mask: [bool; 201],
171    pub reserved: SpareBits,
172}
173
174#[derive(Clone, Debug, PartialEq, Eq)]
175pub struct SbasIonoDelays {
176    pub preamble: u8,
177    pub band_number: u8,
178    pub block_id: u8,
179    pub iodi: u8,
180    pub entries: [SbasIgpDelay; 15],
181    pub reserved: SpareBits,
182}
183
184#[derive(Clone, Debug, PartialEq, Eq, Default)]
185pub struct SbasIgpDelay {
186    pub vertical_delay: u16,
187    pub givei: u8,
188}
189
190#[derive(Clone, Debug, PartialEq, Eq)]
191pub struct SbasUnsupported {
192    pub preamble: u8,
193    pub message_type: SbasMessageType,
194    pub data: Vec<u8>,
195}
196
197#[derive(Clone, Copy, Debug, PartialEq, Eq)]
198pub enum SbasWireForm {
199    Framed250,
200    Body226,
201}
202
203#[derive(Clone, Debug, PartialEq, Eq)]
204pub struct SbasBlock {
205    pub form: SbasWireForm,
206    pub message: SbasMessage,
207}
208
209struct CountedBitWriter {
210    writer: BitWriter,
211    nbits: usize,
212}
213
214impl CountedBitWriter {
215    fn new() -> Self {
216        Self {
217            writer: BitWriter::new(),
218            nbits: 0,
219        }
220    }
221
222    fn push_u(&mut self, value: u64, width: usize) {
223        self.writer.push_u(value, width);
224        self.nbits += width;
225    }
226
227    fn push_i(&mut self, value: i64, width: usize) {
228        self.writer.push_i(value, width);
229        self.nbits += width;
230    }
231
232    fn push_flag(&mut self, value: bool) {
233        self.writer.push_flag(value);
234        self.nbits += 1;
235    }
236
237    fn push_bits_from(&mut self, bytes: &[u8], nbits: usize) {
238        for bit_pos in 0..nbits {
239            self.push_u(u64::from(bit_at(bytes, bit_pos)), 1);
240        }
241    }
242
243    fn pad_to(&mut self, nbits: usize) {
244        while self.nbits < nbits {
245            self.push_u(0, 1);
246        }
247    }
248
249    fn into_bytes(self) -> Vec<u8> {
250        self.writer.into_bytes()
251    }
252}
253
254impl SbasBlock {
255    pub fn decode(bytes: &[u8], form: SbasWireForm) -> Result<Self> {
256        match form {
257            SbasWireForm::Framed250 => {
258                if bytes.len() != FRAMED_LEN {
259                    return Err(parse_error("SBAS framed block must be 32 bytes"));
260                }
261                let got = bits_as_u32(bytes, BODY_BITS, CRC_BITS);
262                let want = crc24q_bits(bytes, BODY_BITS);
263                if got != want {
264                    return Err(parse_error("SBAS CRC mismatch"));
265                }
266            }
267            SbasWireForm::Body226 => {
268                if bytes.len() != BODY_LEN {
269                    return Err(parse_error("SBAS body block must be 29 bytes"));
270                }
271            }
272        }
273
274        let mut reader = BitReader::new(bytes);
275        let preamble = reader.u(8)? as u8;
276        if !PREAMBLES.contains(&preamble) {
277            return Err(parse_error("SBAS preamble not recognized"));
278        }
279        let message_type = reader.u(6)? as u8;
280        let data = read_bits_as_bytes(&mut reader, DATA_BITS)?;
281        let message = decode_message(preamble, message_type, &data)?;
282        Ok(Self { form, message })
283    }
284
285    pub fn encode(&self) -> Vec<u8> {
286        let mut body = CountedBitWriter::new();
287        body.push_u(u64::from(self.message.preamble()), 8);
288        body.push_u(u64::from(self.message.message_type()), 6);
289        let data = self.message.encode_data();
290        body.push_bits_from(&data, DATA_BITS);
291        body.pad_to(BODY_BITS);
292        let body_bytes = body.into_bytes();
293
294        match self.form {
295            SbasWireForm::Body226 => body_bytes,
296            SbasWireForm::Framed250 => {
297                let crc = crc24q_bits(&body_bytes, BODY_BITS);
298                let mut framed = CountedBitWriter::new();
299                framed.push_bits_from(&body_bytes, BODY_BITS);
300                framed.push_u(u64::from(crc), CRC_BITS);
301                framed.pad_to(FRAMED_BITS);
302                framed.into_bytes()
303            }
304        }
305    }
306}
307
308impl SbasMessage {
309    pub fn message_type(&self) -> SbasMessageType {
310        match self {
311            Self::DoNotUse(_) => 0,
312            Self::PrnMask(_) => 1,
313            Self::FastCorrections(m) => m.message_type,
314            Self::Integrity(_) => 6,
315            Self::FastDegradation(_) => 7,
316            Self::GeoNav(_) => 9,
317            Self::NetworkTime(_) => 12,
318            Self::GeoAlmanac(_) => 17,
319            Self::IgpMask(_) => 18,
320            Self::MixedCorrections(_) => 24,
321            Self::LongTermCorrections(_) => 25,
322            Self::IonoDelays(_) => 26,
323            Self::Unsupported(m) => m.message_type,
324        }
325    }
326
327    fn preamble(&self) -> u8 {
328        match self {
329            Self::DoNotUse(m) => m.preamble,
330            Self::PrnMask(m) => m.preamble,
331            Self::FastCorrections(m) => m.preamble,
332            Self::Integrity(m) => m.preamble,
333            Self::FastDegradation(m) => m.preamble,
334            Self::GeoNav(m) => m.preamble,
335            Self::NetworkTime(m) => m.preamble,
336            Self::GeoAlmanac(m) => m.preamble,
337            Self::IgpMask(m) => m.preamble,
338            Self::MixedCorrections(m) => m.preamble,
339            Self::LongTermCorrections(m) => m.preamble,
340            Self::IonoDelays(m) => m.preamble,
341            Self::Unsupported(m) => m.preamble,
342        }
343    }
344
345    fn encode_data(&self) -> Vec<u8> {
346        match self {
347            Self::DoNotUse(m) => data_from_raw(&m.data),
348            Self::PrnMask(m) => encode_prn_mask(m),
349            Self::FastCorrections(m) => encode_fast(m),
350            Self::Integrity(m) => encode_integrity(m),
351            Self::FastDegradation(m) => encode_fast_degradation(m),
352            Self::GeoNav(m) => encode_geo_nav(m),
353            Self::NetworkTime(m) => data_from_raw(&m.data),
354            Self::GeoAlmanac(m) => data_from_raw(&m.data),
355            Self::IgpMask(m) => encode_igp_mask(m),
356            Self::MixedCorrections(m) => encode_mixed(m),
357            Self::LongTermCorrections(m) => encode_long_term(m),
358            Self::IonoDelays(m) => encode_iono_delays(m),
359            Self::Unsupported(m) => data_from_raw(&m.data),
360        }
361    }
362}
363
364pub(crate) fn is_phase_a_message(mt: SbasMessageType) -> bool {
365    matches!(mt, 0..=7 | 9 | 12 | 17 | 18 | 24 | 25 | 26)
366}
367
368fn decode_message(preamble: u8, message_type: u8, data: &[u8]) -> Result<SbasMessage> {
369    if !is_phase_a_message(message_type) {
370        return Ok(SbasMessage::Unsupported(SbasUnsupported {
371            preamble,
372            message_type,
373            data: data_from_raw(data),
374        }));
375    }
376    match message_type {
377        0 => Ok(SbasMessage::DoNotUse(SbasDoNotUse {
378            preamble,
379            data: data_from_raw(data),
380        })),
381        1 => decode_prn_mask(preamble, data).map(SbasMessage::PrnMask),
382        2..=5 => decode_fast(preamble, message_type, data).map(SbasMessage::FastCorrections),
383        6 => decode_integrity(preamble, data).map(SbasMessage::Integrity),
384        7 => decode_fast_degradation(preamble, data).map(SbasMessage::FastDegradation),
385        9 => decode_geo_nav(preamble, data).map(SbasMessage::GeoNav),
386        12 => Ok(SbasMessage::NetworkTime(SbasNetworkTime {
387            preamble,
388            data: data_from_raw(data),
389        })),
390        17 => Ok(SbasMessage::GeoAlmanac(SbasGeoAlmanac {
391            preamble,
392            data: data_from_raw(data),
393        })),
394        18 => decode_igp_mask(preamble, data).map(SbasMessage::IgpMask),
395        24 => decode_mixed(preamble, data).map(SbasMessage::MixedCorrections),
396        25 => decode_long_term(preamble, data).map(SbasMessage::LongTermCorrections),
397        26 => decode_iono_delays(preamble, data).map(SbasMessage::IonoDelays),
398        _ => unreachable!("phase A classification and decode match are out of sync"),
399    }
400}
401
402fn decode_prn_mask(preamble: u8, data: &[u8]) -> Result<SbasPrnMask> {
403    let mut r = BitReader::new(data);
404    let mut mask = [false; 210];
405    for bit in &mut mask {
406        *bit = r.flag()?;
407    }
408    let iodp = r.u(2)? as u8;
409    Ok(SbasPrnMask {
410        preamble,
411        iodp,
412        mask,
413        reserved: SpareBits::new(),
414    })
415}
416
417fn encode_prn_mask(m: &SbasPrnMask) -> Vec<u8> {
418    let mut w = CountedBitWriter::new();
419    for bit in m.mask {
420        w.push_flag(bit);
421    }
422    w.push_u(u64::from(m.iodp), 2);
423    w.pad_to(DATA_BITS);
424    w.into_bytes()
425}
426
427fn decode_fast(preamble: u8, message_type: u8, data: &[u8]) -> Result<SbasFastCorrections> {
428    let mut r = BitReader::new(data);
429    let iodf = r.u(2)? as u8;
430    let iodp = r.u(2)? as u8;
431    let mut prc = [0i16; 13];
432    for value in &mut prc {
433        *value = r.i(12)? as i16;
434    }
435    let mut udrei = [0u8; 13];
436    for value in &mut udrei {
437        *value = r.u(4)? as u8;
438    }
439    Ok(SbasFastCorrections {
440        preamble,
441        message_type,
442        iodf,
443        iodp,
444        prc,
445        udrei,
446        reserved: SpareBits::new(),
447    })
448}
449
450fn encode_fast(m: &SbasFastCorrections) -> Vec<u8> {
451    let mut w = CountedBitWriter::new();
452    w.push_u(u64::from(m.iodf), 2);
453    w.push_u(u64::from(m.iodp), 2);
454    for value in m.prc {
455        w.push_i(i64::from(value), 12);
456    }
457    for value in m.udrei {
458        w.push_u(u64::from(value), 4);
459    }
460    w.pad_to(DATA_BITS);
461    w.into_bytes()
462}
463
464fn decode_integrity(preamble: u8, data: &[u8]) -> Result<SbasIntegrity> {
465    let mut r = BitReader::new(data);
466    let mut iodf = [0u8; 4];
467    for value in &mut iodf {
468        *value = r.u(2)? as u8;
469    }
470    let mut udrei = [0u8; 51];
471    for value in &mut udrei {
472        *value = r.u(4)? as u8;
473    }
474    Ok(SbasIntegrity {
475        preamble,
476        iodf,
477        udrei,
478        reserved: SpareBits::new(),
479    })
480}
481
482fn encode_integrity(m: &SbasIntegrity) -> Vec<u8> {
483    let mut w = CountedBitWriter::new();
484    for value in m.iodf {
485        w.push_u(u64::from(value), 2);
486    }
487    for value in m.udrei {
488        w.push_u(u64::from(value), 4);
489    }
490    w.pad_to(DATA_BITS);
491    w.into_bytes()
492}
493
494fn decode_fast_degradation(preamble: u8, data: &[u8]) -> Result<SbasFastDegradation> {
495    let mut r = BitReader::new(data);
496    let system_latency_s = r.u(4)? as u8;
497    let iodp = r.u(2)? as u8;
498    let mut ai = [0u8; 51];
499    for value in &mut ai {
500        *value = r.u(4)? as u8;
501    }
502    let mut reserved = SpareBits::new();
503    if r.remaining_bits() >= 2 {
504        reserved.push(r.u(2)?, 2);
505    }
506    Ok(SbasFastDegradation {
507        preamble,
508        system_latency_s,
509        iodp,
510        ai,
511        reserved,
512    })
513}
514
515fn encode_fast_degradation(m: &SbasFastDegradation) -> Vec<u8> {
516    let mut w = CountedBitWriter::new();
517    w.push_u(u64::from(m.system_latency_s), 4);
518    w.push_u(u64::from(m.iodp), 2);
519    for value in m.ai {
520        w.push_u(u64::from(value), 4);
521    }
522    push_spares(&mut w, &m.reserved);
523    w.pad_to(DATA_BITS);
524    w.into_bytes()
525}
526
527fn decode_geo_nav(preamble: u8, data: &[u8]) -> Result<SbasGeoNav> {
528    let mut r = BitReader::new(data);
529    let mut reserved = SpareBits::new();
530    reserved.push(r.u(8)?, 8);
531    let time_of_day_s = r.u(13)? as u16;
532    let ura = r.u(4)? as u8;
533    let x_m = r.i(30)? as i32;
534    let y_m = r.i(30)? as i32;
535    let z_m = r.i(25)? as i32;
536    let x_rate_m_s = r.i(17)? as i32;
537    let y_rate_m_s = r.i(17)? as i32;
538    let z_rate_m_s = r.i(18)? as i32;
539    let x_accel_m_s2 = r.i(10)? as i16;
540    let y_accel_m_s2 = r.i(10)? as i16;
541    let z_accel_m_s2 = r.i(10)? as i16;
542    let a_gf0_s = r.i(12)? as i16;
543    let a_gf1_s_s = r.i(8)? as i16;
544    Ok(SbasGeoNav {
545        preamble,
546        time_of_day_s,
547        ura,
548        x_m,
549        y_m,
550        z_m,
551        x_rate_m_s,
552        y_rate_m_s,
553        z_rate_m_s,
554        x_accel_m_s2,
555        y_accel_m_s2,
556        z_accel_m_s2,
557        a_gf0_s,
558        a_gf1_s_s,
559        reserved,
560    })
561}
562
563fn encode_geo_nav(m: &SbasGeoNav) -> Vec<u8> {
564    let mut w = CountedBitWriter::new();
565    let spare_start = push_prefix_spare(&mut w, &m.reserved, 8);
566    w.push_u(u64::from(m.time_of_day_s), 13);
567    w.push_u(u64::from(m.ura), 4);
568    w.push_i(i64::from(m.x_m), 30);
569    w.push_i(i64::from(m.y_m), 30);
570    w.push_i(i64::from(m.z_m), 25);
571    w.push_i(i64::from(m.x_rate_m_s), 17);
572    w.push_i(i64::from(m.y_rate_m_s), 17);
573    w.push_i(i64::from(m.z_rate_m_s), 18);
574    w.push_i(i64::from(m.x_accel_m_s2), 10);
575    w.push_i(i64::from(m.y_accel_m_s2), 10);
576    w.push_i(i64::from(m.z_accel_m_s2), 10);
577    w.push_i(i64::from(m.a_gf0_s), 12);
578    w.push_i(i64::from(m.a_gf1_s_s), 8);
579    push_spares_from(&mut w, &m.reserved, spare_start);
580    w.pad_to(DATA_BITS);
581    w.into_bytes()
582}
583
584fn decode_igp_mask(preamble: u8, data: &[u8]) -> Result<SbasIgpMask> {
585    let mut r = BitReader::new(data);
586    let mut reserved = SpareBits::new();
587    reserved.push(r.u(4)?, 4);
588    let band_number = r.u(4)? as u8;
589    let iodi = r.u(2)? as u8;
590    let mut mask = [false; 201];
591    for bit in &mut mask {
592        *bit = r.flag()?;
593    }
594    if r.remaining_bits() >= 1 {
595        reserved.push(r.u(1)?, 1);
596    }
597    Ok(SbasIgpMask {
598        preamble,
599        band_number,
600        iodi,
601        mask,
602        reserved,
603    })
604}
605
606fn encode_igp_mask(m: &SbasIgpMask) -> Vec<u8> {
607    let mut w = CountedBitWriter::new();
608    let spare_start = push_prefix_spare(&mut w, &m.reserved, 4);
609    w.push_u(u64::from(m.band_number), 4);
610    w.push_u(u64::from(m.iodi), 2);
611    for bit in m.mask {
612        w.push_flag(bit);
613    }
614    push_spares_from(&mut w, &m.reserved, spare_start);
615    w.pad_to(DATA_BITS);
616    w.into_bytes()
617}
618
619fn decode_mixed(preamble: u8, data: &[u8]) -> Result<SbasMixedCorrections> {
620    let mut r = BitReader::new(data);
621    let mut prc = [0i16; 6];
622    for value in &mut prc {
623        *value = r.i(12)? as i16;
624    }
625    let mut udrei = [0u8; 6];
626    for value in &mut udrei {
627        *value = r.u(4)? as u8;
628    }
629    let iodp = r.u(2)? as u8;
630    let block_id = r.u(2)? as u8;
631    let iodf = r.u(2)? as u8;
632    let mut reserved = SpareBits::new();
633    reserved.push(r.u(4)?, 4);
634    let long_raw = read_bits_as_bytes(&mut r, 106)?;
635    let long_term = decode_long_half(&long_raw)?;
636    Ok(SbasMixedCorrections {
637        preamble,
638        fast: SbasMixedFastCorrections {
639            iodf,
640            iodp,
641            block_id,
642            prc,
643            udrei,
644            reserved,
645        },
646        long_term,
647    })
648}
649
650fn encode_mixed(m: &SbasMixedCorrections) -> Vec<u8> {
651    let mut w = CountedBitWriter::new();
652    for value in m.fast.prc {
653        w.push_i(i64::from(value), 12);
654    }
655    for value in m.fast.udrei {
656        w.push_u(u64::from(value), 4);
657    }
658    w.push_u(u64::from(m.fast.iodp), 2);
659    w.push_u(u64::from(m.fast.block_id), 2);
660    w.push_u(u64::from(m.fast.iodf), 2);
661    push_spares(&mut w, &m.fast.reserved);
662    w.pad_to(106);
663    let long = encode_long_half(&m.long_term);
664    w.push_bits_from(&long, 106);
665    w.pad_to(DATA_BITS);
666    w.into_bytes()
667}
668
669fn decode_long_term(preamble: u8, data: &[u8]) -> Result<SbasLongTermCorrections> {
670    let mut r = BitReader::new(data);
671    let first = read_bits_as_bytes(&mut r, 106)?;
672    let second = read_bits_as_bytes(&mut r, 106)?;
673    Ok(SbasLongTermCorrections {
674        preamble,
675        halves: [decode_long_half(&first)?, decode_long_half(&second)?],
676    })
677}
678
679fn encode_long_term(m: &SbasLongTermCorrections) -> Vec<u8> {
680    let mut w = CountedBitWriter::new();
681    for half in &m.halves {
682        let encoded = encode_long_half(half);
683        w.push_bits_from(&encoded, 106);
684    }
685    w.pad_to(DATA_BITS);
686    w.into_bytes()
687}
688
689fn decode_long_half(raw: &[u8]) -> Result<SbasLongTermHalf> {
690    let mut r = BitReader::new(raw);
691    let velocity_code = r.flag()?;
692    let mut records = Vec::new();
693    let mut reserved = SpareBits::new();
694    if velocity_code {
695        records.push(SbasLongTermRecord {
696            monitored_index: r.u(6)? as u8,
697            iode: r.u(8)? as u8,
698            delta_x: r.i(11)? as i32,
699            delta_y: r.i(11)? as i32,
700            delta_z: r.i(11)? as i32,
701            delta_a_f0: r.i(11)? as i32,
702            delta_x_rate: r.i(8)? as i32,
703            delta_y_rate: r.i(8)? as i32,
704            delta_z_rate: r.i(8)? as i32,
705            delta_a_f1: r.i(8)? as i32,
706            time_of_day_s: Some(r.u(13)? as u32),
707        });
708        let iodp = r.u(2)? as u8;
709        Ok(SbasLongTermHalf {
710            velocity_code,
711            iodp,
712            records,
713            reserved,
714        })
715    } else {
716        for _ in 0..2 {
717            records.push(SbasLongTermRecord {
718                monitored_index: r.u(6)? as u8,
719                iode: r.u(8)? as u8,
720                delta_x: r.i(9)? as i32,
721                delta_y: r.i(9)? as i32,
722                delta_z: r.i(9)? as i32,
723                delta_x_rate: 0,
724                delta_y_rate: 0,
725                delta_z_rate: 0,
726                delta_a_f0: r.i(10)? as i32,
727                delta_a_f1: 0,
728                time_of_day_s: None,
729            });
730        }
731        let iodp = r.u(2)? as u8;
732        if r.remaining_bits() >= 1 {
733            reserved.push(r.u(1)?, 1);
734        }
735        Ok(SbasLongTermHalf {
736            velocity_code,
737            iodp,
738            records,
739            reserved,
740        })
741    }
742}
743
744fn encode_long_half(m: &SbasLongTermHalf) -> Vec<u8> {
745    let mut w = CountedBitWriter::new();
746    w.push_flag(m.velocity_code);
747    if m.velocity_code {
748        if let Some(record) = m.records.first() {
749            push_long_record_with_velocity(&mut w, record);
750        } else {
751            push_long_record_with_velocity(&mut w, &zero_long_record());
752        }
753        w.push_u(u64::from(m.iodp), 2);
754    } else {
755        for idx in 0..2 {
756            if let Some(record) = m.records.get(idx) {
757                push_long_record_without_velocity(&mut w, record);
758            } else {
759                push_long_record_without_velocity(&mut w, &zero_long_record());
760            }
761        }
762        w.push_u(u64::from(m.iodp), 2);
763        push_spares(&mut w, &m.reserved);
764    }
765    w.pad_to(106);
766    w.into_bytes()
767}
768
769fn push_long_record_without_velocity(w: &mut CountedBitWriter, record: &SbasLongTermRecord) {
770    w.push_u(u64::from(record.monitored_index), 6);
771    w.push_u(u64::from(record.iode), 8);
772    w.push_i(i64::from(record.delta_x), 9);
773    w.push_i(i64::from(record.delta_y), 9);
774    w.push_i(i64::from(record.delta_z), 9);
775    w.push_i(i64::from(record.delta_a_f0), 10);
776}
777
778fn push_long_record_with_velocity(w: &mut CountedBitWriter, record: &SbasLongTermRecord) {
779    w.push_u(u64::from(record.monitored_index), 6);
780    w.push_u(u64::from(record.iode), 8);
781    w.push_i(i64::from(record.delta_x), 11);
782    w.push_i(i64::from(record.delta_y), 11);
783    w.push_i(i64::from(record.delta_z), 11);
784    w.push_i(i64::from(record.delta_a_f0), 11);
785    w.push_i(i64::from(record.delta_x_rate), 8);
786    w.push_i(i64::from(record.delta_y_rate), 8);
787    w.push_i(i64::from(record.delta_z_rate), 8);
788    w.push_i(i64::from(record.delta_a_f1), 8);
789    w.push_u(u64::from(record.time_of_day_s.unwrap_or(0)), 13);
790}
791
792fn zero_long_record() -> SbasLongTermRecord {
793    SbasLongTermRecord {
794        monitored_index: 0,
795        iode: 0,
796        delta_x: 0,
797        delta_y: 0,
798        delta_z: 0,
799        delta_x_rate: 0,
800        delta_y_rate: 0,
801        delta_z_rate: 0,
802        delta_a_f0: 0,
803        delta_a_f1: 0,
804        time_of_day_s: None,
805    }
806}
807
808fn decode_iono_delays(preamble: u8, data: &[u8]) -> Result<SbasIonoDelays> {
809    let mut r = BitReader::new(data);
810    let band_number = r.u(4)? as u8;
811    let block_id = r.u(4)? as u8;
812    let mut entries: [SbasIgpDelay; 15] = core::array::from_fn(|_| SbasIgpDelay::default());
813    for entry in &mut entries {
814        entry.vertical_delay = r.u(9)? as u16;
815        entry.givei = r.u(4)? as u8;
816    }
817    let iodi = r.u(2)? as u8;
818    let mut reserved = SpareBits::new();
819    if r.remaining_bits() >= 7 {
820        reserved.push(r.u(7)?, 7);
821    }
822    Ok(SbasIonoDelays {
823        preamble,
824        band_number,
825        block_id,
826        iodi,
827        entries,
828        reserved,
829    })
830}
831
832fn encode_iono_delays(m: &SbasIonoDelays) -> Vec<u8> {
833    let mut w = CountedBitWriter::new();
834    w.push_u(u64::from(m.band_number), 4);
835    w.push_u(u64::from(m.block_id), 4);
836    for entry in &m.entries {
837        w.push_u(u64::from(entry.vertical_delay), 9);
838        w.push_u(u64::from(entry.givei), 4);
839    }
840    w.push_u(u64::from(m.iodi), 2);
841    push_spares(&mut w, &m.reserved);
842    w.pad_to(DATA_BITS);
843    w.into_bytes()
844}
845
846fn push_spares(w: &mut CountedBitWriter, spare: &SpareBits) {
847    for &(value, width) in &spare.0 {
848        w.push_u(value, usize::from(width));
849    }
850}
851
852fn push_prefix_spare(w: &mut CountedBitWriter, spare: &SpareBits, width: u8) -> usize {
853    if let Some(&(value, got_width)) = spare
854        .0
855        .first()
856        .filter(|&&(_, got_width)| got_width == width)
857    {
858        w.push_u(value, usize::from(got_width));
859        1
860    } else {
861        w.push_u(0, usize::from(width));
862        0
863    }
864}
865
866fn push_spares_from(w: &mut CountedBitWriter, spare: &SpareBits, start: usize) {
867    for &(value, width) in spare.0.iter().skip(start) {
868        w.push_u(value, usize::from(width));
869    }
870}
871
872fn read_bits_as_bytes(reader: &mut BitReader<'_>, nbits: usize) -> Result<Vec<u8>> {
873    let mut w = CountedBitWriter::new();
874    for _ in 0..nbits {
875        w.push_u(reader.u(1)?, 1);
876    }
877    Ok(w.into_bytes())
878}
879
880fn data_from_raw(raw: &[u8]) -> Vec<u8> {
881    let mut w = CountedBitWriter::new();
882    w.push_bits_from(raw, DATA_BITS.min(raw.len() * 8));
883    w.pad_to(DATA_BITS);
884    w.into_bytes()
885}
886
887fn bit_at(bytes: &[u8], bit_pos: usize) -> u8 {
888    (bytes[bit_pos / 8] >> (7 - (bit_pos % 8))) & 1
889}
890
891fn bits_as_u32(bytes: &[u8], bit_pos: usize, nbits: usize) -> u32 {
892    let mut value = 0u32;
893    for pos in bit_pos..bit_pos + nbits {
894        value = (value << 1) | u32::from(bit_at(bytes, pos));
895    }
896    value
897}
898
899fn parse_error(message: &str) -> Error {
900    Error::Parse(message.to_string())
901}
902
903#[cfg(test)]
904mod tests {
905    use super::*;
906    use crate::rtcm::crc::crc24q;
907
908    fn block(message: SbasMessage, form: SbasWireForm) -> Vec<u8> {
909        SbasBlock { form, message }.encode()
910    }
911
912    fn bits_i(bytes: &[u8], bit_pos: usize, nbits: usize) -> i32 {
913        let raw = bits_as_u32(bytes, bit_pos, nbits);
914        let sign_bit = 1u32 << (nbits - 1);
915        if raw & sign_bit != 0 {
916            raw as i32 - (1i32 << nbits)
917        } else {
918            raw as i32
919        }
920    }
921
922    #[test]
923    fn crc_bits_matches_byte_crc_when_aligned() {
924        let data = [0x53, 0x01, 0x23, 0x45, 0x67, 0x89];
925        assert_eq!(crc24q_bits(&data, data.len() * 8), crc24q(&data));
926    }
927
928    #[test]
929    fn prn_mask_decodes_and_round_trips_in_both_forms() {
930        let mut mask = [false; 210];
931        mask[0] = true;
932        mask[119] = true;
933        mask[157] = true;
934        let msg = SbasMessage::PrnMask(SbasPrnMask {
935            preamble: 0x53,
936            iodp: 2,
937            mask,
938            reserved: SpareBits::new(),
939        });
940        for form in [SbasWireForm::Body226, SbasWireForm::Framed250] {
941            let encoded = block(msg.clone(), form);
942            let decoded = SbasBlock::decode(&encoded, form).expect("valid SBAS block");
943            assert_eq!(decoded.encode(), encoded);
944            assert_eq!(decoded.message.message_type(), 1);
945        }
946    }
947
948    #[test]
949    fn corrupted_crc_is_rejected() {
950        let msg = SbasMessage::Unsupported(SbasUnsupported {
951            preamble: 0x9A,
952            message_type: 62,
953            data: vec![0; BODY_LEN],
954        });
955        let mut encoded = block(msg, SbasWireForm::Framed250);
956        encoded[31] ^= 0x40;
957        assert!(SbasBlock::decode(&encoded, SbasWireForm::Framed250).is_err());
958    }
959
960    #[test]
961    fn phase_a_message_classification() {
962        for mt in [0, 1, 2, 3, 4, 5, 6, 7, 9, 12, 17, 18, 24, 25, 26] {
963            assert!(is_phase_a_message(mt));
964        }
965        assert!(!is_phase_a_message(10));
966        assert!(!is_phase_a_message(63));
967    }
968
969    #[test]
970    fn mt24_encode_uses_rtklib_offsets() {
971        let body = block(
972            SbasMessage::MixedCorrections(SbasMixedCorrections {
973                preamble: 0x53,
974                fast: SbasMixedFastCorrections {
975                    iodf: 1,
976                    iodp: 2,
977                    block_id: 3,
978                    prc: [-1, 2, -3, 4, -5, 6],
979                    udrei: [1, 2, 3, 4, 5, 6],
980                    reserved: SpareBits(vec![(0b1010, 4)]),
981                },
982                long_term: SbasLongTermHalf {
983                    velocity_code: false,
984                    iodp: 2,
985                    records: vec![
986                        SbasLongTermRecord {
987                            monitored_index: 5,
988                            iode: 9,
989                            delta_x: -10,
990                            delta_y: 11,
991                            delta_z: -12,
992                            delta_x_rate: 0,
993                            delta_y_rate: 0,
994                            delta_z_rate: 0,
995                            delta_a_f0: 13,
996                            delta_a_f1: 0,
997                            time_of_day_s: None,
998                        },
999                        SbasLongTermRecord {
1000                            monitored_index: 6,
1001                            iode: 10,
1002                            delta_x: 14,
1003                            delta_y: -15,
1004                            delta_z: 16,
1005                            delta_x_rate: 0,
1006                            delta_y_rate: 0,
1007                            delta_z_rate: 0,
1008                            delta_a_f0: -17,
1009                            delta_a_f1: 0,
1010                            time_of_day_s: None,
1011                        },
1012                    ],
1013                    reserved: SpareBits(vec![(1, 1)]),
1014                },
1015            }),
1016            SbasWireForm::Body226,
1017        );
1018
1019        assert_eq!(bits_as_u32(&body, 8, 6), 24);
1020        assert_eq!(bits_i(&body, 14, 12), -1);
1021        assert_eq!(bits_i(&body, 26, 12), 2);
1022        assert_eq!(bits_i(&body, 38, 12), -3);
1023        assert_eq!(bits_i(&body, 50, 12), 4);
1024        assert_eq!(bits_i(&body, 62, 12), -5);
1025        assert_eq!(bits_i(&body, 74, 12), 6);
1026        assert_eq!(bits_as_u32(&body, 86, 4), 1);
1027        assert_eq!(bits_as_u32(&body, 90, 4), 2);
1028        assert_eq!(bits_as_u32(&body, 94, 4), 3);
1029        assert_eq!(bits_as_u32(&body, 98, 4), 4);
1030        assert_eq!(bits_as_u32(&body, 102, 4), 5);
1031        assert_eq!(bits_as_u32(&body, 106, 4), 6);
1032        assert_eq!(bits_as_u32(&body, 110, 2), 2);
1033        assert_eq!(bits_as_u32(&body, 112, 2), 3);
1034        assert_eq!(bits_as_u32(&body, 114, 2), 1);
1035        assert_eq!(bits_as_u32(&body, 116, 4), 0b1010);
1036        assert_eq!(bits_as_u32(&body, 120, 1), 0);
1037        assert_eq!(bits_as_u32(&body, 121, 6), 5);
1038        assert_eq!(bits_as_u32(&body, 172, 6), 6);
1039        assert_eq!(bits_as_u32(&body, 223, 2), 2);
1040        assert_eq!(bits_as_u32(&body, 225, 1), 1);
1041    }
1042}
sidereon_core/sbas/message.rs

sidereon_core/sbas/
message.rs
