use std::collections::HashMap;
use crate::error::{SbfError, SbfResult};
use crate::types::{SatelliteId, SignalType};
use super::{
Meas3Cn0HiResBlock, Meas3DopplerBlock, Meas3MpBlock, Meas3PpBlock, Meas3RangesBlock, SbfBlock,
};
const C84: f64 = 299_792_458.0;
const E5_FREQ: f64 = 1_191.795e6;
const E5A_FREQ: f64 = 1_176.45e6;
const E5B_FREQ: f64 = 1_207.14e6;
const E6_FREQ: f64 = 1_278.75e6;
const L1_FREQ: f64 = 1_575.42e6;
const L2_FREQ: f64 = 1_227.60e6;
const L5_FREQ: f64 = 1_176.45e6;
const E2_FREQ: f64 = 1_561.098e6;
const L1_GLO_FREQ: f64 = 1_602.00e6;
const L2_GLO_FREQ: f64 = 1_246.00e6;
const L3_GLO_FREQ: f64 = 1_202.025e6;
const B3_FREQ: f64 = 1_268.52e6;
const S1_FREQ: f64 = 2_492.028e6;
const L1_WAVELENGTH: f64 = C84 / L1_FREQ;
const L2_WAVELENGTH: f64 = C84 / L2_FREQ;
const L3_WAVELENGTH: f64 = C84 / L3_GLO_FREQ;
const L5_WAVELENGTH: f64 = C84 / L5_FREQ;
const E2_WAVELENGTH: f64 = C84 / E2_FREQ;
const E5_WAVELENGTH: f64 = C84 / E5_FREQ;
const E5A_WAVELENGTH: f64 = C84 / E5A_FREQ;
const E5B_WAVELENGTH: f64 = C84 / E5B_FREQ;
const E6_WAVELENGTH: f64 = C84 / E6_FREQ;
const B3_WAVELENGTH: f64 = C84 / B3_FREQ;
const S1_WAVELENGTH: f64 = C84 / S1_FREQ;
const F64_NOTVALID: f64 = -2e10;
const F32_NOTVALID: f32 = -2e10;
const MEAS3_SIG_MAX: usize = 16;
const MEAS3_SAT_MAX: usize = 64;
const MAX_ANTENNAS: usize = 3;
const MEASFLAG_SMOOTHING: u8 = 1 << 0;
const MEASFLAG_HALFCYCLEAMBIGUITY: u8 = 1 << 2;
const MEASFLAG_FIRSTMEAS: u8 = 1 << 3;
const MEASFLAG_APMEINSYNC: u8 = 1 << 5;
const MEASFLAG_VALIDITY: u8 = 1 << 7;
const PR_BASE_M: [f64; 7] = [19e6, 19e6, 22e6, 20e6, 34e6, 34e6, 34e6];
const LOCK_INDICATOR_TO_MS: [u32; 16] = [
0, 60_000, 30_000, 15_000, 10_000, 5_000, 2_000, 1_000, 500, 200, 100, 50, 40, 20, 10, 0,
];
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
enum Meas3SatSystem {
Gps = 0,
Glo = 1,
Gal = 2,
Bds = 3,
Sbas = 4,
Qzs = 5,
Irn = 6,
}
impl Meas3SatSystem {
fn from_index(index: usize) -> Option<Self> {
match index {
0 => Some(Self::Gps),
1 => Some(Self::Glo),
2 => Some(Self::Gal),
3 => Some(Self::Bds),
4 => Some(Self::Sbas),
5 => Some(Self::Qzs),
6 => Some(Self::Irn),
_ => None,
}
}
fn base_svid(self) -> u8 {
match self {
Self::Gps => 1,
Self::Glo => 38,
Self::Gal => 71,
Self::Bds => 141,
Self::Sbas => 120,
Self::Qzs => 181,
Self::Irn => 191,
}
}
}
#[derive(Debug, Clone)]
struct InternalMeasurement {
signal_index: u8,
signal_type: SignalType,
flags: u8,
pr_m: f64,
l_cycles: f64,
doppler_hz: f32,
cn0_dbhz: f32,
raw_cn0_dbhz: u8,
pll_timer_ms: u32,
mp_mm: i16,
carrier_mp_1_512c: i8,
smoothing_corr_mm: i16,
lock_count: u8,
}
impl Default for InternalMeasurement {
fn default() -> Self {
Self {
signal_index: 0,
signal_type: SignalType::Other(u8::MAX),
flags: 0,
pr_m: F64_NOTVALID,
l_cycles: F64_NOTVALID,
doppler_hz: F32_NOTVALID,
cn0_dbhz: F32_NOTVALID,
raw_cn0_dbhz: 0,
pll_timer_ms: 0,
mp_mm: 0,
carrier_mp_1_512c: 0,
smoothing_corr_mm: 0,
lock_count: 0,
}
}
}
impl InternalMeasurement {
fn into_public(self) -> Meas3Measurement {
Meas3Measurement {
signal_index: self.signal_index,
signal_type: self.signal_type,
flags: self.flags,
pseudorange_m: if self.flags & MEASFLAG_VALIDITY != 0 && self.pr_m != F64_NOTVALID {
Some(self.pr_m)
} else {
None
},
carrier_phase_cycles: if self.flags & MEASFLAG_VALIDITY != 0
&& self.l_cycles != F64_NOTVALID
{
Some(self.l_cycles)
} else {
None
},
doppler_hz: if self.flags & MEASFLAG_VALIDITY != 0 && self.doppler_hz != F32_NOTVALID {
Some(self.doppler_hz)
} else {
None
},
cn0_dbhz: if self.flags & MEASFLAG_VALIDITY != 0 && self.cn0_dbhz != F32_NOTVALID {
Some(self.cn0_dbhz)
} else {
None
},
raw_cn0_dbhz: if self.flags & MEASFLAG_VALIDITY != 0 {
Some(self.raw_cn0_dbhz)
} else {
None
},
lock_time_ms: if self.flags & MEASFLAG_VALIDITY != 0 && self.pll_timer_ms != 0 {
Some(self.pll_timer_ms)
} else {
None
},
multipath_mm: self.mp_mm,
carrier_multipath_1_512c: self.carrier_mp_1_512c,
smoothing_correction_mm: self.smoothing_corr_mm,
lock_count: if self.lock_count == 0 {
None
} else {
Some(self.lock_count)
},
}
}
}
#[derive(Debug, Clone, Default)]
struct ReferenceSatellite {
sig_order: Vec<u8>,
measurements: HashMap<u8, InternalMeasurement>,
slave_sig_mask: u32,
pr_rate_64mm_s: i16,
}
#[derive(Debug, Clone, Default)]
struct ReferenceConstellation {
m3satdata_copy: Vec<u8>,
satellites: HashMap<u8, ReferenceSatellite>,
}
#[derive(Debug, Clone, Default)]
struct AntennaReferenceEpoch {
tow_ms: Option<u32>,
constellations: HashMap<Meas3SatSystem, ReferenceConstellation>,
}
#[derive(Debug, Clone, Copy)]
struct MasterDecodeContext<'a> {
signal_table: &'a [SignalType; MEAS3_SIG_MAX],
glonass_fn: i8,
short_pr_base_m: f64,
sig_idx_master_short: u8,
reference_sat: &'a ReferenceSatellite,
time_since_ref_epoch_ms: u32,
pr_rate_available: bool,
}
#[derive(Debug, Clone, Copy)]
struct SlaveDecodeContext<'a> {
signal_table: &'a [SignalType; MEAS3_SIG_MAX],
sig_idx: u8,
glonass_fn: i8,
master: &'a InternalMeasurement,
master_sig_idx: u8,
master_ref: Option<&'a InternalMeasurement>,
slave_ref: Option<&'a InternalMeasurement>,
}
#[derive(Debug, Clone)]
pub struct Meas3Measurement {
pub signal_index: u8,
pub signal_type: SignalType,
flags: u8,
pseudorange_m: Option<f64>,
carrier_phase_cycles: Option<f64>,
doppler_hz: Option<f32>,
cn0_dbhz: Option<f32>,
raw_cn0_dbhz: Option<u8>,
lock_time_ms: Option<u32>,
pub multipath_mm: i16,
pub carrier_multipath_1_512c: i8,
pub smoothing_correction_mm: i16,
pub lock_count: Option<u8>,
}
impl Meas3Measurement {
pub fn pseudorange_m(&self) -> Option<f64> {
self.pseudorange_m
}
pub fn carrier_phase_cycles(&self) -> Option<f64> {
self.carrier_phase_cycles
}
pub fn doppler_hz(&self) -> Option<f32> {
self.doppler_hz
}
pub fn cn0_dbhz(&self) -> Option<f32> {
self.cn0_dbhz
}
pub fn raw_cn0_dbhz(&self) -> Option<u8> {
self.raw_cn0_dbhz
}
pub fn lock_time_ms(&self) -> Option<u32> {
self.lock_time_ms
}
pub fn is_valid(&self) -> bool {
(self.flags & MEASFLAG_VALIDITY) != 0
}
pub fn smoothing_active(&self) -> bool {
(self.flags & MEASFLAG_SMOOTHING) != 0
}
pub fn has_half_cycle_ambiguity(&self) -> bool {
(self.flags & MEASFLAG_HALFCYCLEAMBIGUITY) != 0
}
pub fn is_first_measurement(&self) -> bool {
(self.flags & MEASFLAG_FIRSTMEAS) != 0
}
pub fn apme_in_sync(&self) -> bool {
(self.flags & MEASFLAG_APMEINSYNC) != 0
}
pub fn raw_pseudorange_m(&self) -> Option<f64> {
self.pseudorange_m.map(|value| {
value + self.multipath_mm as f64 * 0.001 + self.smoothing_correction_mm as f64 * 0.001
})
}
pub fn raw_carrier_phase_cycles(&self) -> Option<f64> {
self.carrier_phase_cycles
.map(|value| value + self.carrier_multipath_1_512c as f64 / 512.0)
}
}
#[derive(Debug, Clone)]
pub struct Meas3Satellite {
pub sat_id: SatelliteId,
pub glonass_frequency_number: Option<i8>,
pub measurements: Vec<Meas3Measurement>,
}
#[derive(Debug, Clone)]
pub struct Meas3DecodedEpoch {
tow_ms: u32,
wnc: u16,
pub antenna_id: u8,
pub common_flags: u8,
pub total_clock_jump_ms: i32,
pub reference_epoch_interval_ms: u32,
pub is_reference_epoch: bool,
pub reference_epoch_contains_pr_rate: bool,
pub satellites: Vec<Meas3Satellite>,
}
impl Meas3DecodedEpoch {
pub fn tow_seconds(&self) -> f64 {
self.tow_ms as f64 * 0.001
}
pub fn tow_ms(&self) -> u32 {
self.tow_ms
}
pub fn wnc(&self) -> u16 {
self.wnc
}
pub fn num_satellites(&self) -> usize {
self.satellites.len()
}
pub fn num_measurements(&self) -> usize {
self.satellites
.iter()
.map(|sat| sat.measurements.len())
.sum()
}
}
#[derive(Debug, Clone, Default)]
pub struct Meas3BlockSet {
pub ranges: Option<Meas3RangesBlock>,
pub cn0_hi_res: Option<Meas3Cn0HiResBlock>,
pub doppler: Option<Meas3DopplerBlock>,
pub pp: Option<Meas3PpBlock>,
pub mp: Option<Meas3MpBlock>,
}
impl Meas3BlockSet {
pub fn tow_ms(&self) -> Option<u32> {
self.ranges.as_ref().map(Meas3RangesBlock::tow_ms)
}
pub fn wnc(&self) -> Option<u16> {
self.ranges.as_ref().map(Meas3RangesBlock::wnc)
}
pub fn antenna_id(&self) -> Option<u8> {
self.ranges.as_ref().map(Meas3RangesBlock::antenna_id)
}
pub fn insert_block(&mut self, block: &SbfBlock) -> bool {
match block {
SbfBlock::Meas3Ranges(value) => {
self.ranges = Some(value.clone());
true
}
SbfBlock::Meas3Cn0HiRes(value) => {
self.cn0_hi_res = Some(value.clone());
true
}
SbfBlock::Meas3Doppler(value) => {
self.doppler = Some(value.clone());
true
}
SbfBlock::Meas3Pp(value) => {
self.pp = Some(value.clone());
true
}
SbfBlock::Meas3Mp(value) => {
self.mp = Some(value.clone());
true
}
_ => false,
}
}
pub fn clear(&mut self) {
*self = Self::default();
}
}
#[derive(Debug, Clone, Default)]
pub struct Meas3Decoder {
references: Vec<AntennaReferenceEpoch>,
}
impl Meas3Decoder {
pub fn new() -> Self {
Self {
references: vec![AntennaReferenceEpoch::default(); MAX_ANTENNAS],
}
}
pub fn decode_block_set(&mut self, set: &Meas3BlockSet) -> SbfResult<Meas3DecodedEpoch> {
let ranges = set
.ranges
.as_ref()
.ok_or_else(|| SbfError::ParseError("Meas3 block set is missing Meas3Ranges".into()))?;
self.decode(
ranges,
set.cn0_hi_res.as_ref(),
set.doppler.as_ref(),
set.pp.as_ref(),
set.mp.as_ref(),
)
}
pub fn decode(
&mut self,
ranges: &Meas3RangesBlock,
cn0_hi_res: Option<&Meas3Cn0HiResBlock>,
doppler: Option<&Meas3DopplerBlock>,
pp: Option<&Meas3PpBlock>,
mp: Option<&Meas3MpBlock>,
) -> SbfResult<Meas3DecodedEpoch> {
if ranges.has_scrambled_measurements() {
return Err(SbfError::ParseError(
"Meas3 scrambling is not supported by this crate".into(),
));
}
if ranges.reserved > 31 {
return Err(SbfError::ParseError(
"Meas3Ranges revision marker is not supported".into(),
));
}
let antenna_id = ranges.antenna_id() as usize;
if antenna_id >= self.references.len() {
return Err(SbfError::ParseError(
"Meas3 antenna index out of range".into(),
));
}
for block in [
cn0_hi_res.map(|b| (b.tow_ms(), b.wnc(), b.antenna_id(), "Meas3CN0HiRes")),
doppler.map(|b| (b.tow_ms(), b.wnc(), b.antenna_id(), "Meas3Doppler")),
pp.map(|b| (b.tow_ms(), b.wnc(), b.antenna_id(), "Meas3PP")),
mp.map(|b| (b.tow_ms(), b.wnc(), b.antenna_id(), "Meas3MP")),
]
.into_iter()
.flatten()
{
if block.0 != ranges.tow_ms()
|| block.1 != ranges.wnc()
|| block.2 != ranges.antenna_id()
{
return Err(SbfError::ParseError(format!(
"{} does not match the Meas3Ranges epoch",
block.3
)));
}
}
let ref_interval_ms = ranges.reference_epoch_interval_ms();
let is_reference_epoch = ranges.is_reference_epoch();
let ref_epoch_contains_pr_rate = ranges.reference_epoch_contains_pr_rate();
let ant_ref = &mut self.references[antenna_id];
if is_reference_epoch {
*ant_ref = AntennaReferenceEpoch::default();
ant_ref.tow_ms = Some(ranges.tow_ms());
} else {
let required_ref_tow = (ranges.tow_ms() / ref_interval_ms) * ref_interval_ms;
if ant_ref.tow_ms != Some(required_ref_tow) {
return Err(SbfError::ParseError(
"Meas3 delta epoch received before its reference epoch".into(),
));
}
}
let mut payload = ranges.data.as_slice();
let mut cn0_idx = 0usize;
let mut doppler_idx = 0usize;
let mut pp1_idx = 0usize;
let mut pp2_idx = 0usize;
let mut mp_idx = 0usize;
let mut satellites = Vec::new();
for index in 0..7 {
if (ranges.constellations & (1 << index)) == 0 {
continue;
}
let system = Meas3SatSystem::from_index(index).unwrap();
let (consumed, mut decoded) = Self::decode_constellation(
payload,
system,
ant_ref,
ranges.tow_ms(),
ref_interval_ms,
ref_epoch_contains_pr_rate,
cn0_hi_res,
&mut cn0_idx,
doppler,
&mut doppler_idx,
pp,
&mut pp1_idx,
&mut pp2_idx,
mp,
&mut mp_idx,
is_reference_epoch,
)?;
satellites.append(&mut decoded);
payload = &payload[consumed..];
}
let total_clock_jump_ms = if ranges.cum_clk_jumps >= 128 {
ranges.cum_clk_jumps as i32 - 256
} else {
ranges.cum_clk_jumps as i32
};
Ok(Meas3DecodedEpoch {
tow_ms: ranges.tow_ms(),
wnc: ranges.wnc(),
antenna_id: ranges.antenna_id(),
common_flags: ranges.common_flags,
total_clock_jump_ms,
reference_epoch_interval_ms: ref_interval_ms,
is_reference_epoch,
reference_epoch_contains_pr_rate: ref_epoch_contains_pr_rate,
satellites,
})
}
#[allow(clippy::too_many_arguments)]
fn decode_constellation(
buf: &[u8],
system: Meas3SatSystem,
antenna_ref: &mut AntennaReferenceEpoch,
tow_ms: u32,
ref_interval_ms: u32,
ref_epoch_contains_pr_rate: bool,
cn0_hi_res: Option<&Meas3Cn0HiResBlock>,
cn0_idx: &mut usize,
doppler: Option<&Meas3DopplerBlock>,
doppler_idx: &mut usize,
pp: Option<&Meas3PpBlock>,
pp1_idx: &mut usize,
pp2_idx: &mut usize,
mp: Option<&Meas3MpBlock>,
mp_idx: &mut usize,
is_reference_epoch: bool,
) -> SbfResult<(usize, Vec<Meas3Satellite>)> {
if buf.is_empty() {
return Err(SbfError::ParseError(
"Meas3 constellation data is empty".into(),
));
}
let start = buf;
let ref_const = antenna_ref.constellations.entry(system).or_default();
let sat_data_buf = if buf[0] == 0 {
if ref_const.m3satdata_copy.is_empty() {
return Err(SbfError::ParseError(
"Meas3 delta constellation is missing its reference layout".into(),
));
}
ref_const.m3satdata_copy.as_slice()
} else {
buf
};
let mut n = 0usize;
let mut sat_mask = 0u64;
let mut nsats = 0usize;
let mut glo_fn_list = [0u8; 32];
let bf1 = sat_data_buf[0];
let mut nb = (bf1 & 0x07) as usize;
if nb == 7 {
nb = 8;
}
let sig_idx_master_short = (bf1 >> 3) & 0x0f;
let sig_excluded_present = (bf1 & 0x80) != 0;
n += 1;
if sat_data_buf.len() < n + nb {
return Err(SbfError::ParseError("Meas3 SatMask is truncated".into()));
}
for ii in 0..nb {
let byte = sat_data_buf[n + ii];
sat_mask |= (byte as u64) << (ii * 8);
nsats += bit_count(byte) as usize;
}
n += nb;
if system == Meas3SatSystem::Glo {
let len = nsats.div_ceil(2);
if sat_data_buf.len() < n + len {
return Err(SbfError::ParseError(
"Meas3 GLO frequency list is truncated".into(),
));
}
glo_fn_list[..len].copy_from_slice(&sat_data_buf[n..n + len]);
n += len;
}
let bds_long_range = if system == Meas3SatSystem::Bds {
if sat_data_buf.len() < n + 2 {
return Err(SbfError::ParseError(
"Meas3 BDS long-range flags are truncated".into(),
));
}
let value = u16::from_le_bytes(sat_data_buf[n..n + 2].try_into().unwrap());
n += 2;
value
} else {
0
};
let sig_excluded = if sig_excluded_present {
if sat_data_buf.len() <= n {
return Err(SbfError::ParseError(
"Meas3 SigExcluded is truncated".into(),
));
}
let value = sat_data_buf[n];
n += 1;
value
} else {
0
};
let mut buf = if start[0] == 0 {
&start[1..]
} else {
&start[n..]
};
if is_reference_epoch {
ref_const.m3satdata_copy = start[..n].to_vec();
}
let signal_table = prepare_signal_table(system, sig_excluded);
let mut satellites = Vec::new();
let mut sat_count = 0usize;
for sat_idx in 0..MEAS3_SAT_MAX {
if sat_count >= nsats {
break;
}
if (sat_mask & (1u64 << sat_idx)) == 0 {
continue;
}
let glonass_fn = if system == Meas3SatSystem::Glo {
let nibble = (glo_fn_list[sat_count / 2] >> (4 * (sat_count % 2))) & 0x0f;
nibble as i8 - 8
} else {
0
};
let short_pr_base =
if system == Meas3SatSystem::Bds && (bds_long_range & (1 << sat_count)) != 0 {
34e6
} else {
PR_BASE_M[system as usize]
};
let reference_sat = ref_const.satellites.entry(sat_idx as u8).or_default();
let time_since_ref_epoch_ms = tow_ms % ref_interval_ms;
let (mut master, master_sig_idx, mut slave_sig_mask, pr_rate_64mm_s, master_size) =
decode_master(
buf,
MasterDecodeContext {
signal_table: &signal_table,
glonass_fn,
short_pr_base_m: short_pr_base,
sig_idx_master_short,
reference_sat,
time_since_ref_epoch_ms,
pr_rate_available: ref_epoch_contains_pr_rate,
},
)?;
buf = &buf[master_size..];
if is_reference_epoch {
reference_sat.sig_order.clear();
reference_sat.sig_order.push(master_sig_idx);
reference_sat.slave_sig_mask = slave_sig_mask;
reference_sat.pr_rate_64mm_s = pr_rate_64mm_s;
reference_sat
.measurements
.insert(master_sig_idx, master.clone());
}
if let Some(existing) = reference_sat.measurements.get_mut(&master_sig_idx) {
if master.pll_timer_ms > existing.pll_timer_ms {
existing.pll_timer_ms = master.pll_timer_ms;
}
}
let master_wavelength = wavelength_for_signal(master.signal_type, glonass_fn);
add_master_doppler(
&mut master,
doppler,
master_wavelength,
reference_sat.pr_rate_64mm_s,
doppler_idx,
);
add_pp_info(&mut master, pp, pp1_idx, pp2_idx);
add_mp_info(&mut master, mp, mp_idx);
let mut master_hi_res_adjust = 0.0f32;
if let Some(cn0) = cn0_hi_res {
master_hi_res_adjust = decode_cn0_hires(&cn0.data, cn0_idx)?;
}
let sat_id = SatelliteId::from_svid(system.base_svid() + sat_idx as u8)
.ok_or_else(|| SbfError::ParseError("Meas3 produced an invalid SVID".into()))?;
let mut current_measurements = Vec::new();
let mut slave_count = 0usize;
for sig_idx in 1..MEAS3_SIG_MAX {
if slave_sig_mask == 0 {
break;
}
if (slave_sig_mask & (1 << sig_idx)) == 0 {
continue;
}
let slave_uses_reference = buf
.first()
.is_some_and(|first| (first & 1) == 0 && (first & 3) != 0);
let (master_ref, slave_ref) = if slave_uses_reference {
let master_ref_idx = *reference_sat.sig_order.first().ok_or_else(|| {
SbfError::ParseError(
"Meas3 reference epoch is missing the master signal".into(),
)
})?;
let master_ref = reference_sat
.measurements
.get(&master_ref_idx)
.ok_or_else(|| {
SbfError::ParseError(
"Meas3 reference master measurement is missing".into(),
)
})?
.clone();
let slave_ref_idx =
*reference_sat
.sig_order
.get(slave_count + 1)
.ok_or_else(|| {
SbfError::ParseError(
"Meas3 reference slave measurement is missing".into(),
)
})?;
let slave_ref = reference_sat
.measurements
.get(&slave_ref_idx)
.ok_or_else(|| {
SbfError::ParseError(
"Meas3 reference slave measurement is missing".into(),
)
})?
.clone();
(Some(master_ref), Some(slave_ref))
} else {
(None, None)
};
let (mut slave, consumed) = decode_slave(
buf,
SlaveDecodeContext {
signal_table: &signal_table,
sig_idx: sig_idx as u8,
glonass_fn,
master: &master,
master_sig_idx,
master_ref: master_ref.as_ref(),
slave_ref: slave_ref.as_ref(),
},
)?;
buf = &buf[consumed..];
let slave_wavelength = wavelength_for_signal(slave.signal_type, glonass_fn);
add_slave_doppler(
&mut slave,
&master,
doppler,
master_wavelength,
slave_wavelength,
doppler_idx,
);
add_pp_info(&mut slave, pp, pp1_idx, pp2_idx);
add_mp_info(&mut slave, mp, mp_idx);
if is_reference_epoch {
if reference_sat.sig_order.len() <= slave_count + 1 {
reference_sat.sig_order.push(sig_idx as u8);
}
reference_sat
.measurements
.insert(sig_idx as u8, slave.clone());
}
if let Some(existing) = reference_sat.measurements.get_mut(&(sig_idx as u8)) {
if slave.pll_timer_ms > existing.pll_timer_ms {
existing.pll_timer_ms = slave.pll_timer_ms;
}
}
if cn0_hi_res.is_some() {
slave.cn0_dbhz +=
decode_cn0_hires(cn0_hi_res.unwrap().data.as_slice(), cn0_idx)?;
}
current_measurements.push(slave.into_public());
slave_count += 1;
slave_sig_mask ^= 1 << sig_idx;
}
master.cn0_dbhz += master_hi_res_adjust;
current_measurements.insert(0, master.into_public());
satellites.push(Meas3Satellite {
sat_id,
glonass_frequency_number: if system == Meas3SatSystem::Glo {
Some(glonass_fn)
} else {
None
},
measurements: current_measurements,
});
sat_count += 1;
}
let consumed = start.len() - buf.len();
Ok((consumed, satellites))
}
}
fn bit_count(value: u8) -> u32 {
value.count_ones()
}
fn lsb_pos(value: u32) -> u8 {
if value == 0 {
32
} else {
value.trailing_zeros() as u8
}
}
fn decode_cn0_hires(data: &[u8], idx: &mut usize) -> SbfResult<f32> {
let byte = *data
.get(*idx / 2)
.ok_or_else(|| SbfError::ParseError("Meas3CN0HiRes payload is truncated".into()))?;
let nibble = (byte >> ((*idx % 2) * 4)) & 0x0f;
*idx += 1;
Ok(nibble as f32 * 0.0625 - 0.5)
}
fn prepare_signal_table(system: Meas3SatSystem, sig_excluded: u8) -> [SignalType; MEAS3_SIG_MAX] {
let defaults = default_signal_table(system);
let mut result = [SignalType::Other(u8::MAX); MEAS3_SIG_MAX];
let mut positions = Vec::with_capacity(MEAS3_SIG_MAX);
for i in 0..MEAS3_SIG_MAX {
if i >= 8 || (sig_excluded & (1 << i)) == 0 {
positions.push(i);
}
}
for (dst, src) in positions.into_iter().enumerate().take(MEAS3_SIG_MAX) {
result[dst] = defaults[src];
}
result
}
fn default_signal_table(system: Meas3SatSystem) -> [SignalType; MEAS3_SIG_MAX] {
let invalid = SignalType::Other(u8::MAX);
match system {
Meas3SatSystem::Gps => [
SignalType::L1CA,
SignalType::L2C,
SignalType::L5,
SignalType::L1PY,
SignalType::L2P,
SignalType::L1C,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
],
Meas3SatSystem::Glo => [
SignalType::G1CA,
SignalType::G2CA,
SignalType::G1P,
SignalType::G2P,
SignalType::G3,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
],
Meas3SatSystem::Gal => [
SignalType::E1,
SignalType::E5a,
SignalType::E5b,
SignalType::E6,
SignalType::E5AltBOC,
SignalType::Other(16),
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
],
Meas3SatSystem::Bds => [
SignalType::B1I,
SignalType::B2I,
SignalType::B3I,
SignalType::B1C,
SignalType::B2a,
SignalType::B2b,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
],
Meas3SatSystem::Sbas => [
SignalType::SBASL1,
SignalType::SBASL5,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
],
Meas3SatSystem::Qzs => [
SignalType::QZSSL1CA,
SignalType::QZSSL2C,
SignalType::QZSSL5,
SignalType::QZSSL6,
SignalType::QZSSL1C,
SignalType::QZSSL1S,
SignalType::QZSSL5S,
SignalType::QZSSL1CB,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
],
Meas3SatSystem::Irn => [
SignalType::NavICL5,
SignalType::NavICL1,
SignalType::Other(36),
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
invalid,
],
}
}
fn wavelength_for_signal(signal_type: SignalType, glonass_fn: i8) -> f64 {
match signal_type {
SignalType::L1CA
| SignalType::L1PY
| SignalType::L1C
| SignalType::E1
| SignalType::SBASL1
| SignalType::QZSSL1CA
| SignalType::QZSSL1CB
| SignalType::QZSSL1C
| SignalType::QZSSL1S
| SignalType::B1C
| SignalType::NavICL1 => L1_WAVELENGTH,
SignalType::L2P | SignalType::L2C | SignalType::QZSSL2C => L2_WAVELENGTH,
SignalType::E5AltBOC => E5_WAVELENGTH,
SignalType::E5a | SignalType::Other(16) => E5A_WAVELENGTH,
SignalType::E5b | SignalType::B2I | SignalType::B2b => E5B_WAVELENGTH,
SignalType::E6 | SignalType::QZSSL6 => E6_WAVELENGTH,
SignalType::L5
| SignalType::SBASL5
| SignalType::QZSSL5
| SignalType::QZSSL5S
| SignalType::NavICL5
| SignalType::B2a => L5_WAVELENGTH,
SignalType::B1I => E2_WAVELENGTH,
SignalType::B3I => B3_WAVELENGTH,
SignalType::G1CA | SignalType::G1P => {
if (-7..=6).contains(&glonass_fn) {
C84 / (L1_GLO_FREQ + glonass_fn as f64 * 562_500.0)
} else {
F64_NOTVALID
}
}
SignalType::G2CA | SignalType::G2P => {
if (-7..=6).contains(&glonass_fn) {
C84 / (L2_GLO_FREQ + glonass_fn as f64 * 437_500.0)
} else {
F64_NOTVALID
}
}
SignalType::G3 => L3_WAVELENGTH,
SignalType::Other(36) => S1_WAVELENGTH,
SignalType::LBand => L1_WAVELENGTH,
SignalType::L2PY | SignalType::Other(_) => F64_NOTVALID,
}
}
fn is_gps_p_code(signal_type: SignalType) -> bool {
matches!(signal_type, SignalType::L1PY | SignalType::L2P)
}
fn read_u16(bytes: &[u8], offset: usize) -> SbfResult<u16> {
let slice = bytes
.get(offset..offset + 2)
.ok_or_else(|| SbfError::ParseError("Meas3 payload is truncated".into()))?;
Ok(u16::from_le_bytes(slice.try_into().unwrap()))
}
fn read_u32(bytes: &[u8], offset: usize) -> SbfResult<u32> {
let slice = bytes
.get(offset..offset + 4)
.ok_or_else(|| SbfError::ParseError("Meas3 payload is truncated".into()))?;
Ok(u32::from_le_bytes(slice.try_into().unwrap()))
}
fn read_i16(bytes: &[u8], offset: usize) -> SbfResult<i16> {
let slice = bytes
.get(offset..offset + 2)
.ok_or_else(|| SbfError::ParseError("Meas3 payload is truncated".into()))?;
Ok(i16::from_le_bytes(slice.try_into().unwrap()))
}
fn read_u8(bytes: &[u8], offset: usize) -> SbfResult<u8> {
bytes
.get(offset)
.copied()
.ok_or_else(|| SbfError::ParseError("Meas3 payload is truncated".into()))
}
fn decode_master(
buf: &[u8],
ctx: MasterDecodeContext<'_>,
) -> SbfResult<(InternalMeasurement, u8, u32, i16, usize)> {
let MasterDecodeContext {
signal_table,
glonass_fn,
short_pr_base_m,
sig_idx_master_short,
reference_sat,
time_since_ref_epoch_ms,
pr_rate_available,
} = ctx;
if buf.is_empty() {
return Err(SbfError::ParseError(
"Meas3 master sub-block is missing".into(),
));
}
let mut meas = InternalMeasurement::default();
if (buf[0] & 1) == 1 {
if buf.len() < if pr_rate_available { 10 } else { 8 } {
return Err(SbfError::ParseError(
"Meas3 master-short is truncated".into(),
));
}
let bf1 = read_u32(buf, 0)?;
let pr_lsb = read_u32(buf, 4)?;
let cmc = (bf1 >> 1) & 0x3ffff;
let pr_msb = (bf1 >> 19) & 1;
let lti = ((bf1 >> 20) & 0x7) as usize;
let cn0 = (bf1 >> 23) & 0x1f;
let sig_list = (bf1 >> 28) & 0x0f;
let master_sig_idx = sig_idx_master_short;
let signal_type = signal_table[master_sig_idx as usize];
let wavelength = wavelength_for_signal(signal_type, glonass_fn);
meas.signal_index = master_sig_idx;
meas.signal_type = signal_type;
meas.flags = MEASFLAG_VALIDITY;
meas.pr_m = short_pr_base_m + (pr_lsb as f64 + 4_294_967_296.0 * pr_msb as f64) * 0.001;
meas.l_cycles = if cmc == 0 {
F64_NOTVALID
} else {
meas.pr_m / wavelength - 131.072 + cmc as f64 * 0.001
};
meas.cn0_dbhz = cn0 as f32 + 24.0;
meas.pll_timer_ms = LOCK_INDICATOR_TO_MS[lti];
if lti == 0 {
meas.flags |= MEASFLAG_HALFCYCLEAMBIGUITY;
}
let pr_rate_64mm_s = if pr_rate_available {
read_i16(buf, 8)?
} else {
0
};
let slave_sig_mask = sig_list << (master_sig_idx + 1);
Ok((
meas,
master_sig_idx,
slave_sig_mask,
pr_rate_64mm_s,
if pr_rate_available { 10 } else { 8 },
))
} else if (buf[0] & 3) == 0 {
if buf.len() < 10 {
return Err(SbfError::ParseError(
"Meas3 master-long is truncated".into(),
));
}
let bf1 = read_u32(buf, 0)?;
let pr_lsb = read_u32(buf, 4)?;
let bf2 = read_u16(buf, 8)?;
let pr_msb = (bf1 >> 2) & 0x0f;
let cmc = (bf1 >> 6) & 0x3fffff;
let lti = ((bf1 >> 28) & 0x0f) as usize;
let cn0 = (bf2 & 0x3f) as u32;
let mut sig_mask = ((bf2 >> 6) & 0x1ff) as u32;
let cont = ((bf2 >> 15) & 1) != 0;
let pr_rate_offset = 10 + cont as usize;
let consumed = if pr_rate_available {
pr_rate_offset + 2
} else {
pr_rate_offset
};
if buf.len() < consumed {
return Err(SbfError::ParseError(
"Meas3 master-long is truncated".into(),
));
}
if cont {
let bf3 = read_u8(buf, 10)?;
sig_mask |= ((bf3 & 0x7f) as u32) << 9;
}
if sig_mask == 0 {
return Err(SbfError::ParseError(
"Meas3 master-long has an empty signal mask".into(),
));
}
let master_sig_idx = lsb_pos(sig_mask);
let signal_type = signal_table[master_sig_idx as usize];
let wavelength = wavelength_for_signal(signal_type, glonass_fn);
meas.signal_index = master_sig_idx;
meas.signal_type = signal_type;
meas.flags = MEASFLAG_VALIDITY;
meas.pr_m = (pr_lsb as f64 + 4_294_967_296.0 * pr_msb as f64) * 0.001;
meas.l_cycles = if cmc == 0 {
F64_NOTVALID
} else {
meas.pr_m / wavelength - 2_097.152 + cmc as f64 * 0.001
};
meas.cn0_dbhz = if is_gps_p_code(signal_type) {
cn0 as f32
} else {
cn0 as f32 + 10.0
};
meas.pll_timer_ms = LOCK_INDICATOR_TO_MS[lti];
if lti == 0 {
meas.flags |= MEASFLAG_HALFCYCLEAMBIGUITY;
}
let pr_rate_64mm_s = if pr_rate_available {
read_i16(buf, pr_rate_offset)?
} else {
0
};
let slave_sig_mask = sig_mask ^ (1 << master_sig_idx);
Ok((
meas,
master_sig_idx,
slave_sig_mask,
pr_rate_64mm_s,
consumed,
))
} else if (buf[0] & 0x0c) == 0x0c {
if buf.len() < 5 {
return Err(SbfError::ParseError(
"Meas3 master delta-long is truncated".into(),
));
}
let bf1 = read_u8(buf, 0)?;
let bf2 = read_u32(buf, 1)?;
let pr = (((bf1 >> 4) as u32) << 13) | (bf2 & 0x1fff);
let cn0 = (bf2 >> 13) & 0x07;
let cmc = bf2 >> 16;
let master_sig_idx = *reference_sat.sig_order.first().ok_or_else(|| {
SbfError::ParseError("Meas3 master delta-long is missing reference data".into())
})?;
let master_ref = reference_sat
.measurements
.get(&master_sig_idx)
.ok_or_else(|| {
SbfError::ParseError("Meas3 master delta-long is missing reference data".into())
})?;
let signal_type = signal_table[master_sig_idx as usize];
let wavelength = wavelength_for_signal(signal_type, glonass_fn);
meas.signal_index = master_sig_idx;
meas.signal_type = signal_type;
meas.flags = master_ref.flags & (MEASFLAG_VALIDITY | MEASFLAG_HALFCYCLEAMBIGUITY);
meas.pll_timer_ms = master_ref.pll_timer_ms;
meas.pr_m = master_ref.pr_m
+ ((reference_sat.pr_rate_64mm_s as i64 * 64 * time_since_ref_epoch_ms as i64 / 1000)
as f64)
* 0.001
+ pr as f64 * 0.001
- 65.536;
meas.l_cycles = if cmc == 0 {
F64_NOTVALID
} else {
(meas.pr_m - master_ref.pr_m) / wavelength + master_ref.l_cycles - 32.768
+ cmc as f64 * 0.001
};
meas.cn0_dbhz = master_ref.cn0_dbhz - 4.0 + cn0 as f32;
Ok((meas, master_sig_idx, reference_sat.slave_sig_mask, 0, 5))
} else {
if buf.len() < 4 {
return Err(SbfError::ParseError(
"Meas3 master delta-short is truncated".into(),
));
}
let bf1 = read_u32(buf, 0)?;
let pr = (bf1 >> 4) & 0x3fff;
let cmc = (bf1 >> 18) & 0x3fff;
let cn0 = (bf1 >> 2) & 0x03;
let master_sig_idx = *reference_sat.sig_order.first().ok_or_else(|| {
SbfError::ParseError("Meas3 master delta-short is missing reference data".into())
})?;
let master_ref = reference_sat
.measurements
.get(&master_sig_idx)
.ok_or_else(|| {
SbfError::ParseError("Meas3 master delta-short is missing reference data".into())
})?;
let signal_type = signal_table[master_sig_idx as usize];
let wavelength = wavelength_for_signal(signal_type, glonass_fn);
meas.signal_index = master_sig_idx;
meas.signal_type = signal_type;
meas.flags = master_ref.flags & (MEASFLAG_VALIDITY | MEASFLAG_HALFCYCLEAMBIGUITY);
meas.pll_timer_ms = master_ref.pll_timer_ms;
meas.pr_m = master_ref.pr_m
+ ((reference_sat.pr_rate_64mm_s as i64 * 64 * time_since_ref_epoch_ms as i64 / 1000)
as f64)
* 0.001
+ pr as f64 * 0.001
- 8.192;
meas.l_cycles = if cmc == 0 {
F64_NOTVALID
} else {
(meas.pr_m - master_ref.pr_m) / wavelength + master_ref.l_cycles - 8.192
+ cmc as f64 * 0.001
};
meas.cn0_dbhz = master_ref.cn0_dbhz - 1.0 + cn0 as f32;
Ok((meas, master_sig_idx, reference_sat.slave_sig_mask, 0, 4))
}
}
fn decode_slave(
buf: &[u8],
ctx: SlaveDecodeContext<'_>,
) -> SbfResult<(InternalMeasurement, usize)> {
let SlaveDecodeContext {
signal_table,
sig_idx,
glonass_fn,
master,
master_sig_idx,
master_ref,
slave_ref,
} = ctx;
if buf.is_empty() {
return Err(SbfError::ParseError(
"Meas3 slave sub-block is missing".into(),
));
}
let mut meas = InternalMeasurement::default();
let wavelength_master =
wavelength_for_signal(signal_table[master_sig_idx as usize], glonass_fn);
let signal_type = signal_table[sig_idx as usize];
let wavelength_slave = wavelength_for_signal(signal_type, glonass_fn);
meas.flags = MEASFLAG_VALIDITY;
meas.signal_index = sig_idx;
meas.signal_type = signal_type;
if (buf[0] & 1) == 1 {
if buf.len() < 5 {
return Err(SbfError::ParseError(
"Meas3 slave-short is truncated".into(),
));
}
let bf1 = read_u32(buf, 0)?;
let bf2 = read_u8(buf, 4)?;
let cmc_res = (bf1 >> 1) & 0xffff;
let pr_rel = bf1 >> 17;
let lti = (bf2 & 0x07) as usize;
let cn0 = (bf2 >> 3) as u32;
if wavelength_master < wavelength_slave {
meas.pr_m = master.pr_m + pr_rel as f64 * 0.001 - 10.0;
} else {
meas.pr_m = master.pr_m - pr_rel as f64 * 0.001 + 10.0;
}
meas.l_cycles = if cmc_res == 0 {
F64_NOTVALID
} else {
meas.pr_m / wavelength_slave
+ (master.l_cycles - master.pr_m / wavelength_master) * wavelength_slave
/ wavelength_master
- 32.768
+ cmc_res as f64 * 0.001
};
meas.cn0_dbhz = if is_gps_p_code(signal_type) {
master.cn0_dbhz - 3.0 - cn0 as f32
} else {
cn0 as f32 + 24.0
};
meas.pll_timer_ms = LOCK_INDICATOR_TO_MS[lti];
if lti == 0 {
meas.flags |= MEASFLAG_HALFCYCLEAMBIGUITY;
}
Ok((meas, 5))
} else if (buf[0] & 3) == 0 {
if buf.len() < 7 {
return Err(SbfError::ParseError("Meas3 slave-long is truncated".into()));
}
let bf1 = read_u32(buf, 0)?;
let pr_lsb_rel = read_u16(buf, 4)?;
let bf3 = read_u8(buf, 6)?;
let cmc = (bf1 >> 2) & 0x3fffff;
let lti = ((bf1 >> 24) & 0x0f) as usize;
let pr_msb_rel = (bf1 >> 28) & 0x07;
let cn0 = (bf3 & 0x3f) as u32;
meas.pr_m =
master.pr_m + (pr_msb_rel * 65_536 + pr_lsb_rel as u32) as f64 * 0.001 - 262.144;
meas.l_cycles = if cmc == 0 {
F64_NOTVALID
} else {
meas.pr_m / wavelength_slave - 2_097.152 + cmc as f64 * 0.001
};
meas.cn0_dbhz = if is_gps_p_code(signal_type) {
cn0 as f32
} else {
cn0 as f32 + 10.0
};
meas.pll_timer_ms = LOCK_INDICATOR_TO_MS[lti];
if lti == 0 {
meas.flags |= MEASFLAG_HALFCYCLEAMBIGUITY;
}
Ok((meas, 7))
} else {
if buf.len() < 3 {
return Err(SbfError::ParseError(
"Meas3 slave delta is truncated".into(),
));
}
let master_ref = master_ref.ok_or_else(|| {
SbfError::ParseError("Meas3 slave delta is missing reference data".into())
})?;
let slave_ref = slave_ref.ok_or_else(|| {
SbfError::ParseError("Meas3 slave delta is missing reference data".into())
})?;
let bf1 = read_u16(buf, 0)?;
let d_carrier = read_u8(buf, 2)?;
let d_pr = ((bf1 >> 2) & 0x0fff) as u32;
let cn0 = (bf1 >> 14) as u32;
meas.flags = slave_ref.flags & (MEASFLAG_VALIDITY | MEASFLAG_HALFCYCLEAMBIGUITY);
meas.l_cycles = slave_ref.l_cycles
+ (master.l_cycles - master_ref.l_cycles) * wavelength_master / wavelength_slave
- 0.128
+ d_carrier as f64 * 0.001;
meas.pr_m = slave_ref.pr_m + (meas.l_cycles - slave_ref.l_cycles) * wavelength_slave
- 2.048
+ d_pr as f64 * 0.001;
meas.cn0_dbhz = slave_ref.cn0_dbhz - 2.0 + cn0 as f32;
meas.pll_timer_ms = slave_ref.pll_timer_ms;
Ok((meas, 3))
}
}
fn get_pr_rate_mm_s(
doppler: Option<&Meas3DopplerBlock>,
idx: &mut usize,
) -> SbfResult<Option<i32>> {
let Some(doppler) = doppler else {
return Ok(None);
};
let value = read_u32(doppler.data.as_slice(), *idx)?;
let (abs_prr_mm_s, consumed, magnitude_bits) = if (value & 2) == 0 {
(((value & 0xff) >> 2) as i32, 1usize, 6u8)
} else if (value & 6) == 2 {
(((value & 0xffff) >> 3) as i32, 2usize, 13u8)
} else if (value & 0x0e) == 6 {
(((value & 0x00ff_ffff) >> 4) as i32, 3usize, 20u8)
} else {
((value >> 4) as i32, 4usize, 28u8)
};
*idx += consumed;
let negative = (value & 1) != 0;
let invalid_abs = (1i32 << magnitude_bits) - 1;
if negative && abs_prr_mm_s == invalid_abs {
return Ok(None);
}
Ok(Some(if negative {
-abs_prr_mm_s
} else {
abs_prr_mm_s
}))
}
fn add_master_doppler(
meas: &mut InternalMeasurement,
doppler: Option<&Meas3DopplerBlock>,
wavelength_m: f64,
ref_pr_rate_64mm_s: i16,
idx: &mut usize,
) {
let Ok(Some(prr_mm_s)) = get_pr_rate_mm_s(doppler, idx) else {
meas.doppler_hz = F32_NOTVALID;
return;
};
meas.doppler_hz =
-((prr_mm_s + ref_pr_rate_64mm_s as i32 * 64) as f64 * 0.001 / wavelength_m) as f32;
}
fn add_slave_doppler(
meas: &mut InternalMeasurement,
master: &InternalMeasurement,
doppler: Option<&Meas3DopplerBlock>,
wavelength_master_m: f64,
wavelength_slave_m: f64,
idx: &mut usize,
) {
let Ok(Some(prr_mm_s)) = get_pr_rate_mm_s(doppler, idx) else {
meas.doppler_hz = F32_NOTVALID;
return;
};
meas.doppler_hz = ((master.doppler_hz as f64 * wavelength_master_m * 1000.0 - prr_mm_s as f64)
* 0.001
/ wavelength_slave_m) as f32;
}
fn add_pp_info(
meas: &mut InternalMeasurement,
pp: Option<&Meas3PpBlock>,
pp1_idx: &mut usize,
pp2_idx: &mut usize,
) {
meas.raw_cn0_dbhz = ((meas.cn0_dbhz as i32 / 2) * 2).max(0) as u8;
let Some(pp) = pp else {
return;
};
let Some((section2_offset_units, payload)) = pp.data.split_first() else {
return;
};
let Some(first_lo) = payload.get(*pp1_idx / 8) else {
return;
};
let Some(first_hi) = payload.get(*pp1_idx / 8 + 1) else {
return;
};
let dummy = *first_lo as u16 | (*first_hi as u16) << 8;
let val = dummy >> (*pp1_idx % 8);
let start2 = (*section2_offset_units as usize) * 4;
if (val & 1) != 0 {
meas.flags |= MEASFLAG_APMEINSYNC;
}
meas.lock_count = ((val >> 1) & 0x0f) as u8;
*pp1_idx += 5;
if start2 != 0 && payload.get(start2).map(|value| value & 0x0f) == Some(0) {
if let (Some(lo), Some(hi)) = (
payload.get(*pp2_idx / 8 + start2 + 2),
payload.get(*pp2_idx / 8 + start2 + 3),
) {
let dummy = *lo as u16 | (*hi as u16) << 8;
let val = dummy >> (*pp2_idx % 8);
if (val & 1) == 0 {
*pp2_idx += 1;
} else {
meas.raw_cn0_dbhz = (((val >> 1) & 0x1f) * 2) as u8;
if !is_gps_p_code(meas.signal_type) {
meas.raw_cn0_dbhz = meas.raw_cn0_dbhz.saturating_add(10);
}
*pp2_idx += 6;
}
}
}
}
fn add_mp_info(meas: &mut InternalMeasurement, mp: Option<&Meas3MpBlock>, idx: &mut usize) {
let Some(mp) = mp else {
meas.mp_mm = 0;
meas.carrier_mp_1_512c = 0;
return;
};
let Some(base) = mp.data.get(*idx / 8) else {
meas.mp_mm = 0;
meas.carrier_mp_1_512c = 0;
return;
};
let value = *base as u32
| (mp.data.get(*idx / 8 + 1).copied().unwrap_or(0) as u32) << 8
| (mp.data.get(*idx / 8 + 2).copied().unwrap_or(0) as u32) << 16
| (mp.data.get(*idx / 8 + 3).copied().unwrap_or(0) as u32) << 24;
let value = value >> (*idx % 8);
match value & 3 {
0 => {
meas.mp_mm = 0;
meas.carrier_mp_1_512c = 0;
*idx += 2;
}
1 | 2 => {
let code = ((value >> 2) & 0x7f) as i16 * 10;
let carrier = ((value >> 9) & 0x1f) as i16;
meas.mp_mm = if (value & 3) == 1 { code } else { -code };
meas.carrier_mp_1_512c = if carrier < 16 {
carrier as i8
} else {
(carrier - 32) as i8
};
*idx += 14;
}
_ => {
let code = ((value >> 2) & 0x7ff) as i16;
let carrier = ((value >> 13) & 0xff) as i16;
meas.mp_mm = if code < 1024 {
code * 10
} else {
(code - 2048) * 10
};
meas.carrier_mp_1_512c = if carrier < 128 {
carrier as i8
} else {
(carrier - 256) as i8
};
*idx += 21;
}
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::blocks::block_ids;
fn build_sbf_block(block_id: u16, body: &[u8]) -> Vec<u8> {
let block_data_len = 12 + body.len();
let mut total_len = (2 + block_data_len) as u16;
while (total_len as usize & 0x03) != 0 {
total_len += 1;
}
let mut data = vec![0u8; total_len as usize];
data[0] = 0x24;
data[1] = 0x40;
data[4..6].copy_from_slice(&block_id.to_le_bytes());
data[6..8].copy_from_slice(&total_len.to_le_bytes());
data[8..12].copy_from_slice(&1000u32.to_le_bytes());
data[12..14].copy_from_slice(&2200u16.to_le_bytes());
data[14..14 + body.len()].copy_from_slice(body);
data
}
fn parse_doppler_block(payload: &[u8]) -> Meas3DopplerBlock {
let mut body = vec![0u8; 1];
body.extend_from_slice(payload);
let raw = build_sbf_block(block_ids::MEAS3_DOPPLER, &body);
let (block, _) = SbfBlock::parse(&raw).unwrap();
let SbfBlock::Meas3Doppler(doppler) = block else {
panic!("expected Meas3Doppler");
};
doppler
}
fn encode_pr_rate(abs_prr_mm_s: u32, width_bytes: usize, negative: bool) -> Vec<u8> {
let sign = u32::from(negative);
let value = match width_bytes {
1 => (abs_prr_mm_s << 2) | sign,
2 => (abs_prr_mm_s << 3) | 0b010 | sign,
3 => (abs_prr_mm_s << 4) | 0b0110 | sign,
4 => (abs_prr_mm_s << 4) | 0b1110 | sign,
_ => panic!("unsupported Meas3Doppler width"),
};
value.to_le_bytes()[..width_bytes].to_vec()
}
#[test]
fn meas3_decoder_decodes_single_reference_measurement() {
let constellation_header = [0x01u8, 0x01];
let bf1 = 1u32 | (1000u32 << 1) | (1u32 << 20) | (10u32 << 23);
let mut master = Vec::new();
master.extend_from_slice(&bf1.to_le_bytes());
master.extend_from_slice(&10_000u32.to_le_bytes());
let mut body = vec![0u8; 6];
body[2..4].copy_from_slice(&(1u16).to_le_bytes());
body.extend_from_slice(&constellation_header);
body.extend_from_slice(&master);
let raw = build_sbf_block(block_ids::MEAS3_RANGES, &body);
let (block, _) = SbfBlock::parse(&raw).unwrap();
let SbfBlock::Meas3Ranges(ranges) = block else {
panic!("expected Meas3Ranges");
};
let mut decoder = Meas3Decoder::new();
let epoch = decoder.decode(&ranges, None, None, None, None).unwrap();
assert_eq!(epoch.antenna_id, 0);
assert!(epoch.is_reference_epoch);
assert_eq!(epoch.num_satellites(), 1);
assert_eq!(epoch.num_measurements(), 1);
let sat = &epoch.satellites[0];
assert_eq!(sat.sat_id.to_string(), "G01");
let meas = &sat.measurements[0];
assert_eq!(meas.signal_type, SignalType::L1CA);
assert!((meas.cn0_dbhz().unwrap() - 34.0).abs() < 1e-6);
assert_eq!(meas.lock_time_ms(), Some(60_000));
assert!(meas.pseudorange_m().unwrap() > 19_000_000.0);
}
#[test]
fn meas3_decoder_decodes_reference_epoch_with_slave_signal() {
let constellation_header = [0x01u8, 0x01];
let master_bf1 = 1u32 | (1000u32 << 1) | (1u32 << 20) | (10u32 << 23) | (1u32 << 28);
let mut master = Vec::new();
master.extend_from_slice(&master_bf1.to_le_bytes());
master.extend_from_slice(&10_000u32.to_le_bytes());
let slave_bf1 = 1u32 | (1000u32 << 1);
let slave_bf2 = 1u8 | (10u8 << 3);
let mut slave = Vec::new();
slave.extend_from_slice(&slave_bf1.to_le_bytes());
slave.push(slave_bf2);
let mut body = vec![0u8; 6];
body[2..4].copy_from_slice(&(1u16).to_le_bytes());
body.extend_from_slice(&constellation_header);
body.extend_from_slice(&master);
body.extend_from_slice(&slave);
let raw = build_sbf_block(block_ids::MEAS3_RANGES, &body);
let (block, _) = SbfBlock::parse(&raw).unwrap();
let SbfBlock::Meas3Ranges(ranges) = block else {
panic!("expected Meas3Ranges");
};
let mut decoder = Meas3Decoder::new();
let epoch = decoder.decode(&ranges, None, None, None, None).unwrap();
assert!(epoch.is_reference_epoch);
assert_eq!(epoch.num_satellites(), 1);
assert_eq!(epoch.num_measurements(), 2);
let sat = &epoch.satellites[0];
assert_eq!(sat.measurements.len(), 2);
assert_eq!(sat.measurements[0].signal_type, SignalType::L1CA);
assert_eq!(sat.measurements[1].signal_type, SignalType::L2C);
assert!(sat.measurements[1].pseudorange_m().is_some());
}
#[test]
fn decode_master_long_reads_pr_rate_after_continuation_byte() {
let signal_table = prepare_signal_table(Meas3SatSystem::Gps, 0);
let reference_sat = ReferenceSatellite::default();
let bf1 = (1u32 << 6) | (1u32 << 28);
let pr_rate_64mm_s = 0x1234i16;
let mut buf = Vec::new();
buf.extend_from_slice(&bf1.to_le_bytes());
buf.extend_from_slice(&10_000u32.to_le_bytes());
buf.extend_from_slice(&((1u16 << 6) | (20u16) | (1u16 << 15)).to_le_bytes());
buf.push(0x55);
buf.extend_from_slice(&pr_rate_64mm_s.to_le_bytes());
let (_, _, _, decoded_pr_rate, consumed) = decode_master(
&buf,
MasterDecodeContext {
signal_table: &signal_table,
glonass_fn: 0,
short_pr_base_m: PR_BASE_M[Meas3SatSystem::Gps as usize],
sig_idx_master_short: 0,
reference_sat: &reference_sat,
time_since_ref_epoch_ms: 0,
pr_rate_available: true,
},
)
.unwrap();
assert_eq!(decoded_pr_rate, pr_rate_64mm_s);
assert_eq!(consumed, 13);
}
#[test]
fn decode_master_long_with_cont_requires_full_pr_rate_field() {
let signal_table = prepare_signal_table(Meas3SatSystem::Gps, 0);
let reference_sat = ReferenceSatellite::default();
let bf1 = (1u32 << 6) | (1u32 << 28);
let mut buf = Vec::new();
buf.extend_from_slice(&bf1.to_le_bytes());
buf.extend_from_slice(&10_000u32.to_le_bytes());
buf.extend_from_slice(&((1u16 << 6) | (20u16) | (1u16 << 15)).to_le_bytes());
buf.push(0x55);
buf.push(0x34);
let err = decode_master(
&buf,
MasterDecodeContext {
signal_table: &signal_table,
glonass_fn: 0,
short_pr_base_m: PR_BASE_M[Meas3SatSystem::Gps as usize],
sig_idx_master_short: 0,
reference_sat: &reference_sat,
time_since_ref_epoch_ms: 0,
pr_rate_available: true,
},
)
.unwrap_err();
assert!(err.to_string().contains("Meas3 master-long is truncated"));
}
#[test]
fn master_doppler_marks_three_byte_dnu_as_invalid() {
let doppler = parse_doppler_block(&encode_pr_rate((1 << 20) - 1, 3, true));
let mut meas = InternalMeasurement::default();
let mut idx = 0usize;
add_master_doppler(&mut meas, Some(&doppler), L1_WAVELENGTH, 0, &mut idx);
assert_eq!(idx, 3);
assert_eq!(meas.doppler_hz, F32_NOTVALID);
}
#[test]
fn slave_doppler_marks_four_byte_dnu_as_invalid() {
let doppler = parse_doppler_block(&encode_pr_rate((1 << 28) - 1, 4, true));
let master = InternalMeasurement {
doppler_hz: 125.0,
..Default::default()
};
let mut meas = InternalMeasurement::default();
let mut idx = 0usize;
add_slave_doppler(
&mut meas,
&master,
Some(&doppler),
L1_WAVELENGTH,
L2_WAVELENGTH,
&mut idx,
);
assert_eq!(idx, 4);
assert_eq!(meas.doppler_hz, F32_NOTVALID);
}
#[test]
fn meas3_block_set_collects_blocks() {
let mut set = Meas3BlockSet::default();
let body = [0u8; 6];
let raw = build_sbf_block(block_ids::MEAS3_RANGES, &body);
let (block, _) = SbfBlock::parse(&raw).unwrap();
assert!(set.insert_block(&block));
assert_eq!(set.tow_ms(), Some(1000));
}
#[test]
fn meas3_pp_keeps_section2_offset_in_payload() {
let body = [
0x05u8, 0x01, 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x21, 0x00,
];
let raw = build_sbf_block(block_ids::MEAS3_PP, &body);
let (block, _) = SbfBlock::parse(&raw).unwrap();
let SbfBlock::Meas3Pp(pp) = block else {
panic!("expected Meas3Pp");
};
let mut meas = InternalMeasurement {
cn0_dbhz: 34.0,
signal_type: SignalType::L1CA,
..Default::default()
};
let mut pp1_idx = 0usize;
let mut pp2_idx = 0usize;
add_pp_info(&mut meas, Some(&pp), &mut pp1_idx, &mut pp2_idx);
assert_eq!(pp.flags, 0x05);
assert_eq!(pp.antenna_id(), 0x05);
assert_eq!(meas.lock_count, 9);
assert_eq!(meas.raw_cn0_dbhz, 42);
assert_ne!(meas.flags & MEASFLAG_APMEINSYNC, 0);
assert_eq!(pp1_idx, 5);
assert_eq!(pp2_idx, 6);
}
}