use std::collections::{BTreeMap, BTreeSet};
use crate::astro::time::model::GnssWeekTow;
use crate::constants::{F_L1_HZ, GPS_EPOCH_TO_J2000_S, MEAN_EARTH_RADIUS_KM, SECONDS_PER_DAY};
use crate::error::{Error, Result};
use crate::frame::Wgs84Geodetic;
use crate::id::{GnssSatelliteId, GnssSystem};
use crate::ionex::pierce_point;
use crate::staleness::StalenessPolicy;
use crate::tolerances::SBAS_IGP_COORD_EPS_DEG;
use super::message::{
SbasGeoNav, SbasIgpMask, SbasIntegrity, SbasIonoDelays, SbasLongTermHalf, SbasLongTermRecord,
SbasMessage, SbasMixedCorrections,
};
const FAST_PRC_SCALE_M: f64 = 0.125;
const IONO_DELAY_SCALE_M: f64 = 0.125;
const LONG_POS_SCALE_M: f64 = 0.125;
const LONG_RATE_SCALE_M_S: f64 = 0.000625;
const LONG_AF0_SCALE_S: f64 = 1.0 / 2_147_483_648.0;
const LONG_AF1_SCALE_S_S: f64 = 1.0 / 549_755_813_888.0;
const GEO_XY_POS_SCALE_M: f64 = 0.08;
const GEO_Z_POS_SCALE_M: f64 = 0.4;
const GEO_XY_RATE_SCALE_M_S: f64 = 0.000625;
const GEO_Z_RATE_SCALE_M_S: f64 = 0.004;
const GEO_XY_ACCEL_SCALE_M_S2: f64 = 0.0000125;
const GEO_Z_ACCEL_SCALE_M_S2: f64 = 0.0000625;
const GEO_AF0_SCALE_S: f64 = 1.0 / 2_147_483_648.0;
const GEO_AF1_SCALE_S_S: f64 = 1.0 / 1_099_511_627_776.0;
const GEO_DISABLED_TIMEOUT_S: f64 = 60.0;
const SBAS_SHELL_HEIGHT_KM: f64 = 350.0;
#[derive(Clone, Debug, PartialEq)]
pub struct SbasFastCorrection {
pub prc_m: f64,
pub rrc_m_s: f64,
pub udrei: u8,
pub t_of_j2000_s: f64,
pub iodf: u8,
}
#[derive(Clone, Debug, PartialEq)]
pub struct SbasLongTermCorrection {
pub iode: u8,
pub delta_ecef_m: [f64; 3],
pub delta_ecef_rate_m_s: [f64; 3],
pub delta_af0_s: f64,
pub delta_af1_s_s: f64,
pub t0_j2000_s: f64,
}
#[derive(Clone, Debug, PartialEq)]
pub struct SbasIgp {
pub lat_deg: f64,
pub lon_deg: f64,
pub vertical_delay_m: f64,
pub give_variance_m2: Option<f64>,
}
#[derive(Clone, Debug, PartialEq, Default)]
pub struct SbasIonoGrid {
igps: Vec<SbasIgp>,
pub iodi: u8,
}
impl SbasIonoGrid {
pub fn new(igps: Vec<SbasIgp>, iodi: u8) -> Self {
Self { igps, iodi }
}
pub fn igps(&self) -> &[SbasIgp] {
&self.igps
}
pub fn slant_delay_m(
&self,
receiver: Wgs84Geodetic,
elevation_rad: f64,
azimuth_rad: f64,
frequency_hz: f64,
) -> Option<f64> {
if self.igps.is_empty() || !frequency_hz.is_finite() || frequency_hz <= 0.0 {
return None;
}
let geom = pierce_point(
receiver.lat_rad,
receiver.lon_rad,
azimuth_rad,
elevation_rad,
MEAN_EARTH_RADIUS_KM,
SBAS_SHELL_HEIGHT_KM,
);
let vertical_delay_m =
self.vertical_delay_at_ipp(geom.phi_ipp_deg, normalize_lon(geom.lambda_ipp_deg))?;
let mapping = 1.0 / (1.0 - geom.s * geom.s).sqrt();
let frequency_scale = (F_L1_HZ / frequency_hz) * (F_L1_HZ / frequency_hz);
Some(vertical_delay_m * mapping * frequency_scale)
}
fn vertical_delay_at_ipp(&self, lat_deg: f64, lon_deg: f64) -> Option<f64> {
let mut lats: Vec<f64> = self.igps.iter().map(|p| p.lat_deg).collect();
lats.sort_by(f64_total_cmp);
lats.dedup_by(|a, b| (*a - *b).abs() < SBAS_IGP_COORD_EPS_DEG);
let mut lons: Vec<f64> = self.igps.iter().map(|p| normalize_lon(p.lon_deg)).collect();
lons.sort_by(f64_total_cmp);
lons.dedup_by(|a, b| (*a - *b).abs() < SBAS_IGP_COORD_EPS_DEG);
let (lat0, lat1) = bracket_pair(&lats, lat_deg)?;
let (lon0, lon1) = bracket_pair(&lons, lon_deg)?;
if (lat1 - lat0).abs() < f64::EPSILON || (lon1 - lon0).abs() < f64::EPSILON {
return None;
}
let corners = [
self.find_igp(lat0, lon0),
self.find_igp(lat0, lon1),
self.find_igp(lat1, lon0),
self.find_igp(lat1, lon1),
];
let active: Vec<SbasIgp> = corners.into_iter().flatten().collect();
match active.len() {
4 => {
let q = (lat_deg - lat0) / (lat1 - lat0);
let p = (lon_deg - lon0) / (lon1 - lon0);
let e00 = active_point(&active, lat0, lon0)?.vertical_delay_m;
let e01 = active_point(&active, lat0, lon1)?.vertical_delay_m;
let e10 = active_point(&active, lat1, lon0)?.vertical_delay_m;
let e11 = active_point(&active, lat1, lon1)?.vertical_delay_m;
Some(
(1.0 - p) * (1.0 - q) * e00
+ p * (1.0 - q) * e01
+ (1.0 - p) * q * e10
+ p * q * e11,
)
}
3 => plane_interpolate(&active, lat_deg, lon_deg),
_ => None,
}
}
fn find_igp(&self, lat_deg: f64, lon_deg: f64) -> Option<SbasIgp> {
self.igps
.iter()
.find(|p| {
(p.lat_deg - lat_deg).abs() < SBAS_IGP_COORD_EPS_DEG
&& (normalize_lon(p.lon_deg) - lon_deg).abs() < SBAS_IGP_COORD_EPS_DEG
})
.cloned()
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct SbasGeoState {
pub position_ecef_m: [f64; 3],
pub velocity_ecef_m_s: [f64; 3],
pub acceleration_ecef_m_s2: [f64; 3],
pub clock_offset_s: f64,
pub clock_drift_s_s: f64,
pub t0_j2000_s: f64,
}
impl SbasGeoState {
pub fn state_at(&self, t_j2000_s: f64) -> ([f64; 3], f64) {
let dt = t_j2000_s - self.t0_j2000_s;
let dt2 = dt * dt;
let position = [
self.position_ecef_m[0]
+ self.velocity_ecef_m_s[0] * dt
+ 0.5 * self.acceleration_ecef_m_s2[0] * dt2,
self.position_ecef_m[1]
+ self.velocity_ecef_m_s[1] * dt
+ 0.5 * self.acceleration_ecef_m_s2[1] * dt2,
self.position_ecef_m[2]
+ self.velocity_ecef_m_s[2] * dt
+ 0.5 * self.acceleration_ecef_m_s2[2] * dt2,
];
let clock = self.clock_offset_s + self.clock_drift_s_s * dt;
(position, clock)
}
}
#[derive(Debug)]
pub struct SbasCorrectionStore {
partitions: BTreeMap<GnssSatelliteId, GeoPartition>,
policy: StalenessPolicy,
allow_partial: bool,
}
impl Default for SbasCorrectionStore {
fn default() -> Self {
Self::new()
}
}
impl SbasCorrectionStore {
pub fn new() -> Self {
Self {
partitions: BTreeMap::new(),
policy: StalenessPolicy::seconds(360.0),
allow_partial: false,
}
}
pub fn ingest(
&mut self,
message: &SbasMessage,
geo: GnssSatelliteId,
epoch: GnssWeekTow,
) -> Result<()> {
if geo.system != GnssSystem::Sbas {
return Err(Error::InvalidInput(
"SBAS source GEO must be an SBAS id".to_string(),
));
}
let epoch_j2000_s = epoch_to_j2000_s(epoch);
let partition = self.partitions.entry(geo).or_default();
partition.last_update_j2000_s = epoch_j2000_s;
match message {
SbasMessage::DoNotUse(_) => {
partition.disabled_until_j2000_s = Some(epoch_j2000_s + GEO_DISABLED_TIMEOUT_S);
}
SbasMessage::PrnMask(mask) => {
partition.active_iodp = Some(mask.iodp);
partition
.masks
.insert(mask.iodp, resolve_prn_mask(&mask.mask));
}
SbasMessage::FastCorrections(fast) => {
ingest_fast(
partition,
fast.message_type,
fast.iodf,
fast.iodp,
&fast.prc,
&fast.udrei,
epoch_j2000_s,
);
}
SbasMessage::Integrity(integrity) => {
ingest_integrity(partition, integrity);
}
SbasMessage::FastDegradation(degradation) => {
if Some(degradation.iodp) == partition.active_iodp {
partition.system_latency_s = f64::from(degradation.system_latency_s);
}
}
SbasMessage::GeoNav(geo_nav) => {
partition.geo_nav = Some(Timed {
value: geo_state_from_message(geo_nav, epoch),
epoch_j2000_s,
});
}
SbasMessage::IgpMask(mask) => {
partition
.igp_masks
.insert((mask.band_number, mask.iodi), mask.clone());
}
SbasMessage::IonoDelays(delays) => {
ingest_iono(partition, delays, epoch_j2000_s);
}
SbasMessage::MixedCorrections(mixed) => {
ingest_mixed(partition, mixed, epoch, epoch_j2000_s);
}
SbasMessage::LongTermCorrections(long) => {
for half in &long.halves {
ingest_long_half(partition, half, epoch, epoch_j2000_s);
}
}
SbasMessage::NetworkTime(_)
| SbasMessage::GeoAlmanac(_)
| SbasMessage::Unsupported(_) => {}
}
Ok(())
}
pub fn ready_geos(&self, t_j2000_s: f64) -> Vec<GnssSatelliteId> {
let mut geos: Vec<(GnssSatelliteId, f64)> = self
.partitions
.iter()
.filter(|(_, p)| !p.is_disabled(t_j2000_s))
.filter(|(_, p)| p.active_iodp.is_some())
.filter(|(_, p)| p.iono_grid.is_some() || p.geo_nav.is_some() || !p.fast.is_empty())
.map(|(&geo, p)| (geo, p.last_update_j2000_s))
.collect();
geos.sort_by(|a, b| f64_total_cmp(&b.1, &a.1));
geos.into_iter().map(|(geo, _)| geo).collect()
}
pub fn fast(&self, geo: GnssSatelliteId, sat: GnssSatelliteId) -> Option<&SbasFastCorrection> {
self.partitions.get(&geo)?.fast.get(&sat).map(|t| &t.value)
}
pub fn long_term(
&self,
geo: GnssSatelliteId,
sat: GnssSatelliteId,
) -> Option<&SbasLongTermCorrection> {
self.partitions
.get(&geo)?
.long_term
.get(&sat)
.map(|t| &t.value)
}
pub fn iono_grid(&self, geo: GnssSatelliteId) -> Option<&SbasIonoGrid> {
let partition = self.partitions.get(&geo)?;
if partition.is_disabled(partition.last_update_j2000_s) {
return None;
}
partition.iono_grid.as_ref().map(|t| &t.value)
}
pub fn geo_nav(&self, geo: GnssSatelliteId) -> Option<&SbasGeoState> {
self.partitions
.get(&geo)?
.geo_nav
.as_ref()
.map(|t| &t.value)
}
pub fn with_policy(mut self, policy: StalenessPolicy) -> Self {
self.policy = policy;
self
}
pub fn allow_partial(mut self, yes: bool) -> Self {
self.allow_partial = yes;
self
}
pub(crate) fn fresh_fast(
&self,
geo: GnssSatelliteId,
sat: GnssSatelliteId,
t_j2000_s: f64,
) -> Option<&SbasFastCorrection> {
let p = self.partitions.get(&geo)?;
let timed = p.fast.get(&sat)?;
self.fresh(timed.epoch_j2000_s, t_j2000_s)
.then_some(&timed.value)
}
pub(crate) fn fresh_long_term(
&self,
geo: GnssSatelliteId,
sat: GnssSatelliteId,
t_j2000_s: f64,
) -> Option<&SbasLongTermCorrection> {
let p = self.partitions.get(&geo)?;
let timed = p.long_term.get(&sat)?;
self.fresh(timed.epoch_j2000_s, t_j2000_s)
.then_some(&timed.value)
}
pub(crate) fn fresh_geo_nav(
&self,
geo: GnssSatelliteId,
t_j2000_s: f64,
) -> Option<&SbasGeoState> {
let timed = self.partitions.get(&geo)?.geo_nav.as_ref()?;
self.fresh(timed.epoch_j2000_s, t_j2000_s)
.then_some(&timed.value)
}
pub(crate) fn is_disabled(&self, geo: GnssSatelliteId, t_j2000_s: f64) -> bool {
self.partitions
.get(&geo)
.is_some_and(|p| p.is_disabled(t_j2000_s))
}
pub(crate) fn is_withdrawn(&self, geo: GnssSatelliteId, sat: GnssSatelliteId) -> bool {
self.partitions
.get(&geo)
.is_some_and(|p| p.withdrawn.contains(&sat))
}
pub(crate) fn allow_partial_corrections(&self) -> bool {
self.allow_partial
}
#[cfg(test)]
pub(crate) fn insert_long_term_for_test(
&mut self,
geo: GnssSatelliteId,
sat: GnssSatelliteId,
correction: SbasLongTermCorrection,
epoch_j2000_s: f64,
) {
self.partitions.entry(geo).or_default().long_term.insert(
sat,
Timed {
value: correction,
epoch_j2000_s,
},
);
}
fn fresh(&self, source_j2000_s: f64, t_j2000_s: f64) -> bool {
(t_j2000_s - source_j2000_s).abs() <= self.policy.max_staleness_s
}
}
#[derive(Debug, Default)]
struct GeoPartition {
active_iodp: Option<u8>,
masks: BTreeMap<u8, Vec<GnssSatelliteId>>,
fast: BTreeMap<GnssSatelliteId, Timed<SbasFastCorrection>>,
previous_fast: BTreeMap<GnssSatelliteId, SbasFastCorrection>,
long_term: BTreeMap<GnssSatelliteId, Timed<SbasLongTermCorrection>>,
igp_masks: BTreeMap<(u8, u8), SbasIgpMask>,
iono_grid: Option<Timed<SbasIonoGrid>>,
geo_nav: Option<Timed<SbasGeoState>>,
withdrawn: BTreeSet<GnssSatelliteId>,
system_latency_s: f64,
disabled_until_j2000_s: Option<f64>,
last_update_j2000_s: f64,
}
impl GeoPartition {
fn is_disabled(&self, t_j2000_s: f64) -> bool {
self.disabled_until_j2000_s
.is_some_and(|until| t_j2000_s <= until)
}
}
#[derive(Debug)]
struct Timed<T> {
value: T,
epoch_j2000_s: f64,
}
fn ingest_fast(
partition: &mut GeoPartition,
message_type: u8,
iodf: u8,
iodp: u8,
prc: &[i16],
udrei: &[u8],
epoch_j2000_s: f64,
) {
if Some(iodp) != partition.active_iodp {
return;
}
let start = match message_type {
2..=5 => usize::from(message_type - 2) * 13,
_ => 0,
};
for (i, (&prc_raw, &udrei)) in prc.iter().zip(udrei.iter()).enumerate() {
let Some(sat) = monitored_sat(partition, iodp, start + i) else {
continue;
};
if udrei >= 14 {
partition.withdrawn.insert(sat);
continue;
}
let t_of_j2000_s = epoch_j2000_s + partition.system_latency_s;
let prc_m = f64::from(prc_raw) * FAST_PRC_SCALE_M;
let rrc_m_s = partition
.previous_fast
.get(&sat)
.filter(|prev| prev.iodf == iodf)
.and_then(|prev| {
let dt = t_of_j2000_s - prev.t_of_j2000_s;
(dt > 0.0).then_some((prc_m - prev.prc_m) / dt)
})
.unwrap_or(0.0);
let correction = SbasFastCorrection {
prc_m,
rrc_m_s,
udrei,
t_of_j2000_s,
iodf,
};
partition.previous_fast.insert(sat, correction.clone());
partition.fast.insert(
sat,
Timed {
value: correction,
epoch_j2000_s: t_of_j2000_s,
},
);
partition.withdrawn.remove(&sat);
}
}
fn ingest_integrity(partition: &mut GeoPartition, integrity: &SbasIntegrity) {
let Some(iodp) = partition.active_iodp else {
return;
};
for (idx, &udrei) in integrity.udrei.iter().enumerate() {
let Some(sat) = monitored_sat(partition, iodp, idx) else {
continue;
};
let block = idx / 13;
let Some(fast) = partition.fast.get_mut(&sat) else {
continue;
};
if integrity.iodf[block] != fast.value.iodf {
continue;
}
fast.value.udrei = udrei;
if udrei >= 14 {
partition.withdrawn.insert(sat);
}
}
}
fn ingest_mixed(
partition: &mut GeoPartition,
mixed: &SbasMixedCorrections,
epoch: GnssWeekTow,
epoch_j2000_s: f64,
) {
ingest_fast(
partition,
2 + mixed.fast.block_id.min(3),
mixed.fast.iodf,
mixed.fast.iodp,
&mixed.fast.prc,
&mixed.fast.udrei,
epoch_j2000_s,
);
ingest_long_half(partition, &mixed.long_term, epoch, epoch_j2000_s);
}
fn ingest_long_half(
partition: &mut GeoPartition,
half: &SbasLongTermHalf,
epoch: GnssWeekTow,
epoch_j2000_s: f64,
) {
if Some(half.iodp) != partition.active_iodp {
return;
}
for record in &half.records {
let Some(sat) = monitored_sat(
partition,
half.iodp,
usize::from(record.monitored_index.saturating_sub(1)),
) else {
continue;
};
if sat.system != GnssSystem::Gps {
continue;
}
let t0_j2000_s = record
.time_of_day_s
.map(|tod| lift_time_of_day(epoch, f64::from(tod) * 16.0))
.unwrap_or(epoch_j2000_s);
let correction = long_record_to_correction(record, t0_j2000_s);
partition.long_term.insert(
sat,
Timed {
value: correction,
epoch_j2000_s,
},
);
}
}
fn ingest_iono(partition: &mut GeoPartition, delays: &SbasIonoDelays, epoch_j2000_s: f64) {
let Some(mask) = partition.igp_masks.get(&(delays.band_number, delays.iodi)) else {
return;
};
let active_positions: Vec<usize> = mask
.mask
.iter()
.enumerate()
.filter_map(|(idx, active)| active.then_some(idx))
.collect();
let start = usize::from(delays.block_id) * delays.entries.len();
let mut igps = partition
.iono_grid
.as_ref()
.filter(|grid| grid.value.iodi == delays.iodi)
.map(|grid| grid.value.igps.clone())
.unwrap_or_default();
for (slot, entry) in delays.entries.iter().enumerate() {
if entry.givei == 15 {
continue;
}
let Some(&position) = active_positions.get(start + slot) else {
continue;
};
let Some((lat_deg, lon_deg)) = igp_location(delays.band_number, position) else {
continue;
};
let point = SbasIgp {
lat_deg,
lon_deg,
vertical_delay_m: f64::from(entry.vertical_delay) * IONO_DELAY_SCALE_M,
give_variance_m2: None,
};
if let Some(existing) = igps.iter_mut().find(|p| {
(p.lat_deg - lat_deg).abs() < SBAS_IGP_COORD_EPS_DEG
&& (normalize_lon(p.lon_deg) - normalize_lon(lon_deg)).abs()
< SBAS_IGP_COORD_EPS_DEG
}) {
*existing = point;
} else {
igps.push(point);
}
}
partition.iono_grid = Some(Timed {
value: SbasIonoGrid::new(igps, delays.iodi),
epoch_j2000_s,
});
}
fn long_record_to_correction(
record: &SbasLongTermRecord,
t0_j2000_s: f64,
) -> SbasLongTermCorrection {
SbasLongTermCorrection {
iode: record.iode,
delta_ecef_m: [
f64::from(record.delta_x) * LONG_POS_SCALE_M,
f64::from(record.delta_y) * LONG_POS_SCALE_M,
f64::from(record.delta_z) * LONG_POS_SCALE_M,
],
delta_ecef_rate_m_s: [
f64::from(record.delta_x_rate) * LONG_RATE_SCALE_M_S,
f64::from(record.delta_y_rate) * LONG_RATE_SCALE_M_S,
f64::from(record.delta_z_rate) * LONG_RATE_SCALE_M_S,
],
delta_af0_s: f64::from(record.delta_a_f0) * LONG_AF0_SCALE_S,
delta_af1_s_s: f64::from(record.delta_a_f1) * LONG_AF1_SCALE_S_S,
t0_j2000_s,
}
}
fn geo_state_from_message(message: &SbasGeoNav, epoch: GnssWeekTow) -> SbasGeoState {
SbasGeoState {
position_ecef_m: [
f64::from(message.x_m) * GEO_XY_POS_SCALE_M,
f64::from(message.y_m) * GEO_XY_POS_SCALE_M,
f64::from(message.z_m) * GEO_Z_POS_SCALE_M,
],
velocity_ecef_m_s: [
f64::from(message.x_rate_m_s) * GEO_XY_RATE_SCALE_M_S,
f64::from(message.y_rate_m_s) * GEO_XY_RATE_SCALE_M_S,
f64::from(message.z_rate_m_s) * GEO_Z_RATE_SCALE_M_S,
],
acceleration_ecef_m_s2: [
f64::from(message.x_accel_m_s2) * GEO_XY_ACCEL_SCALE_M_S2,
f64::from(message.y_accel_m_s2) * GEO_XY_ACCEL_SCALE_M_S2,
f64::from(message.z_accel_m_s2) * GEO_Z_ACCEL_SCALE_M_S2,
],
clock_offset_s: f64::from(message.a_gf0_s) * GEO_AF0_SCALE_S,
clock_drift_s_s: f64::from(message.a_gf1_s_s) * GEO_AF1_SCALE_S_S,
t0_j2000_s: lift_time_of_day(epoch, f64::from(message.time_of_day_s) * 16.0),
}
}
fn monitored_sat(
partition: &GeoPartition,
iodp: u8,
zero_based_index: usize,
) -> Option<GnssSatelliteId> {
partition.masks.get(&iodp)?.get(zero_based_index).copied()
}
fn resolve_prn_mask(mask: &[bool; 210]) -> Vec<GnssSatelliteId> {
mask.iter()
.enumerate()
.filter_map(|(idx, active)| active.then(|| mask_position_to_sat(idx)).flatten())
.collect()
}
fn mask_position_to_sat(position: usize) -> Option<GnssSatelliteId> {
match position {
0..=31 => GnssSatelliteId::new(GnssSystem::Gps, (position + 1) as u8).ok(),
37..=63 => GnssSatelliteId::new(GnssSystem::Glonass, (position - 36) as u8).ok(),
119..=157 => sbas_prn_to_sat((position + 1) as u16),
_ => None,
}
}
pub fn sbas_prn_to_sat(broadcast_prn: u16) -> Option<GnssSatelliteId> {
let slot = broadcast_prn.checked_sub(100)?;
if (20..=58).contains(&slot) {
GnssSatelliteId::new(GnssSystem::Sbas, slot as u8).ok()
} else {
None
}
}
pub fn sat_to_sbas_prn(sat: GnssSatelliteId) -> Option<u16> {
(sat.system == GnssSystem::Sbas).then_some(u16::from(sat.prn) + 100)
}
fn epoch_to_j2000_s(epoch: GnssWeekTow) -> f64 {
f64::from(epoch.week) * crate::constants::SECONDS_PER_WEEK + epoch.tow_s - GPS_EPOCH_TO_J2000_S
}
fn lift_time_of_day(epoch: GnssWeekTow, time_of_day_s: f64) -> f64 {
let continuous = f64::from(epoch.week) * crate::constants::SECONDS_PER_WEEK + epoch.tow_s;
let day_start = (continuous / SECONDS_PER_DAY).floor() * SECONDS_PER_DAY;
let candidates = [
day_start - SECONDS_PER_DAY + time_of_day_s,
day_start + time_of_day_s,
day_start + SECONDS_PER_DAY + time_of_day_s,
];
let best = candidates
.into_iter()
.min_by(|a, b| f64_total_cmp(&(a - continuous).abs(), &(b - continuous).abs()))
.unwrap_or(day_start + time_of_day_s);
best - GPS_EPOCH_TO_J2000_S
}
#[derive(Clone, Copy)]
struct IgpBandSegment {
fixed_deg: i16,
variable_deg: &'static [i16],
first_bit: usize,
last_bit: usize,
}
const IGP_X1: [i16; 28] = [
-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 35,
40, 45, 50, 55, 65, 75, 85,
];
const IGP_X2: [i16; 23] = [
-55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50,
55,
];
const IGP_X3: [i16; 27] = [
-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 35,
40, 45, 50, 55, 65, 75,
];
const IGP_X4: [i16; 28] = [
-85, -75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30,
35, 40, 45, 50, 55, 65, 75,
];
const IGP_X5: [i16; 72] = [
-180, -175, -170, -165, -160, -155, -150, -145, -140, -135, -130, -125, -120, -115, -110, -105,
-100, -95, -90, -85, -80, -75, -70, -65, -60, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10,
-5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, 100, 105,
110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165, 170, 175,
];
const IGP_X6: [i16; 36] = [
-180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20,
-10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170,
];
const IGP_X7: [i16; 12] = [-180, -150, -120, -90, -60, -30, 0, 30, 60, 90, 120, 150];
const IGP_X8: [i16; 12] = [-170, -140, -110, -80, -50, -20, 10, 40, 70, 100, 130, 160];
const IGP_BANDS_LOW: [[IgpBandSegment; 8]; 9] = [
[
IgpBandSegment {
fixed_deg: -180,
variable_deg: &IGP_X1,
first_bit: 1,
last_bit: 28,
},
IgpBandSegment {
fixed_deg: -175,
variable_deg: &IGP_X2,
first_bit: 29,
last_bit: 51,
},
IgpBandSegment {
fixed_deg: -170,
variable_deg: &IGP_X3,
first_bit: 52,
last_bit: 78,
},
IgpBandSegment {
fixed_deg: -165,
variable_deg: &IGP_X2,
first_bit: 79,
last_bit: 101,
},
IgpBandSegment {
fixed_deg: -160,
variable_deg: &IGP_X3,
first_bit: 102,
last_bit: 128,
},
IgpBandSegment {
fixed_deg: -155,
variable_deg: &IGP_X2,
first_bit: 129,
last_bit: 151,
},
IgpBandSegment {
fixed_deg: -150,
variable_deg: &IGP_X3,
first_bit: 152,
last_bit: 178,
},
IgpBandSegment {
fixed_deg: -145,
variable_deg: &IGP_X2,
first_bit: 179,
last_bit: 201,
},
],
[
IgpBandSegment {
fixed_deg: -140,
variable_deg: &IGP_X4,
first_bit: 1,
last_bit: 28,
},
IgpBandSegment {
fixed_deg: -135,
variable_deg: &IGP_X2,
first_bit: 29,
last_bit: 51,
},
IgpBandSegment {
fixed_deg: -130,
variable_deg: &IGP_X3,
first_bit: 52,
last_bit: 78,
},
IgpBandSegment {
fixed_deg: -125,
variable_deg: &IGP_X2,
first_bit: 79,
last_bit: 101,
},
IgpBandSegment {
fixed_deg: -120,
variable_deg: &IGP_X3,
first_bit: 102,
last_bit: 128,
},
IgpBandSegment {
fixed_deg: -115,
variable_deg: &IGP_X2,
first_bit: 129,
last_bit: 151,
},
IgpBandSegment {
fixed_deg: -110,
variable_deg: &IGP_X3,
first_bit: 152,
last_bit: 178,
},
IgpBandSegment {
fixed_deg: -105,
variable_deg: &IGP_X2,
first_bit: 179,
last_bit: 201,
},
],
[
IgpBandSegment {
fixed_deg: -100,
variable_deg: &IGP_X3,
first_bit: 1,
last_bit: 27,
},
IgpBandSegment {
fixed_deg: -95,
variable_deg: &IGP_X2,
first_bit: 28,
last_bit: 50,
},
IgpBandSegment {
fixed_deg: -90,
variable_deg: &IGP_X1,
first_bit: 51,
last_bit: 78,
},
IgpBandSegment {
fixed_deg: -85,
variable_deg: &IGP_X2,
first_bit: 79,
last_bit: 101,
},
IgpBandSegment {
fixed_deg: -80,
variable_deg: &IGP_X3,
first_bit: 102,
last_bit: 128,
},
IgpBandSegment {
fixed_deg: -75,
variable_deg: &IGP_X2,
first_bit: 129,
last_bit: 151,
},
IgpBandSegment {
fixed_deg: -70,
variable_deg: &IGP_X3,
first_bit: 152,
last_bit: 178,
},
IgpBandSegment {
fixed_deg: -65,
variable_deg: &IGP_X2,
first_bit: 179,
last_bit: 201,
},
],
[
IgpBandSegment {
fixed_deg: -60,
variable_deg: &IGP_X3,
first_bit: 1,
last_bit: 27,
},
IgpBandSegment {
fixed_deg: -55,
variable_deg: &IGP_X2,
first_bit: 28,
last_bit: 50,
},
IgpBandSegment {
fixed_deg: -50,
variable_deg: &IGP_X4,
first_bit: 51,
last_bit: 78,
},
IgpBandSegment {
fixed_deg: -45,
variable_deg: &IGP_X2,
first_bit: 79,
last_bit: 101,
},
IgpBandSegment {
fixed_deg: -40,
variable_deg: &IGP_X3,
first_bit: 102,
last_bit: 128,
},
IgpBandSegment {
fixed_deg: -35,
variable_deg: &IGP_X2,
first_bit: 129,
last_bit: 151,
},
IgpBandSegment {
fixed_deg: -30,
variable_deg: &IGP_X3,
first_bit: 152,
last_bit: 178,
},
IgpBandSegment {
fixed_deg: -25,
variable_deg: &IGP_X2,
first_bit: 179,
last_bit: 201,
},
],
[
IgpBandSegment {
fixed_deg: -20,
variable_deg: &IGP_X3,
first_bit: 1,
last_bit: 27,
},
IgpBandSegment {
fixed_deg: -15,
variable_deg: &IGP_X2,
first_bit: 28,
last_bit: 50,
},
IgpBandSegment {
fixed_deg: -10,
variable_deg: &IGP_X3,
first_bit: 51,
last_bit: 77,
},
IgpBandSegment {
fixed_deg: -5,
variable_deg: &IGP_X2,
first_bit: 78,
last_bit: 100,
},
IgpBandSegment {
fixed_deg: 0,
variable_deg: &IGP_X1,
first_bit: 101,
last_bit: 128,
},
IgpBandSegment {
fixed_deg: 5,
variable_deg: &IGP_X2,
first_bit: 129,
last_bit: 151,
},
IgpBandSegment {
fixed_deg: 10,
variable_deg: &IGP_X3,
first_bit: 152,
last_bit: 178,
},
IgpBandSegment {
fixed_deg: 15,
variable_deg: &IGP_X2,
first_bit: 179,
last_bit: 201,
},
],
[
IgpBandSegment {
fixed_deg: 20,
variable_deg: &IGP_X3,
first_bit: 1,
last_bit: 27,
},
IgpBandSegment {
fixed_deg: 25,
variable_deg: &IGP_X2,
first_bit: 28,
last_bit: 50,
},
IgpBandSegment {
fixed_deg: 30,
variable_deg: &IGP_X3,
first_bit: 51,
last_bit: 77,
},
IgpBandSegment {
fixed_deg: 35,
variable_deg: &IGP_X2,
first_bit: 78,
last_bit: 100,
},
IgpBandSegment {
fixed_deg: 40,
variable_deg: &IGP_X4,
first_bit: 101,
last_bit: 128,
},
IgpBandSegment {
fixed_deg: 45,
variable_deg: &IGP_X2,
first_bit: 129,
last_bit: 151,
},
IgpBandSegment {
fixed_deg: 50,
variable_deg: &IGP_X3,
first_bit: 152,
last_bit: 178,
},
IgpBandSegment {
fixed_deg: 55,
variable_deg: &IGP_X2,
first_bit: 179,
last_bit: 201,
},
],
[
IgpBandSegment {
fixed_deg: 60,
variable_deg: &IGP_X3,
first_bit: 1,
last_bit: 27,
},
IgpBandSegment {
fixed_deg: 65,
variable_deg: &IGP_X2,
first_bit: 28,
last_bit: 50,
},
IgpBandSegment {
fixed_deg: 70,
variable_deg: &IGP_X3,
first_bit: 51,
last_bit: 77,
},
IgpBandSegment {
fixed_deg: 75,
variable_deg: &IGP_X2,
first_bit: 78,
last_bit: 100,
},
IgpBandSegment {
fixed_deg: 80,
variable_deg: &IGP_X3,
first_bit: 101,
last_bit: 127,
},
IgpBandSegment {
fixed_deg: 85,
variable_deg: &IGP_X2,
first_bit: 128,
last_bit: 150,
},
IgpBandSegment {
fixed_deg: 90,
variable_deg: &IGP_X1,
first_bit: 151,
last_bit: 178,
},
IgpBandSegment {
fixed_deg: 95,
variable_deg: &IGP_X2,
first_bit: 179,
last_bit: 201,
},
],
[
IgpBandSegment {
fixed_deg: 100,
variable_deg: &IGP_X3,
first_bit: 1,
last_bit: 27,
},
IgpBandSegment {
fixed_deg: 105,
variable_deg: &IGP_X2,
first_bit: 28,
last_bit: 50,
},
IgpBandSegment {
fixed_deg: 110,
variable_deg: &IGP_X3,
first_bit: 51,
last_bit: 77,
},
IgpBandSegment {
fixed_deg: 115,
variable_deg: &IGP_X2,
first_bit: 78,
last_bit: 100,
},
IgpBandSegment {
fixed_deg: 120,
variable_deg: &IGP_X3,
first_bit: 101,
last_bit: 127,
},
IgpBandSegment {
fixed_deg: 125,
variable_deg: &IGP_X2,
first_bit: 128,
last_bit: 150,
},
IgpBandSegment {
fixed_deg: 130,
variable_deg: &IGP_X4,
first_bit: 151,
last_bit: 178,
},
IgpBandSegment {
fixed_deg: 135,
variable_deg: &IGP_X2,
first_bit: 179,
last_bit: 201,
},
],
[
IgpBandSegment {
fixed_deg: 140,
variable_deg: &IGP_X3,
first_bit: 1,
last_bit: 27,
},
IgpBandSegment {
fixed_deg: 145,
variable_deg: &IGP_X2,
first_bit: 28,
last_bit: 50,
},
IgpBandSegment {
fixed_deg: 150,
variable_deg: &IGP_X3,
first_bit: 51,
last_bit: 77,
},
IgpBandSegment {
fixed_deg: 155,
variable_deg: &IGP_X2,
first_bit: 78,
last_bit: 100,
},
IgpBandSegment {
fixed_deg: 160,
variable_deg: &IGP_X3,
first_bit: 101,
last_bit: 127,
},
IgpBandSegment {
fixed_deg: 165,
variable_deg: &IGP_X2,
first_bit: 128,
last_bit: 150,
},
IgpBandSegment {
fixed_deg: 170,
variable_deg: &IGP_X3,
first_bit: 151,
last_bit: 177,
},
IgpBandSegment {
fixed_deg: 175,
variable_deg: &IGP_X2,
first_bit: 178,
last_bit: 200,
},
],
];
const IGP_BANDS_POLAR: [[IgpBandSegment; 5]; 2] = [
[
IgpBandSegment {
fixed_deg: 60,
variable_deg: &IGP_X5,
first_bit: 1,
last_bit: 72,
},
IgpBandSegment {
fixed_deg: 65,
variable_deg: &IGP_X6,
first_bit: 73,
last_bit: 108,
},
IgpBandSegment {
fixed_deg: 70,
variable_deg: &IGP_X6,
first_bit: 109,
last_bit: 144,
},
IgpBandSegment {
fixed_deg: 75,
variable_deg: &IGP_X6,
first_bit: 145,
last_bit: 180,
},
IgpBandSegment {
fixed_deg: 85,
variable_deg: &IGP_X7,
first_bit: 181,
last_bit: 192,
},
],
[
IgpBandSegment {
fixed_deg: -60,
variable_deg: &IGP_X5,
first_bit: 1,
last_bit: 72,
},
IgpBandSegment {
fixed_deg: -65,
variable_deg: &IGP_X6,
first_bit: 73,
last_bit: 108,
},
IgpBandSegment {
fixed_deg: -70,
variable_deg: &IGP_X6,
first_bit: 109,
last_bit: 144,
},
IgpBandSegment {
fixed_deg: -75,
variable_deg: &IGP_X6,
first_bit: 145,
last_bit: 180,
},
IgpBandSegment {
fixed_deg: -85,
variable_deg: &IGP_X8,
first_bit: 181,
last_bit: 192,
},
],
];
fn igp_location(band_number: u8, position: usize) -> Option<(f64, f64)> {
let one_based = position + 1;
if band_number <= 8 {
let segment = IGP_BANDS_LOW[usize::from(band_number)]
.iter()
.find(|segment| (segment.first_bit..=segment.last_bit).contains(&one_based))?;
let lat = segment.variable_deg[one_based - segment.first_bit];
return Some((f64::from(lat), f64::from(segment.fixed_deg)));
}
if (9..=10).contains(&band_number) {
let segment = IGP_BANDS_POLAR[usize::from(band_number - 9)]
.iter()
.find(|segment| (segment.first_bit..=segment.last_bit).contains(&one_based))?;
let lon = segment.variable_deg[one_based - segment.first_bit];
return Some((f64::from(segment.fixed_deg), f64::from(lon)));
}
None
}
fn bracket_pair(values: &[f64], value: f64) -> Option<(f64, f64)> {
if values.len() < 2 {
return None;
}
values.windows(2).find_map(|pair| {
let lo = pair[0];
let hi = pair[1];
(value >= lo && value <= hi).then_some((lo, hi))
})
}
fn active_point(points: &[SbasIgp], lat_deg: f64, lon_deg: f64) -> Option<&SbasIgp> {
points.iter().find(|p| {
(p.lat_deg - lat_deg).abs() < SBAS_IGP_COORD_EPS_DEG
&& (normalize_lon(p.lon_deg) - lon_deg).abs() < SBAS_IGP_COORD_EPS_DEG
})
}
fn plane_interpolate(points: &[SbasIgp], lat_deg: f64, lon_deg: f64) -> Option<f64> {
let [p0, p1, p2] = points else {
return None;
};
let x0 = p0.lon_deg;
let y0 = p0.lat_deg;
let z0 = p0.vertical_delay_m;
let x1 = p1.lon_deg;
let y1 = p1.lat_deg;
let z1 = p1.vertical_delay_m;
let x2 = p2.lon_deg;
let y2 = p2.lat_deg;
let z2 = p2.vertical_delay_m;
let det = x0 * (y1 - y2) + x1 * (y2 - y0) + x2 * (y0 - y1);
if det.abs() < 1.0e-12 {
return None;
}
let a = (z0 * (y1 - y2) + z1 * (y2 - y0) + z2 * (y0 - y1)) / det;
let b = (x0 * (z1 - z2) + x1 * (z2 - z0) + x2 * (z0 - z1)) / det;
let c = (x0 * (y1 * z2 - y2 * z1) + x1 * (y2 * z0 - y0 * z2) + x2 * (y0 * z1 - y1 * z0)) / det;
Some(a * lon_deg + b * lat_deg + c)
}
fn normalize_lon(mut lon_deg: f64) -> f64 {
while lon_deg < -180.0 {
lon_deg += 360.0;
}
while lon_deg > 180.0 {
lon_deg -= 360.0;
}
lon_deg
}
fn f64_total_cmp(a: &f64, b: &f64) -> std::cmp::Ordering {
a.total_cmp(b)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::astro::time::model::TimeScale;
use crate::sbas::message::{
SbasFastCorrections, SbasIgpDelay, SbasIgpMask, SbasIonoDelays, SbasPrnMask, SpareBits,
};
fn epoch(tow_s: f64) -> GnssWeekTow {
GnssWeekTow::new(TimeScale::Gpst, 2400, tow_s).expect("valid epoch")
}
fn geo() -> GnssSatelliteId {
sbas_prn_to_sat(120).expect("valid SBAS PRN")
}
fn gps(prn: u8) -> GnssSatelliteId {
GnssSatelliteId::new(GnssSystem::Gps, prn).expect("valid GPS PRN")
}
fn mask_message() -> SbasMessage {
let mut mask = [false; 210];
mask[0] = true;
mask[1] = true;
SbasMessage::PrnMask(SbasPrnMask {
preamble: 0x53,
iodp: 1,
mask,
reserved: SpareBits::new(),
})
}
#[test]
fn sbas_prn_helpers_map_broadcast_to_slot_form() {
assert_eq!(sbas_prn_to_sat(120), Some(geo()));
assert_eq!(sat_to_sbas_prn(geo()), Some(120));
assert_eq!(sbas_prn_to_sat(119), None);
}
#[test]
fn fast_corrections_scale_and_derive_rrc_for_same_issue() {
let mut store = SbasCorrectionStore::new();
store.ingest(&mask_message(), geo(), epoch(10.0)).unwrap();
let mut prc = [0i16; 13];
let udrei = [0u8; 13];
prc[0] = 8;
let first = SbasMessage::FastCorrections(SbasFastCorrections {
preamble: 0x53,
message_type: 2,
iodf: 1,
iodp: 1,
prc,
udrei,
reserved: SpareBits::new(),
});
store.ingest(&first, geo(), epoch(20.0)).unwrap();
prc[0] = 16;
let second = SbasMessage::FastCorrections(SbasFastCorrections {
preamble: 0x9A,
message_type: 2,
iodf: 1,
iodp: 1,
prc,
udrei,
reserved: SpareBits::new(),
});
store.ingest(&second, geo(), epoch(28.0)).unwrap();
let fast = store.fast(geo(), gps(1)).expect("fast correction");
assert_eq!(fast.prc_m.to_bits(), 2.0_f64.to_bits());
assert!((fast.rrc_m_s - 0.125).abs() < 1.0e-12);
}
#[test]
fn udrei_withdraws_satellite() {
let mut store = SbasCorrectionStore::new();
store.ingest(&mask_message(), geo(), epoch(10.0)).unwrap();
let prc = [0i16; 13];
let mut udrei = [0u8; 13];
udrei[0] = 15;
let msg = SbasMessage::FastCorrections(SbasFastCorrections {
preamble: 0x53,
message_type: 2,
iodf: 1,
iodp: 1,
prc,
udrei,
reserved: SpareBits::new(),
});
store.ingest(&msg, geo(), epoch(20.0)).unwrap();
assert!(store.is_withdrawn(geo(), gps(1)));
}
#[test]
fn igp_givei_15_is_not_added_to_grid() {
let mut store = SbasCorrectionStore::new();
let mut mask = [false; 201];
mask[0] = true;
mask[1] = true;
let igp_mask = SbasMessage::IgpMask(SbasIgpMask {
preamble: 0x53,
band_number: 0,
iodi: 2,
mask,
reserved: SpareBits::new(),
});
store.ingest(&igp_mask, geo(), epoch(0.0)).unwrap();
let mut entries: [SbasIgpDelay; 15] = core::array::from_fn(|_| SbasIgpDelay::default());
entries[0] = SbasIgpDelay {
vertical_delay: 8,
givei: 0,
};
entries[1] = SbasIgpDelay {
vertical_delay: 16,
givei: 15,
};
let delays = SbasMessage::IonoDelays(SbasIonoDelays {
preamble: 0x53,
band_number: 0,
block_id: 0,
iodi: 2,
entries,
reserved: SpareBits::new(),
});
store.ingest(&delays, geo(), epoch(1.0)).unwrap();
assert_eq!(store.iono_grid(geo()).unwrap().igps().len(), 1);
}
#[test]
fn igp_locations_match_do229_fixture() {
#[derive(serde::Deserialize)]
struct Fixture {
band: u8,
position: usize,
lat_deg: Option<f64>,
lon_deg: Option<f64>,
}
let fixtures: Vec<Fixture> =
serde_json::from_str(include_str!("../../tests/fixtures/sbas_igp_locations.json"))
.expect("valid IGP fixture");
for fixture in fixtures {
let got = igp_location(fixture.band, fixture.position);
match (fixture.lat_deg, fixture.lon_deg) {
(Some(lat), Some(lon)) => {
assert_eq!(got, Some((lat, lon)));
}
(None, None) => {
assert_eq!(got, None);
}
_ => panic!("fixture must contain both coordinates or neither"),
}
}
}
#[test]
fn iono_grid_interpolates_four_points() {
let grid = SbasIonoGrid::new(
vec![
SbasIgp {
lat_deg: 0.0,
lon_deg: 0.0,
vertical_delay_m: 1.0,
give_variance_m2: None,
},
SbasIgp {
lat_deg: 0.0,
lon_deg: 5.0,
vertical_delay_m: 2.0,
give_variance_m2: None,
},
SbasIgp {
lat_deg: 5.0,
lon_deg: 0.0,
vertical_delay_m: 3.0,
give_variance_m2: None,
},
SbasIgp {
lat_deg: 5.0,
lon_deg: 5.0,
vertical_delay_m: 4.0,
give_variance_m2: None,
},
],
0,
);
let vertical = grid.vertical_delay_at_ipp(2.5, 2.5).unwrap();
assert_eq!(vertical.to_bits(), 2.5_f64.to_bits());
}
}