use crate::error::{Error, Result};
use crate::rtcm::bits::{BitReader, BitWriter};
use crate::rtcm::crc::crc24q_bits;
pub type SbasMessageType = u8;
const FRAMED_LEN: usize = 32;
const BODY_LEN: usize = 29;
const HEADER_BITS: usize = 14;
const DATA_BITS: usize = 212;
const BODY_BITS: usize = HEADER_BITS + DATA_BITS;
const CRC_BITS: usize = 24;
const FRAMED_BITS: usize = BODY_BITS + CRC_BITS;
const PREAMBLES: [u8; 3] = [0x53, 0x9A, 0xC6];
#[derive(Clone, Debug, PartialEq, Eq, Default)]
pub struct SpareBits(pub Vec<(u64, u8)>);
impl SpareBits {
pub fn new() -> Self {
Self(Vec::new())
}
pub fn push(&mut self, value: u64, width: u8) {
self.0.push((value, width));
}
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub enum SbasMessage {
DoNotUse(SbasDoNotUse),
PrnMask(SbasPrnMask),
FastCorrections(SbasFastCorrections),
Integrity(SbasIntegrity),
FastDegradation(SbasFastDegradation),
GeoNav(SbasGeoNav),
NetworkTime(SbasNetworkTime),
GeoAlmanac(SbasGeoAlmanac),
IgpMask(SbasIgpMask),
MixedCorrections(SbasMixedCorrections),
LongTermCorrections(SbasLongTermCorrections),
IonoDelays(SbasIonoDelays),
Unsupported(SbasUnsupported),
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasDoNotUse {
pub preamble: u8,
pub data: Vec<u8>,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasPrnMask {
pub preamble: u8,
pub iodp: u8,
pub mask: [bool; 210],
pub reserved: SpareBits,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasFastCorrections {
pub preamble: u8,
pub message_type: SbasMessageType,
pub iodf: u8,
pub iodp: u8,
pub prc: [i16; 13],
pub udrei: [u8; 13],
pub reserved: SpareBits,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasIntegrity {
pub preamble: u8,
pub iodf: [u8; 4],
pub udrei: [u8; 51],
pub reserved: SpareBits,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasFastDegradation {
pub preamble: u8,
pub system_latency_s: u8,
pub iodp: u8,
pub ai: [u8; 51],
pub reserved: SpareBits,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasGeoNav {
pub preamble: u8,
pub time_of_day_s: u16,
pub ura: u8,
pub x_m: i32,
pub y_m: i32,
pub z_m: i32,
pub x_rate_m_s: i32,
pub y_rate_m_s: i32,
pub z_rate_m_s: i32,
pub x_accel_m_s2: i16,
pub y_accel_m_s2: i16,
pub z_accel_m_s2: i16,
pub a_gf0_s: i16,
pub a_gf1_s_s: i16,
pub reserved: SpareBits,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasNetworkTime {
pub preamble: u8,
pub data: Vec<u8>,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasGeoAlmanac {
pub preamble: u8,
pub data: Vec<u8>,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasMixedCorrections {
pub preamble: u8,
pub fast: SbasMixedFastCorrections,
pub long_term: SbasLongTermHalf,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasMixedFastCorrections {
pub iodf: u8,
pub iodp: u8,
pub block_id: u8,
pub prc: [i16; 6],
pub udrei: [u8; 6],
pub reserved: SpareBits,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasLongTermCorrections {
pub preamble: u8,
pub halves: [SbasLongTermHalf; 2],
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasLongTermHalf {
pub velocity_code: bool,
pub iodp: u8,
pub records: Vec<SbasLongTermRecord>,
pub reserved: SpareBits,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasLongTermRecord {
pub monitored_index: u8,
pub iode: u8,
pub delta_x: i32,
pub delta_y: i32,
pub delta_z: i32,
pub delta_x_rate: i32,
pub delta_y_rate: i32,
pub delta_z_rate: i32,
pub delta_a_f0: i32,
pub delta_a_f1: i32,
pub time_of_day_s: Option<u32>,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasIgpMask {
pub preamble: u8,
pub band_number: u8,
pub iodi: u8,
pub mask: [bool; 201],
pub reserved: SpareBits,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasIonoDelays {
pub preamble: u8,
pub band_number: u8,
pub block_id: u8,
pub iodi: u8,
pub entries: [SbasIgpDelay; 15],
pub reserved: SpareBits,
}
#[derive(Clone, Debug, PartialEq, Eq, Default)]
pub struct SbasIgpDelay {
pub vertical_delay: u16,
pub givei: u8,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasUnsupported {
pub preamble: u8,
pub message_type: SbasMessageType,
pub data: Vec<u8>,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum SbasWireForm {
Framed250,
Body226,
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct SbasBlock {
pub form: SbasWireForm,
pub message: SbasMessage,
}
struct CountedBitWriter {
writer: BitWriter,
nbits: usize,
}
impl CountedBitWriter {
fn new() -> Self {
Self {
writer: BitWriter::new(),
nbits: 0,
}
}
fn push_u(&mut self, value: u64, width: usize) {
self.writer.push_u(value, width);
self.nbits += width;
}
fn push_i(&mut self, value: i64, width: usize) {
self.writer.push_i(value, width);
self.nbits += width;
}
fn push_flag(&mut self, value: bool) {
self.writer.push_flag(value);
self.nbits += 1;
}
fn push_bits_from(&mut self, bytes: &[u8], nbits: usize) {
for bit_pos in 0..nbits {
self.push_u(u64::from(bit_at(bytes, bit_pos)), 1);
}
}
fn pad_to(&mut self, nbits: usize) {
while self.nbits < nbits {
self.push_u(0, 1);
}
}
fn into_bytes(self) -> Vec<u8> {
self.writer.into_bytes()
}
}
impl SbasBlock {
pub fn decode(bytes: &[u8], form: SbasWireForm) -> Result<Self> {
match form {
SbasWireForm::Framed250 => {
if bytes.len() != FRAMED_LEN {
return Err(parse_error("SBAS framed block must be 32 bytes"));
}
let got = bits_as_u32(bytes, BODY_BITS, CRC_BITS);
let want = crc24q_bits(bytes, BODY_BITS);
if got != want {
return Err(parse_error("SBAS CRC mismatch"));
}
}
SbasWireForm::Body226 => {
if bytes.len() != BODY_LEN {
return Err(parse_error("SBAS body block must be 29 bytes"));
}
}
}
let mut reader = BitReader::new(bytes);
let preamble = reader.u(8)? as u8;
if !PREAMBLES.contains(&preamble) {
return Err(parse_error("SBAS preamble not recognized"));
}
let message_type = reader.u(6)? as u8;
let data = read_bits_as_bytes(&mut reader, DATA_BITS)?;
let message = decode_message(preamble, message_type, &data)?;
Ok(Self { form, message })
}
pub fn encode(&self) -> Vec<u8> {
let mut body = CountedBitWriter::new();
body.push_u(u64::from(self.message.preamble()), 8);
body.push_u(u64::from(self.message.message_type()), 6);
let data = self.message.encode_data();
body.push_bits_from(&data, DATA_BITS);
body.pad_to(BODY_BITS);
let body_bytes = body.into_bytes();
match self.form {
SbasWireForm::Body226 => body_bytes,
SbasWireForm::Framed250 => {
let crc = crc24q_bits(&body_bytes, BODY_BITS);
let mut framed = CountedBitWriter::new();
framed.push_bits_from(&body_bytes, BODY_BITS);
framed.push_u(u64::from(crc), CRC_BITS);
framed.pad_to(FRAMED_BITS);
framed.into_bytes()
}
}
}
}
impl SbasMessage {
pub fn message_type(&self) -> SbasMessageType {
match self {
Self::DoNotUse(_) => 0,
Self::PrnMask(_) => 1,
Self::FastCorrections(m) => m.message_type,
Self::Integrity(_) => 6,
Self::FastDegradation(_) => 7,
Self::GeoNav(_) => 9,
Self::NetworkTime(_) => 12,
Self::GeoAlmanac(_) => 17,
Self::IgpMask(_) => 18,
Self::MixedCorrections(_) => 24,
Self::LongTermCorrections(_) => 25,
Self::IonoDelays(_) => 26,
Self::Unsupported(m) => m.message_type,
}
}
fn preamble(&self) -> u8 {
match self {
Self::DoNotUse(m) => m.preamble,
Self::PrnMask(m) => m.preamble,
Self::FastCorrections(m) => m.preamble,
Self::Integrity(m) => m.preamble,
Self::FastDegradation(m) => m.preamble,
Self::GeoNav(m) => m.preamble,
Self::NetworkTime(m) => m.preamble,
Self::GeoAlmanac(m) => m.preamble,
Self::IgpMask(m) => m.preamble,
Self::MixedCorrections(m) => m.preamble,
Self::LongTermCorrections(m) => m.preamble,
Self::IonoDelays(m) => m.preamble,
Self::Unsupported(m) => m.preamble,
}
}
fn encode_data(&self) -> Vec<u8> {
match self {
Self::DoNotUse(m) => data_from_raw(&m.data),
Self::PrnMask(m) => encode_prn_mask(m),
Self::FastCorrections(m) => encode_fast(m),
Self::Integrity(m) => encode_integrity(m),
Self::FastDegradation(m) => encode_fast_degradation(m),
Self::GeoNav(m) => encode_geo_nav(m),
Self::NetworkTime(m) => data_from_raw(&m.data),
Self::GeoAlmanac(m) => data_from_raw(&m.data),
Self::IgpMask(m) => encode_igp_mask(m),
Self::MixedCorrections(m) => encode_mixed(m),
Self::LongTermCorrections(m) => encode_long_term(m),
Self::IonoDelays(m) => encode_iono_delays(m),
Self::Unsupported(m) => data_from_raw(&m.data),
}
}
}
pub(crate) fn is_phase_a_message(mt: SbasMessageType) -> bool {
matches!(mt, 0..=7 | 9 | 12 | 17 | 18 | 24 | 25 | 26)
}
fn decode_message(preamble: u8, message_type: u8, data: &[u8]) -> Result<SbasMessage> {
if !is_phase_a_message(message_type) {
return Ok(SbasMessage::Unsupported(SbasUnsupported {
preamble,
message_type,
data: data_from_raw(data),
}));
}
match message_type {
0 => Ok(SbasMessage::DoNotUse(SbasDoNotUse {
preamble,
data: data_from_raw(data),
})),
1 => decode_prn_mask(preamble, data).map(SbasMessage::PrnMask),
2..=5 => decode_fast(preamble, message_type, data).map(SbasMessage::FastCorrections),
6 => decode_integrity(preamble, data).map(SbasMessage::Integrity),
7 => decode_fast_degradation(preamble, data).map(SbasMessage::FastDegradation),
9 => decode_geo_nav(preamble, data).map(SbasMessage::GeoNav),
12 => Ok(SbasMessage::NetworkTime(SbasNetworkTime {
preamble,
data: data_from_raw(data),
})),
17 => Ok(SbasMessage::GeoAlmanac(SbasGeoAlmanac {
preamble,
data: data_from_raw(data),
})),
18 => decode_igp_mask(preamble, data).map(SbasMessage::IgpMask),
24 => decode_mixed(preamble, data).map(SbasMessage::MixedCorrections),
25 => decode_long_term(preamble, data).map(SbasMessage::LongTermCorrections),
26 => decode_iono_delays(preamble, data).map(SbasMessage::IonoDelays),
_ => unreachable!("phase A classification and decode match are out of sync"),
}
}
fn decode_prn_mask(preamble: u8, data: &[u8]) -> Result<SbasPrnMask> {
let mut r = BitReader::new(data);
let mut mask = [false; 210];
for bit in &mut mask {
*bit = r.flag()?;
}
let iodp = r.u(2)? as u8;
Ok(SbasPrnMask {
preamble,
iodp,
mask,
reserved: SpareBits::new(),
})
}
fn encode_prn_mask(m: &SbasPrnMask) -> Vec<u8> {
let mut w = CountedBitWriter::new();
for bit in m.mask {
w.push_flag(bit);
}
w.push_u(u64::from(m.iodp), 2);
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn decode_fast(preamble: u8, message_type: u8, data: &[u8]) -> Result<SbasFastCorrections> {
let mut r = BitReader::new(data);
let iodf = r.u(2)? as u8;
let iodp = r.u(2)? as u8;
let mut prc = [0i16; 13];
for value in &mut prc {
*value = r.i(12)? as i16;
}
let mut udrei = [0u8; 13];
for value in &mut udrei {
*value = r.u(4)? as u8;
}
Ok(SbasFastCorrections {
preamble,
message_type,
iodf,
iodp,
prc,
udrei,
reserved: SpareBits::new(),
})
}
fn encode_fast(m: &SbasFastCorrections) -> Vec<u8> {
let mut w = CountedBitWriter::new();
w.push_u(u64::from(m.iodf), 2);
w.push_u(u64::from(m.iodp), 2);
for value in m.prc {
w.push_i(i64::from(value), 12);
}
for value in m.udrei {
w.push_u(u64::from(value), 4);
}
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn decode_integrity(preamble: u8, data: &[u8]) -> Result<SbasIntegrity> {
let mut r = BitReader::new(data);
let mut iodf = [0u8; 4];
for value in &mut iodf {
*value = r.u(2)? as u8;
}
let mut udrei = [0u8; 51];
for value in &mut udrei {
*value = r.u(4)? as u8;
}
Ok(SbasIntegrity {
preamble,
iodf,
udrei,
reserved: SpareBits::new(),
})
}
fn encode_integrity(m: &SbasIntegrity) -> Vec<u8> {
let mut w = CountedBitWriter::new();
for value in m.iodf {
w.push_u(u64::from(value), 2);
}
for value in m.udrei {
w.push_u(u64::from(value), 4);
}
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn decode_fast_degradation(preamble: u8, data: &[u8]) -> Result<SbasFastDegradation> {
let mut r = BitReader::new(data);
let system_latency_s = r.u(4)? as u8;
let iodp = r.u(2)? as u8;
let mut ai = [0u8; 51];
for value in &mut ai {
*value = r.u(4)? as u8;
}
let mut reserved = SpareBits::new();
if r.remaining_bits() >= 2 {
reserved.push(r.u(2)?, 2);
}
Ok(SbasFastDegradation {
preamble,
system_latency_s,
iodp,
ai,
reserved,
})
}
fn encode_fast_degradation(m: &SbasFastDegradation) -> Vec<u8> {
let mut w = CountedBitWriter::new();
w.push_u(u64::from(m.system_latency_s), 4);
w.push_u(u64::from(m.iodp), 2);
for value in m.ai {
w.push_u(u64::from(value), 4);
}
push_spares(&mut w, &m.reserved);
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn decode_geo_nav(preamble: u8, data: &[u8]) -> Result<SbasGeoNav> {
let mut r = BitReader::new(data);
let mut reserved = SpareBits::new();
reserved.push(r.u(8)?, 8);
let time_of_day_s = r.u(13)? as u16;
let ura = r.u(4)? as u8;
let x_m = r.i(30)? as i32;
let y_m = r.i(30)? as i32;
let z_m = r.i(25)? as i32;
let x_rate_m_s = r.i(17)? as i32;
let y_rate_m_s = r.i(17)? as i32;
let z_rate_m_s = r.i(18)? as i32;
let x_accel_m_s2 = r.i(10)? as i16;
let y_accel_m_s2 = r.i(10)? as i16;
let z_accel_m_s2 = r.i(10)? as i16;
let a_gf0_s = r.i(12)? as i16;
let a_gf1_s_s = r.i(8)? as i16;
Ok(SbasGeoNav {
preamble,
time_of_day_s,
ura,
x_m,
y_m,
z_m,
x_rate_m_s,
y_rate_m_s,
z_rate_m_s,
x_accel_m_s2,
y_accel_m_s2,
z_accel_m_s2,
a_gf0_s,
a_gf1_s_s,
reserved,
})
}
fn encode_geo_nav(m: &SbasGeoNav) -> Vec<u8> {
let mut w = CountedBitWriter::new();
let spare_start = push_prefix_spare(&mut w, &m.reserved, 8);
w.push_u(u64::from(m.time_of_day_s), 13);
w.push_u(u64::from(m.ura), 4);
w.push_i(i64::from(m.x_m), 30);
w.push_i(i64::from(m.y_m), 30);
w.push_i(i64::from(m.z_m), 25);
w.push_i(i64::from(m.x_rate_m_s), 17);
w.push_i(i64::from(m.y_rate_m_s), 17);
w.push_i(i64::from(m.z_rate_m_s), 18);
w.push_i(i64::from(m.x_accel_m_s2), 10);
w.push_i(i64::from(m.y_accel_m_s2), 10);
w.push_i(i64::from(m.z_accel_m_s2), 10);
w.push_i(i64::from(m.a_gf0_s), 12);
w.push_i(i64::from(m.a_gf1_s_s), 8);
push_spares_from(&mut w, &m.reserved, spare_start);
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn decode_igp_mask(preamble: u8, data: &[u8]) -> Result<SbasIgpMask> {
let mut r = BitReader::new(data);
let mut reserved = SpareBits::new();
reserved.push(r.u(4)?, 4);
let band_number = r.u(4)? as u8;
let iodi = r.u(2)? as u8;
let mut mask = [false; 201];
for bit in &mut mask {
*bit = r.flag()?;
}
if r.remaining_bits() >= 1 {
reserved.push(r.u(1)?, 1);
}
Ok(SbasIgpMask {
preamble,
band_number,
iodi,
mask,
reserved,
})
}
fn encode_igp_mask(m: &SbasIgpMask) -> Vec<u8> {
let mut w = CountedBitWriter::new();
let spare_start = push_prefix_spare(&mut w, &m.reserved, 4);
w.push_u(u64::from(m.band_number), 4);
w.push_u(u64::from(m.iodi), 2);
for bit in m.mask {
w.push_flag(bit);
}
push_spares_from(&mut w, &m.reserved, spare_start);
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn decode_mixed(preamble: u8, data: &[u8]) -> Result<SbasMixedCorrections> {
let mut r = BitReader::new(data);
let mut prc = [0i16; 6];
for value in &mut prc {
*value = r.i(12)? as i16;
}
let mut udrei = [0u8; 6];
for value in &mut udrei {
*value = r.u(4)? as u8;
}
let iodp = r.u(2)? as u8;
let block_id = r.u(2)? as u8;
let iodf = r.u(2)? as u8;
let mut reserved = SpareBits::new();
reserved.push(r.u(4)?, 4);
let long_raw = read_bits_as_bytes(&mut r, 106)?;
let long_term = decode_long_half(&long_raw)?;
Ok(SbasMixedCorrections {
preamble,
fast: SbasMixedFastCorrections {
iodf,
iodp,
block_id,
prc,
udrei,
reserved,
},
long_term,
})
}
fn encode_mixed(m: &SbasMixedCorrections) -> Vec<u8> {
let mut w = CountedBitWriter::new();
for value in m.fast.prc {
w.push_i(i64::from(value), 12);
}
for value in m.fast.udrei {
w.push_u(u64::from(value), 4);
}
w.push_u(u64::from(m.fast.iodp), 2);
w.push_u(u64::from(m.fast.block_id), 2);
w.push_u(u64::from(m.fast.iodf), 2);
push_spares(&mut w, &m.fast.reserved);
w.pad_to(106);
let long = encode_long_half(&m.long_term);
w.push_bits_from(&long, 106);
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn decode_long_term(preamble: u8, data: &[u8]) -> Result<SbasLongTermCorrections> {
let mut r = BitReader::new(data);
let first = read_bits_as_bytes(&mut r, 106)?;
let second = read_bits_as_bytes(&mut r, 106)?;
Ok(SbasLongTermCorrections {
preamble,
halves: [decode_long_half(&first)?, decode_long_half(&second)?],
})
}
fn encode_long_term(m: &SbasLongTermCorrections) -> Vec<u8> {
let mut w = CountedBitWriter::new();
for half in &m.halves {
let encoded = encode_long_half(half);
w.push_bits_from(&encoded, 106);
}
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn decode_long_half(raw: &[u8]) -> Result<SbasLongTermHalf> {
let mut r = BitReader::new(raw);
let velocity_code = r.flag()?;
let mut records = Vec::new();
let mut reserved = SpareBits::new();
if velocity_code {
records.push(SbasLongTermRecord {
monitored_index: r.u(6)? as u8,
iode: r.u(8)? as u8,
delta_x: r.i(11)? as i32,
delta_y: r.i(11)? as i32,
delta_z: r.i(11)? as i32,
delta_a_f0: r.i(11)? as i32,
delta_x_rate: r.i(8)? as i32,
delta_y_rate: r.i(8)? as i32,
delta_z_rate: r.i(8)? as i32,
delta_a_f1: r.i(8)? as i32,
time_of_day_s: Some(r.u(13)? as u32),
});
let iodp = r.u(2)? as u8;
Ok(SbasLongTermHalf {
velocity_code,
iodp,
records,
reserved,
})
} else {
for _ in 0..2 {
records.push(SbasLongTermRecord {
monitored_index: r.u(6)? as u8,
iode: r.u(8)? as u8,
delta_x: r.i(9)? as i32,
delta_y: r.i(9)? as i32,
delta_z: r.i(9)? as i32,
delta_x_rate: 0,
delta_y_rate: 0,
delta_z_rate: 0,
delta_a_f0: r.i(10)? as i32,
delta_a_f1: 0,
time_of_day_s: None,
});
}
let iodp = r.u(2)? as u8;
if r.remaining_bits() >= 1 {
reserved.push(r.u(1)?, 1);
}
Ok(SbasLongTermHalf {
velocity_code,
iodp,
records,
reserved,
})
}
}
fn encode_long_half(m: &SbasLongTermHalf) -> Vec<u8> {
let mut w = CountedBitWriter::new();
w.push_flag(m.velocity_code);
if m.velocity_code {
if let Some(record) = m.records.first() {
push_long_record_with_velocity(&mut w, record);
} else {
push_long_record_with_velocity(&mut w, &zero_long_record());
}
w.push_u(u64::from(m.iodp), 2);
} else {
for idx in 0..2 {
if let Some(record) = m.records.get(idx) {
push_long_record_without_velocity(&mut w, record);
} else {
push_long_record_without_velocity(&mut w, &zero_long_record());
}
}
w.push_u(u64::from(m.iodp), 2);
push_spares(&mut w, &m.reserved);
}
w.pad_to(106);
w.into_bytes()
}
fn push_long_record_without_velocity(w: &mut CountedBitWriter, record: &SbasLongTermRecord) {
w.push_u(u64::from(record.monitored_index), 6);
w.push_u(u64::from(record.iode), 8);
w.push_i(i64::from(record.delta_x), 9);
w.push_i(i64::from(record.delta_y), 9);
w.push_i(i64::from(record.delta_z), 9);
w.push_i(i64::from(record.delta_a_f0), 10);
}
fn push_long_record_with_velocity(w: &mut CountedBitWriter, record: &SbasLongTermRecord) {
w.push_u(u64::from(record.monitored_index), 6);
w.push_u(u64::from(record.iode), 8);
w.push_i(i64::from(record.delta_x), 11);
w.push_i(i64::from(record.delta_y), 11);
w.push_i(i64::from(record.delta_z), 11);
w.push_i(i64::from(record.delta_a_f0), 11);
w.push_i(i64::from(record.delta_x_rate), 8);
w.push_i(i64::from(record.delta_y_rate), 8);
w.push_i(i64::from(record.delta_z_rate), 8);
w.push_i(i64::from(record.delta_a_f1), 8);
w.push_u(u64::from(record.time_of_day_s.unwrap_or(0)), 13);
}
fn zero_long_record() -> SbasLongTermRecord {
SbasLongTermRecord {
monitored_index: 0,
iode: 0,
delta_x: 0,
delta_y: 0,
delta_z: 0,
delta_x_rate: 0,
delta_y_rate: 0,
delta_z_rate: 0,
delta_a_f0: 0,
delta_a_f1: 0,
time_of_day_s: None,
}
}
fn decode_iono_delays(preamble: u8, data: &[u8]) -> Result<SbasIonoDelays> {
let mut r = BitReader::new(data);
let band_number = r.u(4)? as u8;
let block_id = r.u(4)? as u8;
let mut entries: [SbasIgpDelay; 15] = core::array::from_fn(|_| SbasIgpDelay::default());
for entry in &mut entries {
entry.vertical_delay = r.u(9)? as u16;
entry.givei = r.u(4)? as u8;
}
let iodi = r.u(2)? as u8;
let mut reserved = SpareBits::new();
if r.remaining_bits() >= 7 {
reserved.push(r.u(7)?, 7);
}
Ok(SbasIonoDelays {
preamble,
band_number,
block_id,
iodi,
entries,
reserved,
})
}
fn encode_iono_delays(m: &SbasIonoDelays) -> Vec<u8> {
let mut w = CountedBitWriter::new();
w.push_u(u64::from(m.band_number), 4);
w.push_u(u64::from(m.block_id), 4);
for entry in &m.entries {
w.push_u(u64::from(entry.vertical_delay), 9);
w.push_u(u64::from(entry.givei), 4);
}
w.push_u(u64::from(m.iodi), 2);
push_spares(&mut w, &m.reserved);
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn push_spares(w: &mut CountedBitWriter, spare: &SpareBits) {
for &(value, width) in &spare.0 {
w.push_u(value, usize::from(width));
}
}
fn push_prefix_spare(w: &mut CountedBitWriter, spare: &SpareBits, width: u8) -> usize {
if let Some(&(value, got_width)) = spare
.0
.first()
.filter(|&&(_, got_width)| got_width == width)
{
w.push_u(value, usize::from(got_width));
1
} else {
w.push_u(0, usize::from(width));
0
}
}
fn push_spares_from(w: &mut CountedBitWriter, spare: &SpareBits, start: usize) {
for &(value, width) in spare.0.iter().skip(start) {
w.push_u(value, usize::from(width));
}
}
fn read_bits_as_bytes(reader: &mut BitReader<'_>, nbits: usize) -> Result<Vec<u8>> {
let mut w = CountedBitWriter::new();
for _ in 0..nbits {
w.push_u(reader.u(1)?, 1);
}
Ok(w.into_bytes())
}
fn data_from_raw(raw: &[u8]) -> Vec<u8> {
let mut w = CountedBitWriter::new();
w.push_bits_from(raw, DATA_BITS.min(raw.len() * 8));
w.pad_to(DATA_BITS);
w.into_bytes()
}
fn bit_at(bytes: &[u8], bit_pos: usize) -> u8 {
(bytes[bit_pos / 8] >> (7 - (bit_pos % 8))) & 1
}
fn bits_as_u32(bytes: &[u8], bit_pos: usize, nbits: usize) -> u32 {
let mut value = 0u32;
for pos in bit_pos..bit_pos + nbits {
value = (value << 1) | u32::from(bit_at(bytes, pos));
}
value
}
fn parse_error(message: &str) -> Error {
Error::Parse(message.to_string())
}
#[cfg(test)]
mod tests {
use super::*;
use crate::rtcm::crc::crc24q;
fn block(message: SbasMessage, form: SbasWireForm) -> Vec<u8> {
SbasBlock { form, message }.encode()
}
fn bits_i(bytes: &[u8], bit_pos: usize, nbits: usize) -> i32 {
let raw = bits_as_u32(bytes, bit_pos, nbits);
let sign_bit = 1u32 << (nbits - 1);
if raw & sign_bit != 0 {
raw as i32 - (1i32 << nbits)
} else {
raw as i32
}
}
#[test]
fn crc_bits_matches_byte_crc_when_aligned() {
let data = [0x53, 0x01, 0x23, 0x45, 0x67, 0x89];
assert_eq!(crc24q_bits(&data, data.len() * 8), crc24q(&data));
}
#[test]
fn prn_mask_decodes_and_round_trips_in_both_forms() {
let mut mask = [false; 210];
mask[0] = true;
mask[119] = true;
mask[157] = true;
let msg = SbasMessage::PrnMask(SbasPrnMask {
preamble: 0x53,
iodp: 2,
mask,
reserved: SpareBits::new(),
});
for form in [SbasWireForm::Body226, SbasWireForm::Framed250] {
let encoded = block(msg.clone(), form);
let decoded = SbasBlock::decode(&encoded, form).expect("valid SBAS block");
assert_eq!(decoded.encode(), encoded);
assert_eq!(decoded.message.message_type(), 1);
}
}
#[test]
fn corrupted_crc_is_rejected() {
let msg = SbasMessage::Unsupported(SbasUnsupported {
preamble: 0x9A,
message_type: 62,
data: vec![0; BODY_LEN],
});
let mut encoded = block(msg, SbasWireForm::Framed250);
encoded[31] ^= 0x40;
assert!(SbasBlock::decode(&encoded, SbasWireForm::Framed250).is_err());
}
#[test]
fn phase_a_message_classification() {
for mt in [0, 1, 2, 3, 4, 5, 6, 7, 9, 12, 17, 18, 24, 25, 26] {
assert!(is_phase_a_message(mt));
}
assert!(!is_phase_a_message(10));
assert!(!is_phase_a_message(63));
}
#[test]
fn mt24_encode_uses_rtklib_offsets() {
let body = block(
SbasMessage::MixedCorrections(SbasMixedCorrections {
preamble: 0x53,
fast: SbasMixedFastCorrections {
iodf: 1,
iodp: 2,
block_id: 3,
prc: [-1, 2, -3, 4, -5, 6],
udrei: [1, 2, 3, 4, 5, 6],
reserved: SpareBits(vec![(0b1010, 4)]),
},
long_term: SbasLongTermHalf {
velocity_code: false,
iodp: 2,
records: vec![
SbasLongTermRecord {
monitored_index: 5,
iode: 9,
delta_x: -10,
delta_y: 11,
delta_z: -12,
delta_x_rate: 0,
delta_y_rate: 0,
delta_z_rate: 0,
delta_a_f0: 13,
delta_a_f1: 0,
time_of_day_s: None,
},
SbasLongTermRecord {
monitored_index: 6,
iode: 10,
delta_x: 14,
delta_y: -15,
delta_z: 16,
delta_x_rate: 0,
delta_y_rate: 0,
delta_z_rate: 0,
delta_a_f0: -17,
delta_a_f1: 0,
time_of_day_s: None,
},
],
reserved: SpareBits(vec![(1, 1)]),
},
}),
SbasWireForm::Body226,
);
assert_eq!(bits_as_u32(&body, 8, 6), 24);
assert_eq!(bits_i(&body, 14, 12), -1);
assert_eq!(bits_i(&body, 26, 12), 2);
assert_eq!(bits_i(&body, 38, 12), -3);
assert_eq!(bits_i(&body, 50, 12), 4);
assert_eq!(bits_i(&body, 62, 12), -5);
assert_eq!(bits_i(&body, 74, 12), 6);
assert_eq!(bits_as_u32(&body, 86, 4), 1);
assert_eq!(bits_as_u32(&body, 90, 4), 2);
assert_eq!(bits_as_u32(&body, 94, 4), 3);
assert_eq!(bits_as_u32(&body, 98, 4), 4);
assert_eq!(bits_as_u32(&body, 102, 4), 5);
assert_eq!(bits_as_u32(&body, 106, 4), 6);
assert_eq!(bits_as_u32(&body, 110, 2), 2);
assert_eq!(bits_as_u32(&body, 112, 2), 3);
assert_eq!(bits_as_u32(&body, 114, 2), 1);
assert_eq!(bits_as_u32(&body, 116, 4), 0b1010);
assert_eq!(bits_as_u32(&body, 120, 1), 0);
assert_eq!(bits_as_u32(&body, 121, 6), 5);
assert_eq!(bits_as_u32(&body, 172, 6), 6);
assert_eq!(bits_as_u32(&body, 223, 2), 2);
assert_eq!(bits_as_u32(&body, 225, 1), 1);
}
}