1use std::collections::{BTreeMap, BTreeSet};
2
3use crate::astro::time::model::GnssWeekTow;
4use crate::constants::{F_L1_HZ, GPS_EPOCH_TO_J2000_S, MEAN_EARTH_RADIUS_KM, SECONDS_PER_DAY};
5use crate::error::{Error, Result};
6use crate::frame::Wgs84Geodetic;
7use crate::id::{GnssSatelliteId, GnssSystem};
8use crate::ionex::pierce_point;
9use crate::staleness::StalenessPolicy;
10use crate::tolerances::SBAS_IGP_COORD_EPS_DEG;
11
12use super::message::{
13 SbasGeoNav, SbasIgpMask, SbasIntegrity, SbasIonoDelays, SbasLongTermHalf, SbasLongTermRecord,
14 SbasMessage, SbasMixedCorrections,
15};
16
17const FAST_PRC_SCALE_M: f64 = 0.125;
18const IONO_DELAY_SCALE_M: f64 = 0.125;
19const LONG_POS_SCALE_M: f64 = 0.125;
20const LONG_RATE_SCALE_M_S: f64 = 0.000625;
21const LONG_AF0_SCALE_S: f64 = 1.0 / 2_147_483_648.0;
22const LONG_AF1_SCALE_S_S: f64 = 1.0 / 549_755_813_888.0;
23const GEO_XY_POS_SCALE_M: f64 = 0.08;
24const GEO_Z_POS_SCALE_M: f64 = 0.4;
25const GEO_XY_RATE_SCALE_M_S: f64 = 0.000625;
26const GEO_Z_RATE_SCALE_M_S: f64 = 0.004;
27const GEO_XY_ACCEL_SCALE_M_S2: f64 = 0.0000125;
28const GEO_Z_ACCEL_SCALE_M_S2: f64 = 0.0000625;
29const GEO_AF0_SCALE_S: f64 = 1.0 / 2_147_483_648.0;
30const GEO_AF1_SCALE_S_S: f64 = 1.0 / 1_099_511_627_776.0;
31const GEO_DISABLED_TIMEOUT_S: f64 = 60.0;
32const SBAS_SHELL_HEIGHT_KM: f64 = 350.0;
33
34#[derive(Clone, Debug, PartialEq)]
35pub struct SbasFastCorrection {
36 pub prc_m: f64,
37 pub rrc_m_s: f64,
38 pub udrei: u8,
39 pub t_of_j2000_s: f64,
40 pub iodf: u8,
41}
42
43#[derive(Clone, Debug, PartialEq)]
44pub struct SbasLongTermCorrection {
45 pub iode: u8,
46 pub delta_ecef_m: [f64; 3],
47 pub delta_ecef_rate_m_s: [f64; 3],
48 pub delta_af0_s: f64,
49 pub delta_af1_s_s: f64,
50 pub t0_j2000_s: f64,
51}
52
53#[derive(Clone, Debug, PartialEq)]
54pub struct SbasIgp {
55 pub lat_deg: f64,
56 pub lon_deg: f64,
57 pub vertical_delay_m: f64,
58 pub give_variance_m2: Option<f64>,
59}
60
61#[derive(Clone, Debug, PartialEq, Default)]
62pub struct SbasIonoGrid {
63 igps: Vec<SbasIgp>,
64 pub iodi: u8,
65}
66
67impl SbasIonoGrid {
68 pub fn new(igps: Vec<SbasIgp>, iodi: u8) -> Self {
69 Self { igps, iodi }
70 }
71
72 pub fn igps(&self) -> &[SbasIgp] {
73 &self.igps
74 }
75
76 pub fn slant_delay_m(
77 &self,
78 receiver: Wgs84Geodetic,
79 elevation_rad: f64,
80 azimuth_rad: f64,
81 frequency_hz: f64,
82 ) -> Option<f64> {
83 if self.igps.is_empty() || !frequency_hz.is_finite() || frequency_hz <= 0.0 {
84 return None;
85 }
86 let geom = pierce_point(
87 receiver.lat_rad,
88 receiver.lon_rad,
89 azimuth_rad,
90 elevation_rad,
91 MEAN_EARTH_RADIUS_KM,
92 SBAS_SHELL_HEIGHT_KM,
93 );
94 let vertical_delay_m =
95 self.vertical_delay_at_ipp(geom.phi_ipp_deg, normalize_lon(geom.lambda_ipp_deg))?;
96 let mapping = 1.0 / (1.0 - geom.s * geom.s).sqrt();
97 let frequency_scale = (F_L1_HZ / frequency_hz) * (F_L1_HZ / frequency_hz);
98 Some(vertical_delay_m * mapping * frequency_scale)
99 }
100
101 fn vertical_delay_at_ipp(&self, lat_deg: f64, lon_deg: f64) -> Option<f64> {
102 let mut lats: Vec<f64> = self.igps.iter().map(|p| p.lat_deg).collect();
103 lats.sort_by(f64_total_cmp);
104 lats.dedup_by(|a, b| (*a - *b).abs() < SBAS_IGP_COORD_EPS_DEG);
105 let mut lons: Vec<f64> = self.igps.iter().map(|p| normalize_lon(p.lon_deg)).collect();
106 lons.sort_by(f64_total_cmp);
107 lons.dedup_by(|a, b| (*a - *b).abs() < SBAS_IGP_COORD_EPS_DEG);
108 let (lat0, lat1) = bracket_pair(&lats, lat_deg)?;
109 let (lon0, lon1) = bracket_pair(&lons, lon_deg)?;
110 if (lat1 - lat0).abs() < f64::EPSILON || (lon1 - lon0).abs() < f64::EPSILON {
111 return None;
112 }
113
114 let corners = [
115 self.find_igp(lat0, lon0),
116 self.find_igp(lat0, lon1),
117 self.find_igp(lat1, lon0),
118 self.find_igp(lat1, lon1),
119 ];
120 let active: Vec<SbasIgp> = corners.into_iter().flatten().collect();
121 match active.len() {
122 4 => {
123 let q = (lat_deg - lat0) / (lat1 - lat0);
124 let p = (lon_deg - lon0) / (lon1 - lon0);
125 let e00 = active_point(&active, lat0, lon0)?.vertical_delay_m;
126 let e01 = active_point(&active, lat0, lon1)?.vertical_delay_m;
127 let e10 = active_point(&active, lat1, lon0)?.vertical_delay_m;
128 let e11 = active_point(&active, lat1, lon1)?.vertical_delay_m;
129 Some(
130 (1.0 - p) * (1.0 - q) * e00
131 + p * (1.0 - q) * e01
132 + (1.0 - p) * q * e10
133 + p * q * e11,
134 )
135 }
136 3 => plane_interpolate(&active, lat_deg, lon_deg),
137 _ => None,
138 }
139 }
140
141 fn find_igp(&self, lat_deg: f64, lon_deg: f64) -> Option<SbasIgp> {
142 self.igps
143 .iter()
144 .find(|p| {
145 (p.lat_deg - lat_deg).abs() < SBAS_IGP_COORD_EPS_DEG
146 && (normalize_lon(p.lon_deg) - lon_deg).abs() < SBAS_IGP_COORD_EPS_DEG
147 })
148 .cloned()
149 }
150}
151
152#[derive(Clone, Debug, PartialEq)]
153pub struct SbasGeoState {
154 pub position_ecef_m: [f64; 3],
155 pub velocity_ecef_m_s: [f64; 3],
156 pub acceleration_ecef_m_s2: [f64; 3],
157 pub clock_offset_s: f64,
158 pub clock_drift_s_s: f64,
159 pub t0_j2000_s: f64,
160}
161
162impl SbasGeoState {
163 pub fn state_at(&self, t_j2000_s: f64) -> ([f64; 3], f64) {
164 let dt = t_j2000_s - self.t0_j2000_s;
165 let dt2 = dt * dt;
166 let position = [
167 self.position_ecef_m[0]
168 + self.velocity_ecef_m_s[0] * dt
169 + 0.5 * self.acceleration_ecef_m_s2[0] * dt2,
170 self.position_ecef_m[1]
171 + self.velocity_ecef_m_s[1] * dt
172 + 0.5 * self.acceleration_ecef_m_s2[1] * dt2,
173 self.position_ecef_m[2]
174 + self.velocity_ecef_m_s[2] * dt
175 + 0.5 * self.acceleration_ecef_m_s2[2] * dt2,
176 ];
177 let clock = self.clock_offset_s + self.clock_drift_s_s * dt;
178 (position, clock)
179 }
180}
181
182#[derive(Debug)]
183pub struct SbasCorrectionStore {
184 partitions: BTreeMap<GnssSatelliteId, GeoPartition>,
185 policy: StalenessPolicy,
186 allow_partial: bool,
187}
188
189impl Default for SbasCorrectionStore {
190 fn default() -> Self {
191 Self::new()
192 }
193}
194
195impl SbasCorrectionStore {
196 pub fn new() -> Self {
197 Self {
198 partitions: BTreeMap::new(),
199 policy: StalenessPolicy::seconds(360.0),
200 allow_partial: false,
201 }
202 }
203
204 pub fn ingest(
205 &mut self,
206 message: &SbasMessage,
207 geo: GnssSatelliteId,
208 epoch: GnssWeekTow,
209 ) -> Result<()> {
210 if geo.system != GnssSystem::Sbas {
211 return Err(Error::InvalidInput(
212 "SBAS source GEO must be an SBAS id".to_string(),
213 ));
214 }
215 let epoch_j2000_s = epoch_to_j2000_s(epoch);
216 let partition = self.partitions.entry(geo).or_default();
217 partition.last_update_j2000_s = epoch_j2000_s;
218 match message {
219 SbasMessage::DoNotUse(_) => {
220 partition.disabled_until_j2000_s = Some(epoch_j2000_s + GEO_DISABLED_TIMEOUT_S);
221 }
222 SbasMessage::PrnMask(mask) => {
223 partition.active_iodp = Some(mask.iodp);
224 partition
225 .masks
226 .insert(mask.iodp, resolve_prn_mask(&mask.mask));
227 }
228 SbasMessage::FastCorrections(fast) => {
229 ingest_fast(
230 partition,
231 fast.message_type,
232 fast.iodf,
233 fast.iodp,
234 &fast.prc,
235 &fast.udrei,
236 epoch_j2000_s,
237 );
238 }
239 SbasMessage::Integrity(integrity) => {
240 ingest_integrity(partition, integrity);
241 }
242 SbasMessage::FastDegradation(degradation) => {
243 if Some(degradation.iodp) == partition.active_iodp {
244 partition.system_latency_s = f64::from(degradation.system_latency_s);
245 }
246 }
247 SbasMessage::GeoNav(geo_nav) => {
248 partition.geo_nav = Some(Timed {
249 value: geo_state_from_message(geo_nav, epoch),
250 epoch_j2000_s,
251 });
252 }
253 SbasMessage::IgpMask(mask) => {
254 partition
255 .igp_masks
256 .insert((mask.band_number, mask.iodi), mask.clone());
257 }
258 SbasMessage::IonoDelays(delays) => {
259 ingest_iono(partition, delays, epoch_j2000_s);
260 }
261 SbasMessage::MixedCorrections(mixed) => {
262 ingest_mixed(partition, mixed, epoch, epoch_j2000_s);
263 }
264 SbasMessage::LongTermCorrections(long) => {
265 for half in &long.halves {
266 ingest_long_half(partition, half, epoch, epoch_j2000_s);
267 }
268 }
269 SbasMessage::NetworkTime(_)
270 | SbasMessage::GeoAlmanac(_)
271 | SbasMessage::Unsupported(_) => {}
272 }
273 Ok(())
274 }
275
276 pub fn ready_geos(&self, t_j2000_s: f64) -> Vec<GnssSatelliteId> {
277 let mut geos: Vec<(GnssSatelliteId, f64)> = self
278 .partitions
279 .iter()
280 .filter(|(_, p)| !p.is_disabled(t_j2000_s))
281 .filter(|(_, p)| p.active_iodp.is_some())
282 .filter(|(_, p)| p.iono_grid.is_some() || p.geo_nav.is_some() || !p.fast.is_empty())
283 .map(|(&geo, p)| (geo, p.last_update_j2000_s))
284 .collect();
285 geos.sort_by(|a, b| f64_total_cmp(&b.1, &a.1));
286 geos.into_iter().map(|(geo, _)| geo).collect()
287 }
288
289 pub fn fast(&self, geo: GnssSatelliteId, sat: GnssSatelliteId) -> Option<&SbasFastCorrection> {
290 self.partitions.get(&geo)?.fast.get(&sat).map(|t| &t.value)
291 }
292
293 pub fn long_term(
294 &self,
295 geo: GnssSatelliteId,
296 sat: GnssSatelliteId,
297 ) -> Option<&SbasLongTermCorrection> {
298 self.partitions
299 .get(&geo)?
300 .long_term
301 .get(&sat)
302 .map(|t| &t.value)
303 }
304
305 pub fn iono_grid(&self, geo: GnssSatelliteId) -> Option<&SbasIonoGrid> {
306 let partition = self.partitions.get(&geo)?;
307 if partition.is_disabled(partition.last_update_j2000_s) {
308 return None;
309 }
310 partition.iono_grid.as_ref().map(|t| &t.value)
311 }
312
313 pub fn geo_nav(&self, geo: GnssSatelliteId) -> Option<&SbasGeoState> {
314 self.partitions
315 .get(&geo)?
316 .geo_nav
317 .as_ref()
318 .map(|t| &t.value)
319 }
320
321 pub fn with_policy(mut self, policy: StalenessPolicy) -> Self {
322 self.policy = policy;
323 self
324 }
325
326 pub fn allow_partial(mut self, yes: bool) -> Self {
327 self.allow_partial = yes;
328 self
329 }
330
331 pub(crate) fn fresh_fast(
332 &self,
333 geo: GnssSatelliteId,
334 sat: GnssSatelliteId,
335 t_j2000_s: f64,
336 ) -> Option<&SbasFastCorrection> {
337 let p = self.partitions.get(&geo)?;
338 let timed = p.fast.get(&sat)?;
339 self.fresh(timed.epoch_j2000_s, t_j2000_s)
340 .then_some(&timed.value)
341 }
342
343 pub(crate) fn fresh_long_term(
344 &self,
345 geo: GnssSatelliteId,
346 sat: GnssSatelliteId,
347 t_j2000_s: f64,
348 ) -> Option<&SbasLongTermCorrection> {
349 let p = self.partitions.get(&geo)?;
350 let timed = p.long_term.get(&sat)?;
351 self.fresh(timed.epoch_j2000_s, t_j2000_s)
352 .then_some(&timed.value)
353 }
354
355 pub(crate) fn fresh_geo_nav(
356 &self,
357 geo: GnssSatelliteId,
358 t_j2000_s: f64,
359 ) -> Option<&SbasGeoState> {
360 let timed = self.partitions.get(&geo)?.geo_nav.as_ref()?;
361 self.fresh(timed.epoch_j2000_s, t_j2000_s)
362 .then_some(&timed.value)
363 }
364
365 pub(crate) fn is_disabled(&self, geo: GnssSatelliteId, t_j2000_s: f64) -> bool {
366 self.partitions
367 .get(&geo)
368 .is_some_and(|p| p.is_disabled(t_j2000_s))
369 }
370
371 pub(crate) fn is_withdrawn(&self, geo: GnssSatelliteId, sat: GnssSatelliteId) -> bool {
372 self.partitions
373 .get(&geo)
374 .is_some_and(|p| p.withdrawn.contains(&sat))
375 }
376
377 pub(crate) fn allow_partial_corrections(&self) -> bool {
378 self.allow_partial
379 }
380
381 #[cfg(test)]
382 pub(crate) fn insert_long_term_for_test(
383 &mut self,
384 geo: GnssSatelliteId,
385 sat: GnssSatelliteId,
386 correction: SbasLongTermCorrection,
387 epoch_j2000_s: f64,
388 ) {
389 self.partitions.entry(geo).or_default().long_term.insert(
390 sat,
391 Timed {
392 value: correction,
393 epoch_j2000_s,
394 },
395 );
396 }
397
398 fn fresh(&self, source_j2000_s: f64, t_j2000_s: f64) -> bool {
399 (t_j2000_s - source_j2000_s).abs() <= self.policy.max_staleness_s
400 }
401}
402
403#[derive(Debug, Default)]
404struct GeoPartition {
405 active_iodp: Option<u8>,
406 masks: BTreeMap<u8, Vec<GnssSatelliteId>>,
407 fast: BTreeMap<GnssSatelliteId, Timed<SbasFastCorrection>>,
408 previous_fast: BTreeMap<GnssSatelliteId, SbasFastCorrection>,
409 long_term: BTreeMap<GnssSatelliteId, Timed<SbasLongTermCorrection>>,
410 igp_masks: BTreeMap<(u8, u8), SbasIgpMask>,
411 iono_grid: Option<Timed<SbasIonoGrid>>,
412 geo_nav: Option<Timed<SbasGeoState>>,
413 withdrawn: BTreeSet<GnssSatelliteId>,
414 system_latency_s: f64,
415 disabled_until_j2000_s: Option<f64>,
416 last_update_j2000_s: f64,
417}
418
419impl GeoPartition {
420 fn is_disabled(&self, t_j2000_s: f64) -> bool {
421 self.disabled_until_j2000_s
422 .is_some_and(|until| t_j2000_s <= until)
423 }
424}
425
426#[derive(Debug)]
427struct Timed<T> {
428 value: T,
429 epoch_j2000_s: f64,
430}
431
432fn ingest_fast(
433 partition: &mut GeoPartition,
434 message_type: u8,
435 iodf: u8,
436 iodp: u8,
437 prc: &[i16],
438 udrei: &[u8],
439 epoch_j2000_s: f64,
440) {
441 if Some(iodp) != partition.active_iodp {
442 return;
443 }
444 let start = match message_type {
445 2..=5 => usize::from(message_type - 2) * 13,
446 _ => 0,
447 };
448 for (i, (&prc_raw, &udrei)) in prc.iter().zip(udrei.iter()).enumerate() {
449 let Some(sat) = monitored_sat(partition, iodp, start + i) else {
450 continue;
451 };
452 if udrei >= 14 {
453 partition.withdrawn.insert(sat);
454 continue;
455 }
456 let t_of_j2000_s = epoch_j2000_s + partition.system_latency_s;
457 let prc_m = f64::from(prc_raw) * FAST_PRC_SCALE_M;
458 let rrc_m_s = partition
459 .previous_fast
460 .get(&sat)
461 .filter(|prev| prev.iodf == iodf)
462 .and_then(|prev| {
463 let dt = t_of_j2000_s - prev.t_of_j2000_s;
464 (dt > 0.0).then_some((prc_m - prev.prc_m) / dt)
465 })
466 .unwrap_or(0.0);
467 let correction = SbasFastCorrection {
468 prc_m,
469 rrc_m_s,
470 udrei,
471 t_of_j2000_s,
472 iodf,
473 };
474 partition.previous_fast.insert(sat, correction.clone());
475 partition.fast.insert(
476 sat,
477 Timed {
478 value: correction,
479 epoch_j2000_s: t_of_j2000_s,
480 },
481 );
482 partition.withdrawn.remove(&sat);
483 }
484}
485
486fn ingest_integrity(partition: &mut GeoPartition, integrity: &SbasIntegrity) {
487 let Some(iodp) = partition.active_iodp else {
488 return;
489 };
490 for (idx, &udrei) in integrity.udrei.iter().enumerate() {
491 let Some(sat) = monitored_sat(partition, iodp, idx) else {
492 continue;
493 };
494 let block = idx / 13;
495 let Some(fast) = partition.fast.get_mut(&sat) else {
496 continue;
497 };
498 if integrity.iodf[block] != fast.value.iodf {
499 continue;
500 }
501 fast.value.udrei = udrei;
502 if udrei >= 14 {
503 partition.withdrawn.insert(sat);
504 }
505 }
506}
507
508fn ingest_mixed(
509 partition: &mut GeoPartition,
510 mixed: &SbasMixedCorrections,
511 epoch: GnssWeekTow,
512 epoch_j2000_s: f64,
513) {
514 ingest_fast(
515 partition,
516 2 + mixed.fast.block_id.min(3),
517 mixed.fast.iodf,
518 mixed.fast.iodp,
519 &mixed.fast.prc,
520 &mixed.fast.udrei,
521 epoch_j2000_s,
522 );
523 ingest_long_half(partition, &mixed.long_term, epoch, epoch_j2000_s);
524}
525
526fn ingest_long_half(
527 partition: &mut GeoPartition,
528 half: &SbasLongTermHalf,
529 epoch: GnssWeekTow,
530 epoch_j2000_s: f64,
531) {
532 if Some(half.iodp) != partition.active_iodp {
533 return;
534 }
535 for record in &half.records {
536 let Some(sat) = monitored_sat(
537 partition,
538 half.iodp,
539 usize::from(record.monitored_index.saturating_sub(1)),
540 ) else {
541 continue;
542 };
543 if sat.system != GnssSystem::Gps {
544 continue;
545 }
546 let t0_j2000_s = record
547 .time_of_day_s
548 .map(|tod| lift_time_of_day(epoch, f64::from(tod) * 16.0))
549 .unwrap_or(epoch_j2000_s);
550 let correction = long_record_to_correction(record, t0_j2000_s);
551 partition.long_term.insert(
552 sat,
553 Timed {
554 value: correction,
555 epoch_j2000_s,
556 },
557 );
558 }
559}
560
561fn ingest_iono(partition: &mut GeoPartition, delays: &SbasIonoDelays, epoch_j2000_s: f64) {
562 let Some(mask) = partition.igp_masks.get(&(delays.band_number, delays.iodi)) else {
563 return;
564 };
565 let active_positions: Vec<usize> = mask
566 .mask
567 .iter()
568 .enumerate()
569 .filter_map(|(idx, active)| active.then_some(idx))
570 .collect();
571 let start = usize::from(delays.block_id) * delays.entries.len();
572 let mut igps = partition
573 .iono_grid
574 .as_ref()
575 .filter(|grid| grid.value.iodi == delays.iodi)
576 .map(|grid| grid.value.igps.clone())
577 .unwrap_or_default();
578 for (slot, entry) in delays.entries.iter().enumerate() {
579 if entry.givei == 15 {
580 continue;
581 }
582 let Some(&position) = active_positions.get(start + slot) else {
583 continue;
584 };
585 let Some((lat_deg, lon_deg)) = igp_location(delays.band_number, position) else {
586 continue;
587 };
588 let point = SbasIgp {
589 lat_deg,
590 lon_deg,
591 vertical_delay_m: f64::from(entry.vertical_delay) * IONO_DELAY_SCALE_M,
592 give_variance_m2: None,
593 };
594 if let Some(existing) = igps.iter_mut().find(|p| {
595 (p.lat_deg - lat_deg).abs() < SBAS_IGP_COORD_EPS_DEG
596 && (normalize_lon(p.lon_deg) - normalize_lon(lon_deg)).abs()
597 < SBAS_IGP_COORD_EPS_DEG
598 }) {
599 *existing = point;
600 } else {
601 igps.push(point);
602 }
603 }
604 partition.iono_grid = Some(Timed {
605 value: SbasIonoGrid::new(igps, delays.iodi),
606 epoch_j2000_s,
607 });
608}
609
610fn long_record_to_correction(
611 record: &SbasLongTermRecord,
612 t0_j2000_s: f64,
613) -> SbasLongTermCorrection {
614 SbasLongTermCorrection {
615 iode: record.iode,
616 delta_ecef_m: [
617 f64::from(record.delta_x) * LONG_POS_SCALE_M,
618 f64::from(record.delta_y) * LONG_POS_SCALE_M,
619 f64::from(record.delta_z) * LONG_POS_SCALE_M,
620 ],
621 delta_ecef_rate_m_s: [
622 f64::from(record.delta_x_rate) * LONG_RATE_SCALE_M_S,
623 f64::from(record.delta_y_rate) * LONG_RATE_SCALE_M_S,
624 f64::from(record.delta_z_rate) * LONG_RATE_SCALE_M_S,
625 ],
626 delta_af0_s: f64::from(record.delta_a_f0) * LONG_AF0_SCALE_S,
627 delta_af1_s_s: f64::from(record.delta_a_f1) * LONG_AF1_SCALE_S_S,
628 t0_j2000_s,
629 }
630}
631
632fn geo_state_from_message(message: &SbasGeoNav, epoch: GnssWeekTow) -> SbasGeoState {
633 SbasGeoState {
634 position_ecef_m: [
635 f64::from(message.x_m) * GEO_XY_POS_SCALE_M,
636 f64::from(message.y_m) * GEO_XY_POS_SCALE_M,
637 f64::from(message.z_m) * GEO_Z_POS_SCALE_M,
638 ],
639 velocity_ecef_m_s: [
640 f64::from(message.x_rate_m_s) * GEO_XY_RATE_SCALE_M_S,
641 f64::from(message.y_rate_m_s) * GEO_XY_RATE_SCALE_M_S,
642 f64::from(message.z_rate_m_s) * GEO_Z_RATE_SCALE_M_S,
643 ],
644 acceleration_ecef_m_s2: [
645 f64::from(message.x_accel_m_s2) * GEO_XY_ACCEL_SCALE_M_S2,
646 f64::from(message.y_accel_m_s2) * GEO_XY_ACCEL_SCALE_M_S2,
647 f64::from(message.z_accel_m_s2) * GEO_Z_ACCEL_SCALE_M_S2,
648 ],
649 clock_offset_s: f64::from(message.a_gf0_s) * GEO_AF0_SCALE_S,
650 clock_drift_s_s: f64::from(message.a_gf1_s_s) * GEO_AF1_SCALE_S_S,
651 t0_j2000_s: lift_time_of_day(epoch, f64::from(message.time_of_day_s) * 16.0),
652 }
653}
654
655fn monitored_sat(
656 partition: &GeoPartition,
657 iodp: u8,
658 zero_based_index: usize,
659) -> Option<GnssSatelliteId> {
660 partition.masks.get(&iodp)?.get(zero_based_index).copied()
661}
662
663fn resolve_prn_mask(mask: &[bool; 210]) -> Vec<GnssSatelliteId> {
664 mask.iter()
665 .enumerate()
666 .filter_map(|(idx, active)| active.then(|| mask_position_to_sat(idx)).flatten())
667 .collect()
668}
669
670fn mask_position_to_sat(position: usize) -> Option<GnssSatelliteId> {
671 match position {
672 0..=31 => GnssSatelliteId::new(GnssSystem::Gps, (position + 1) as u8).ok(),
673 37..=63 => GnssSatelliteId::new(GnssSystem::Glonass, (position - 36) as u8).ok(),
674 119..=157 => sbas_prn_to_sat((position + 1) as u16),
675 _ => None,
676 }
677}
678
679pub fn sbas_prn_to_sat(broadcast_prn: u16) -> Option<GnssSatelliteId> {
680 let slot = broadcast_prn.checked_sub(100)?;
681 if (20..=58).contains(&slot) {
682 GnssSatelliteId::new(GnssSystem::Sbas, slot as u8).ok()
683 } else {
684 None
685 }
686}
687
688pub fn sat_to_sbas_prn(sat: GnssSatelliteId) -> Option<u16> {
689 (sat.system == GnssSystem::Sbas).then_some(u16::from(sat.prn) + 100)
690}
691
692fn epoch_to_j2000_s(epoch: GnssWeekTow) -> f64 {
693 f64::from(epoch.week) * crate::constants::SECONDS_PER_WEEK + epoch.tow_s - GPS_EPOCH_TO_J2000_S
694}
695
696fn lift_time_of_day(epoch: GnssWeekTow, time_of_day_s: f64) -> f64 {
697 let continuous = f64::from(epoch.week) * crate::constants::SECONDS_PER_WEEK + epoch.tow_s;
698 let day_start = (continuous / SECONDS_PER_DAY).floor() * SECONDS_PER_DAY;
699 let candidates = [
700 day_start - SECONDS_PER_DAY + time_of_day_s,
701 day_start + time_of_day_s,
702 day_start + SECONDS_PER_DAY + time_of_day_s,
703 ];
704 let best = candidates
705 .into_iter()
706 .min_by(|a, b| f64_total_cmp(&(a - continuous).abs(), &(b - continuous).abs()))
707 .unwrap_or(day_start + time_of_day_s);
708 best - GPS_EPOCH_TO_J2000_S
709}
710
711#[derive(Clone, Copy)]
712struct IgpBandSegment {
713 fixed_deg: i16,
714 variable_deg: &'static [i16],
715 first_bit: usize,
716 last_bit: usize,
717}
718
719const IGP_X1: [i16; 28] = [
720 -75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 35,
721 40, 45, 50, 55, 65, 75, 85,
722];
723const IGP_X2: [i16; 23] = [
724 -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50,
725 55,
726];
727const IGP_X3: [i16; 27] = [
728 -75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 35,
729 40, 45, 50, 55, 65, 75,
730];
731const IGP_X4: [i16; 28] = [
732 -85, -75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30,
733 35, 40, 45, 50, 55, 65, 75,
734];
735const IGP_X5: [i16; 72] = [
736 -180, -175, -170, -165, -160, -155, -150, -145, -140, -135, -130, -125, -120, -115, -110, -105,
737 -100, -95, -90, -85, -80, -75, -70, -65, -60, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10,
738 -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, 100, 105,
739 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165, 170, 175,
740];
741const IGP_X6: [i16; 36] = [
742 -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20,
743 -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170,
744];
745const IGP_X7: [i16; 12] = [-180, -150, -120, -90, -60, -30, 0, 30, 60, 90, 120, 150];
746const IGP_X8: [i16; 12] = [-170, -140, -110, -80, -50, -20, 10, 40, 70, 100, 130, 160];
747
748const IGP_BANDS_LOW: [[IgpBandSegment; 8]; 9] = [
749 [
750 IgpBandSegment {
751 fixed_deg: -180,
752 variable_deg: &IGP_X1,
753 first_bit: 1,
754 last_bit: 28,
755 },
756 IgpBandSegment {
757 fixed_deg: -175,
758 variable_deg: &IGP_X2,
759 first_bit: 29,
760 last_bit: 51,
761 },
762 IgpBandSegment {
763 fixed_deg: -170,
764 variable_deg: &IGP_X3,
765 first_bit: 52,
766 last_bit: 78,
767 },
768 IgpBandSegment {
769 fixed_deg: -165,
770 variable_deg: &IGP_X2,
771 first_bit: 79,
772 last_bit: 101,
773 },
774 IgpBandSegment {
775 fixed_deg: -160,
776 variable_deg: &IGP_X3,
777 first_bit: 102,
778 last_bit: 128,
779 },
780 IgpBandSegment {
781 fixed_deg: -155,
782 variable_deg: &IGP_X2,
783 first_bit: 129,
784 last_bit: 151,
785 },
786 IgpBandSegment {
787 fixed_deg: -150,
788 variable_deg: &IGP_X3,
789 first_bit: 152,
790 last_bit: 178,
791 },
792 IgpBandSegment {
793 fixed_deg: -145,
794 variable_deg: &IGP_X2,
795 first_bit: 179,
796 last_bit: 201,
797 },
798 ],
799 [
800 IgpBandSegment {
801 fixed_deg: -140,
802 variable_deg: &IGP_X4,
803 first_bit: 1,
804 last_bit: 28,
805 },
806 IgpBandSegment {
807 fixed_deg: -135,
808 variable_deg: &IGP_X2,
809 first_bit: 29,
810 last_bit: 51,
811 },
812 IgpBandSegment {
813 fixed_deg: -130,
814 variable_deg: &IGP_X3,
815 first_bit: 52,
816 last_bit: 78,
817 },
818 IgpBandSegment {
819 fixed_deg: -125,
820 variable_deg: &IGP_X2,
821 first_bit: 79,
822 last_bit: 101,
823 },
824 IgpBandSegment {
825 fixed_deg: -120,
826 variable_deg: &IGP_X3,
827 first_bit: 102,
828 last_bit: 128,
829 },
830 IgpBandSegment {
831 fixed_deg: -115,
832 variable_deg: &IGP_X2,
833 first_bit: 129,
834 last_bit: 151,
835 },
836 IgpBandSegment {
837 fixed_deg: -110,
838 variable_deg: &IGP_X3,
839 first_bit: 152,
840 last_bit: 178,
841 },
842 IgpBandSegment {
843 fixed_deg: -105,
844 variable_deg: &IGP_X2,
845 first_bit: 179,
846 last_bit: 201,
847 },
848 ],
849 [
850 IgpBandSegment {
851 fixed_deg: -100,
852 variable_deg: &IGP_X3,
853 first_bit: 1,
854 last_bit: 27,
855 },
856 IgpBandSegment {
857 fixed_deg: -95,
858 variable_deg: &IGP_X2,
859 first_bit: 28,
860 last_bit: 50,
861 },
862 IgpBandSegment {
863 fixed_deg: -90,
864 variable_deg: &IGP_X1,
865 first_bit: 51,
866 last_bit: 78,
867 },
868 IgpBandSegment {
869 fixed_deg: -85,
870 variable_deg: &IGP_X2,
871 first_bit: 79,
872 last_bit: 101,
873 },
874 IgpBandSegment {
875 fixed_deg: -80,
876 variable_deg: &IGP_X3,
877 first_bit: 102,
878 last_bit: 128,
879 },
880 IgpBandSegment {
881 fixed_deg: -75,
882 variable_deg: &IGP_X2,
883 first_bit: 129,
884 last_bit: 151,
885 },
886 IgpBandSegment {
887 fixed_deg: -70,
888 variable_deg: &IGP_X3,
889 first_bit: 152,
890 last_bit: 178,
891 },
892 IgpBandSegment {
893 fixed_deg: -65,
894 variable_deg: &IGP_X2,
895 first_bit: 179,
896 last_bit: 201,
897 },
898 ],
899 [
900 IgpBandSegment {
901 fixed_deg: -60,
902 variable_deg: &IGP_X3,
903 first_bit: 1,
904 last_bit: 27,
905 },
906 IgpBandSegment {
907 fixed_deg: -55,
908 variable_deg: &IGP_X2,
909 first_bit: 28,
910 last_bit: 50,
911 },
912 IgpBandSegment {
913 fixed_deg: -50,
914 variable_deg: &IGP_X4,
915 first_bit: 51,
916 last_bit: 78,
917 },
918 IgpBandSegment {
919 fixed_deg: -45,
920 variable_deg: &IGP_X2,
921 first_bit: 79,
922 last_bit: 101,
923 },
924 IgpBandSegment {
925 fixed_deg: -40,
926 variable_deg: &IGP_X3,
927 first_bit: 102,
928 last_bit: 128,
929 },
930 IgpBandSegment {
931 fixed_deg: -35,
932 variable_deg: &IGP_X2,
933 first_bit: 129,
934 last_bit: 151,
935 },
936 IgpBandSegment {
937 fixed_deg: -30,
938 variable_deg: &IGP_X3,
939 first_bit: 152,
940 last_bit: 178,
941 },
942 IgpBandSegment {
943 fixed_deg: -25,
944 variable_deg: &IGP_X2,
945 first_bit: 179,
946 last_bit: 201,
947 },
948 ],
949 [
950 IgpBandSegment {
951 fixed_deg: -20,
952 variable_deg: &IGP_X3,
953 first_bit: 1,
954 last_bit: 27,
955 },
956 IgpBandSegment {
957 fixed_deg: -15,
958 variable_deg: &IGP_X2,
959 first_bit: 28,
960 last_bit: 50,
961 },
962 IgpBandSegment {
963 fixed_deg: -10,
964 variable_deg: &IGP_X3,
965 first_bit: 51,
966 last_bit: 77,
967 },
968 IgpBandSegment {
969 fixed_deg: -5,
970 variable_deg: &IGP_X2,
971 first_bit: 78,
972 last_bit: 100,
973 },
974 IgpBandSegment {
975 fixed_deg: 0,
976 variable_deg: &IGP_X1,
977 first_bit: 101,
978 last_bit: 128,
979 },
980 IgpBandSegment {
981 fixed_deg: 5,
982 variable_deg: &IGP_X2,
983 first_bit: 129,
984 last_bit: 151,
985 },
986 IgpBandSegment {
987 fixed_deg: 10,
988 variable_deg: &IGP_X3,
989 first_bit: 152,
990 last_bit: 178,
991 },
992 IgpBandSegment {
993 fixed_deg: 15,
994 variable_deg: &IGP_X2,
995 first_bit: 179,
996 last_bit: 201,
997 },
998 ],
999 [
1000 IgpBandSegment {
1001 fixed_deg: 20,
1002 variable_deg: &IGP_X3,
1003 first_bit: 1,
1004 last_bit: 27,
1005 },
1006 IgpBandSegment {
1007 fixed_deg: 25,
1008 variable_deg: &IGP_X2,
1009 first_bit: 28,
1010 last_bit: 50,
1011 },
1012 IgpBandSegment {
1013 fixed_deg: 30,
1014 variable_deg: &IGP_X3,
1015 first_bit: 51,
1016 last_bit: 77,
1017 },
1018 IgpBandSegment {
1019 fixed_deg: 35,
1020 variable_deg: &IGP_X2,
1021 first_bit: 78,
1022 last_bit: 100,
1023 },
1024 IgpBandSegment {
1025 fixed_deg: 40,
1026 variable_deg: &IGP_X4,
1027 first_bit: 101,
1028 last_bit: 128,
1029 },
1030 IgpBandSegment {
1031 fixed_deg: 45,
1032 variable_deg: &IGP_X2,
1033 first_bit: 129,
1034 last_bit: 151,
1035 },
1036 IgpBandSegment {
1037 fixed_deg: 50,
1038 variable_deg: &IGP_X3,
1039 first_bit: 152,
1040 last_bit: 178,
1041 },
1042 IgpBandSegment {
1043 fixed_deg: 55,
1044 variable_deg: &IGP_X2,
1045 first_bit: 179,
1046 last_bit: 201,
1047 },
1048 ],
1049 [
1050 IgpBandSegment {
1051 fixed_deg: 60,
1052 variable_deg: &IGP_X3,
1053 first_bit: 1,
1054 last_bit: 27,
1055 },
1056 IgpBandSegment {
1057 fixed_deg: 65,
1058 variable_deg: &IGP_X2,
1059 first_bit: 28,
1060 last_bit: 50,
1061 },
1062 IgpBandSegment {
1063 fixed_deg: 70,
1064 variable_deg: &IGP_X3,
1065 first_bit: 51,
1066 last_bit: 77,
1067 },
1068 IgpBandSegment {
1069 fixed_deg: 75,
1070 variable_deg: &IGP_X2,
1071 first_bit: 78,
1072 last_bit: 100,
1073 },
1074 IgpBandSegment {
1075 fixed_deg: 80,
1076 variable_deg: &IGP_X3,
1077 first_bit: 101,
1078 last_bit: 127,
1079 },
1080 IgpBandSegment {
1081 fixed_deg: 85,
1082 variable_deg: &IGP_X2,
1083 first_bit: 128,
1084 last_bit: 150,
1085 },
1086 IgpBandSegment {
1087 fixed_deg: 90,
1088 variable_deg: &IGP_X1,
1089 first_bit: 151,
1090 last_bit: 178,
1091 },
1092 IgpBandSegment {
1093 fixed_deg: 95,
1094 variable_deg: &IGP_X2,
1095 first_bit: 179,
1096 last_bit: 201,
1097 },
1098 ],
1099 [
1100 IgpBandSegment {
1101 fixed_deg: 100,
1102 variable_deg: &IGP_X3,
1103 first_bit: 1,
1104 last_bit: 27,
1105 },
1106 IgpBandSegment {
1107 fixed_deg: 105,
1108 variable_deg: &IGP_X2,
1109 first_bit: 28,
1110 last_bit: 50,
1111 },
1112 IgpBandSegment {
1113 fixed_deg: 110,
1114 variable_deg: &IGP_X3,
1115 first_bit: 51,
1116 last_bit: 77,
1117 },
1118 IgpBandSegment {
1119 fixed_deg: 115,
1120 variable_deg: &IGP_X2,
1121 first_bit: 78,
1122 last_bit: 100,
1123 },
1124 IgpBandSegment {
1125 fixed_deg: 120,
1126 variable_deg: &IGP_X3,
1127 first_bit: 101,
1128 last_bit: 127,
1129 },
1130 IgpBandSegment {
1131 fixed_deg: 125,
1132 variable_deg: &IGP_X2,
1133 first_bit: 128,
1134 last_bit: 150,
1135 },
1136 IgpBandSegment {
1137 fixed_deg: 130,
1138 variable_deg: &IGP_X4,
1139 first_bit: 151,
1140 last_bit: 178,
1141 },
1142 IgpBandSegment {
1143 fixed_deg: 135,
1144 variable_deg: &IGP_X2,
1145 first_bit: 179,
1146 last_bit: 201,
1147 },
1148 ],
1149 [
1150 IgpBandSegment {
1151 fixed_deg: 140,
1152 variable_deg: &IGP_X3,
1153 first_bit: 1,
1154 last_bit: 27,
1155 },
1156 IgpBandSegment {
1157 fixed_deg: 145,
1158 variable_deg: &IGP_X2,
1159 first_bit: 28,
1160 last_bit: 50,
1161 },
1162 IgpBandSegment {
1163 fixed_deg: 150,
1164 variable_deg: &IGP_X3,
1165 first_bit: 51,
1166 last_bit: 77,
1167 },
1168 IgpBandSegment {
1169 fixed_deg: 155,
1170 variable_deg: &IGP_X2,
1171 first_bit: 78,
1172 last_bit: 100,
1173 },
1174 IgpBandSegment {
1175 fixed_deg: 160,
1176 variable_deg: &IGP_X3,
1177 first_bit: 101,
1178 last_bit: 127,
1179 },
1180 IgpBandSegment {
1181 fixed_deg: 165,
1182 variable_deg: &IGP_X2,
1183 first_bit: 128,
1184 last_bit: 150,
1185 },
1186 IgpBandSegment {
1187 fixed_deg: 170,
1188 variable_deg: &IGP_X3,
1189 first_bit: 151,
1190 last_bit: 177,
1191 },
1192 IgpBandSegment {
1193 fixed_deg: 175,
1194 variable_deg: &IGP_X2,
1195 first_bit: 178,
1196 last_bit: 200,
1197 },
1198 ],
1199];
1200
1201const IGP_BANDS_POLAR: [[IgpBandSegment; 5]; 2] = [
1202 [
1203 IgpBandSegment {
1204 fixed_deg: 60,
1205 variable_deg: &IGP_X5,
1206 first_bit: 1,
1207 last_bit: 72,
1208 },
1209 IgpBandSegment {
1210 fixed_deg: 65,
1211 variable_deg: &IGP_X6,
1212 first_bit: 73,
1213 last_bit: 108,
1214 },
1215 IgpBandSegment {
1216 fixed_deg: 70,
1217 variable_deg: &IGP_X6,
1218 first_bit: 109,
1219 last_bit: 144,
1220 },
1221 IgpBandSegment {
1222 fixed_deg: 75,
1223 variable_deg: &IGP_X6,
1224 first_bit: 145,
1225 last_bit: 180,
1226 },
1227 IgpBandSegment {
1228 fixed_deg: 85,
1229 variable_deg: &IGP_X7,
1230 first_bit: 181,
1231 last_bit: 192,
1232 },
1233 ],
1234 [
1235 IgpBandSegment {
1236 fixed_deg: -60,
1237 variable_deg: &IGP_X5,
1238 first_bit: 1,
1239 last_bit: 72,
1240 },
1241 IgpBandSegment {
1242 fixed_deg: -65,
1243 variable_deg: &IGP_X6,
1244 first_bit: 73,
1245 last_bit: 108,
1246 },
1247 IgpBandSegment {
1248 fixed_deg: -70,
1249 variable_deg: &IGP_X6,
1250 first_bit: 109,
1251 last_bit: 144,
1252 },
1253 IgpBandSegment {
1254 fixed_deg: -75,
1255 variable_deg: &IGP_X6,
1256 first_bit: 145,
1257 last_bit: 180,
1258 },
1259 IgpBandSegment {
1260 fixed_deg: -85,
1261 variable_deg: &IGP_X8,
1262 first_bit: 181,
1263 last_bit: 192,
1264 },
1265 ],
1266];
1267
1268fn igp_location(band_number: u8, position: usize) -> Option<(f64, f64)> {
1269 let one_based = position + 1;
1270 if band_number <= 8 {
1271 let segment = IGP_BANDS_LOW[usize::from(band_number)]
1272 .iter()
1273 .find(|segment| (segment.first_bit..=segment.last_bit).contains(&one_based))?;
1274 let lat = segment.variable_deg[one_based - segment.first_bit];
1275 return Some((f64::from(lat), f64::from(segment.fixed_deg)));
1276 }
1277 if (9..=10).contains(&band_number) {
1278 let segment = IGP_BANDS_POLAR[usize::from(band_number - 9)]
1279 .iter()
1280 .find(|segment| (segment.first_bit..=segment.last_bit).contains(&one_based))?;
1281 let lon = segment.variable_deg[one_based - segment.first_bit];
1282 return Some((f64::from(segment.fixed_deg), f64::from(lon)));
1283 }
1284 None
1285}
1286
1287fn bracket_pair(values: &[f64], value: f64) -> Option<(f64, f64)> {
1288 if values.len() < 2 {
1289 return None;
1290 }
1291 values.windows(2).find_map(|pair| {
1292 let lo = pair[0];
1293 let hi = pair[1];
1294 (value >= lo && value <= hi).then_some((lo, hi))
1295 })
1296}
1297
1298fn active_point(points: &[SbasIgp], lat_deg: f64, lon_deg: f64) -> Option<&SbasIgp> {
1299 points.iter().find(|p| {
1300 (p.lat_deg - lat_deg).abs() < SBAS_IGP_COORD_EPS_DEG
1301 && (normalize_lon(p.lon_deg) - lon_deg).abs() < SBAS_IGP_COORD_EPS_DEG
1302 })
1303}
1304
1305fn plane_interpolate(points: &[SbasIgp], lat_deg: f64, lon_deg: f64) -> Option<f64> {
1306 let [p0, p1, p2] = points else {
1307 return None;
1308 };
1309 let x0 = p0.lon_deg;
1310 let y0 = p0.lat_deg;
1311 let z0 = p0.vertical_delay_m;
1312 let x1 = p1.lon_deg;
1313 let y1 = p1.lat_deg;
1314 let z1 = p1.vertical_delay_m;
1315 let x2 = p2.lon_deg;
1316 let y2 = p2.lat_deg;
1317 let z2 = p2.vertical_delay_m;
1318 let det = x0 * (y1 - y2) + x1 * (y2 - y0) + x2 * (y0 - y1);
1319 if det.abs() < 1.0e-12 {
1320 return None;
1321 }
1322 let a = (z0 * (y1 - y2) + z1 * (y2 - y0) + z2 * (y0 - y1)) / det;
1323 let b = (x0 * (z1 - z2) + x1 * (z2 - z0) + x2 * (z0 - z1)) / det;
1324 let c = (x0 * (y1 * z2 - y2 * z1) + x1 * (y2 * z0 - y0 * z2) + x2 * (y0 * z1 - y1 * z0)) / det;
1325 Some(a * lon_deg + b * lat_deg + c)
1326}
1327
1328fn normalize_lon(mut lon_deg: f64) -> f64 {
1329 while lon_deg < -180.0 {
1330 lon_deg += 360.0;
1331 }
1332 while lon_deg > 180.0 {
1333 lon_deg -= 360.0;
1334 }
1335 lon_deg
1336}
1337
1338fn f64_total_cmp(a: &f64, b: &f64) -> std::cmp::Ordering {
1339 a.total_cmp(b)
1340}
1341
1342#[cfg(test)]
1343mod tests {
1344 use super::*;
1345 use crate::astro::time::model::TimeScale;
1346 use crate::sbas::message::{
1347 SbasFastCorrections, SbasIgpDelay, SbasIgpMask, SbasIonoDelays, SbasPrnMask, SpareBits,
1348 };
1349
1350 fn epoch(tow_s: f64) -> GnssWeekTow {
1351 GnssWeekTow::new(TimeScale::Gpst, 2400, tow_s).expect("valid epoch")
1352 }
1353
1354 fn geo() -> GnssSatelliteId {
1355 sbas_prn_to_sat(120).expect("valid SBAS PRN")
1356 }
1357
1358 fn gps(prn: u8) -> GnssSatelliteId {
1359 GnssSatelliteId::new(GnssSystem::Gps, prn).expect("valid GPS PRN")
1360 }
1361
1362 fn mask_message() -> SbasMessage {
1363 let mut mask = [false; 210];
1364 mask[0] = true;
1365 mask[1] = true;
1366 SbasMessage::PrnMask(SbasPrnMask {
1367 preamble: 0x53,
1368 iodp: 1,
1369 mask,
1370 reserved: SpareBits::new(),
1371 })
1372 }
1373
1374 #[test]
1375 fn sbas_prn_helpers_map_broadcast_to_slot_form() {
1376 assert_eq!(sbas_prn_to_sat(120), Some(geo()));
1377 assert_eq!(sat_to_sbas_prn(geo()), Some(120));
1378 assert_eq!(sbas_prn_to_sat(119), None);
1379 }
1380
1381 #[test]
1382 fn fast_corrections_scale_and_derive_rrc_for_same_issue() {
1383 let mut store = SbasCorrectionStore::new();
1384 store.ingest(&mask_message(), geo(), epoch(10.0)).unwrap();
1385 let mut prc = [0i16; 13];
1386 let udrei = [0u8; 13];
1387 prc[0] = 8;
1388 let first = SbasMessage::FastCorrections(SbasFastCorrections {
1389 preamble: 0x53,
1390 message_type: 2,
1391 iodf: 1,
1392 iodp: 1,
1393 prc,
1394 udrei,
1395 reserved: SpareBits::new(),
1396 });
1397 store.ingest(&first, geo(), epoch(20.0)).unwrap();
1398 prc[0] = 16;
1399 let second = SbasMessage::FastCorrections(SbasFastCorrections {
1400 preamble: 0x9A,
1401 message_type: 2,
1402 iodf: 1,
1403 iodp: 1,
1404 prc,
1405 udrei,
1406 reserved: SpareBits::new(),
1407 });
1408 store.ingest(&second, geo(), epoch(28.0)).unwrap();
1409 let fast = store.fast(geo(), gps(1)).expect("fast correction");
1410 assert_eq!(fast.prc_m.to_bits(), 2.0_f64.to_bits());
1411 assert!((fast.rrc_m_s - 0.125).abs() < 1.0e-12);
1412 }
1413
1414 #[test]
1415 fn udrei_withdraws_satellite() {
1416 let mut store = SbasCorrectionStore::new();
1417 store.ingest(&mask_message(), geo(), epoch(10.0)).unwrap();
1418 let prc = [0i16; 13];
1419 let mut udrei = [0u8; 13];
1420 udrei[0] = 15;
1421 let msg = SbasMessage::FastCorrections(SbasFastCorrections {
1422 preamble: 0x53,
1423 message_type: 2,
1424 iodf: 1,
1425 iodp: 1,
1426 prc,
1427 udrei,
1428 reserved: SpareBits::new(),
1429 });
1430 store.ingest(&msg, geo(), epoch(20.0)).unwrap();
1431 assert!(store.is_withdrawn(geo(), gps(1)));
1432 }
1433
1434 #[test]
1435 fn igp_givei_15_is_not_added_to_grid() {
1436 let mut store = SbasCorrectionStore::new();
1437 let mut mask = [false; 201];
1438 mask[0] = true;
1439 mask[1] = true;
1440 let igp_mask = SbasMessage::IgpMask(SbasIgpMask {
1441 preamble: 0x53,
1442 band_number: 0,
1443 iodi: 2,
1444 mask,
1445 reserved: SpareBits::new(),
1446 });
1447 store.ingest(&igp_mask, geo(), epoch(0.0)).unwrap();
1448 let mut entries: [SbasIgpDelay; 15] = core::array::from_fn(|_| SbasIgpDelay::default());
1449 entries[0] = SbasIgpDelay {
1450 vertical_delay: 8,
1451 givei: 0,
1452 };
1453 entries[1] = SbasIgpDelay {
1454 vertical_delay: 16,
1455 givei: 15,
1456 };
1457 let delays = SbasMessage::IonoDelays(SbasIonoDelays {
1458 preamble: 0x53,
1459 band_number: 0,
1460 block_id: 0,
1461 iodi: 2,
1462 entries,
1463 reserved: SpareBits::new(),
1464 });
1465 store.ingest(&delays, geo(), epoch(1.0)).unwrap();
1466 assert_eq!(store.iono_grid(geo()).unwrap().igps().len(), 1);
1467 }
1468
1469 #[test]
1470 fn igp_locations_match_do229_fixture() {
1471 #[derive(serde::Deserialize)]
1472 struct Fixture {
1473 band: u8,
1474 position: usize,
1475 lat_deg: Option<f64>,
1476 lon_deg: Option<f64>,
1477 }
1478
1479 let fixtures: Vec<Fixture> =
1480 serde_json::from_str(include_str!("../../tests/fixtures/sbas_igp_locations.json"))
1481 .expect("valid IGP fixture");
1482 for fixture in fixtures {
1483 let got = igp_location(fixture.band, fixture.position);
1484 match (fixture.lat_deg, fixture.lon_deg) {
1485 (Some(lat), Some(lon)) => {
1486 assert_eq!(got, Some((lat, lon)));
1487 }
1488 (None, None) => {
1489 assert_eq!(got, None);
1490 }
1491 _ => panic!("fixture must contain both coordinates or neither"),
1492 }
1493 }
1494 }
1495
1496 #[test]
1497 fn iono_grid_interpolates_four_points() {
1498 let grid = SbasIonoGrid::new(
1499 vec![
1500 SbasIgp {
1501 lat_deg: 0.0,
1502 lon_deg: 0.0,
1503 vertical_delay_m: 1.0,
1504 give_variance_m2: None,
1505 },
1506 SbasIgp {
1507 lat_deg: 0.0,
1508 lon_deg: 5.0,
1509 vertical_delay_m: 2.0,
1510 give_variance_m2: None,
1511 },
1512 SbasIgp {
1513 lat_deg: 5.0,
1514 lon_deg: 0.0,
1515 vertical_delay_m: 3.0,
1516 give_variance_m2: None,
1517 },
1518 SbasIgp {
1519 lat_deg: 5.0,
1520 lon_deg: 5.0,
1521 vertical_delay_m: 4.0,
1522 give_variance_m2: None,
1523 },
1524 ],
1525 0,
1526 );
1527 let vertical = grid.vertical_delay_at_ipp(2.5, 2.5).unwrap();
1528 assert_eq!(vertical.to_bits(), 2.5_f64.to_bits());
1529 }
1530}