sidereon_core/rinex_nav/
store.rs

1//! Broadcast-store selection and SPP source adapter.
2
3use crate::broadcast::{
4    satellite_state, satellite_state_cnav, satellite_state_cnav_unchecked,
5    satellite_state_unchecked, CnavRates, SatelliteState,
6};
7use crate::constants::{
8    BDS_EPOCH_MINUS_GPS_EPOCH_S, GPST_MINUS_BDT_S, GPS_EPOCH_TO_J2000_S, SECONDS_PER_WEEK,
9};
10use crate::error::{Error, Result as CoreResult};
11use crate::glonass;
12use crate::id::{GnssSatelliteId, GnssSystem};
13use crate::spp::EphemerisSource;
14
15use super::{
16    cnav_ura_nominal_m, is_beidou_geo, parse_glonass, parse_iono_corrections_checked,
17    parse_leap_seconds_checked, parse_nav, BroadcastGroupDelays, BroadcastIssue, BroadcastRecord,
18    CnavParameters, GlonassRecord, IonoCorrections, NavMessage, NavParseError, GLONASS_MAX_AGE_S,
19    MAX_EPHEMERIS_AGE_S,
20};
21
22/// Which navigation-message generation a store prefers when a GPS/QZSS
23/// satellite has both legacy and CNAV-family records.
24#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
25pub enum NavMessagePreference {
26    /// Prefer legacy records and use CNAV-family records only as fallback.
27    #[default]
28    PreferLegacy,
29    /// Prefer CNAV-family records and use legacy records as fallback.
30    PreferModern,
31}
32
33/// A queryable set of parsed broadcast records, usable as an SPP
34/// [`EphemerisSource`].
35///
36/// For a satellite and epoch it selects the record whose reference time `toe` is
37/// nearest in **continuous** GPS time (week number times the week length plus
38/// the seconds of week, not the seconds of week alone), and rejects the query as
39/// having no ephemeris if it falls outside that record's validity window - half
40/// the broadcast GPS curve-fit interval, or the coarse [`MAX_EPHEMERIS_AGE_S`]
41/// fallback for systems that do not broadcast one - so a stale or wrong-week
42/// product cannot silently produce a position.
43///
44/// [`from_nav`](BroadcastStore::from_nav) applies a default usability policy:
45/// healthy GPS LNAV, GPS/QZSS CNAV-family, Galileo I/NAV, BeiDou D1/D2, and
46/// GLONASS records are kept, with CNAV no-prediction URA records excluded.
47/// [`new`](BroadcastStore::new) keeps records verbatim for callers that want
48/// their own policy.
49pub struct BroadcastStore {
50    records: Vec<BroadcastRecord>,
51    glonass: Vec<GlonassRecord>,
52    leap_seconds: Option<f64>,
53    iono: IonoCorrections,
54    message_preference: NavMessagePreference,
55}
56
57impl BroadcastStore {
58    /// Build a store from already-parsed Keplerian records, verbatim (no policy
59    /// filter, no GLONASS records, no leap-second offset, and no ionosphere
60    /// coefficients; use [`from_nav`](Self::from_nav) to capture those).
61    pub fn new(records: Vec<BroadcastRecord>) -> CoreResult<Self> {
62        for record in &records {
63            validate_manual_record(record)?;
64        }
65        Ok(Self {
66            records,
67            glonass: Vec::new(),
68            leap_seconds: None,
69            iono: IonoCorrections::default(),
70            message_preference: NavMessagePreference::default(),
71        })
72    }
73
74    /// Parse a RINEX 3.x/4.xx navigation file and keep the records usable for
75    /// single-frequency positioning under the default policy: healthy GPS LNAV,
76    /// GPS/QZSS CNAV-family records with URA prediction, Galileo I/NAV, BeiDou
77    /// D1/D2, and GLONASS. The header's broadcast ionosphere coefficients (see
78    /// [`iono_corrections`](Self::iono_corrections)) and leap-second offset are
79    /// captured.
80    pub fn from_nav(text: &str) -> Result<Self, NavParseError> {
81        let records = parse_nav(text)?
82            .into_iter()
83            .filter(Self::is_default_usable)
84            .collect();
85        let glonass = parse_glonass(text)?
86            .into_iter()
87            .filter(|r| r.sv_health == 0.0)
88            .collect();
89        Ok(Self {
90            records,
91            glonass,
92            leap_seconds: parse_leap_seconds_checked(text)?,
93            iono: parse_iono_corrections_checked(text)?,
94            message_preference: NavMessagePreference::default(),
95        })
96    }
97
98    /// Set the GPS/QZSS legacy-vs-CNAV selection preference.
99    pub fn set_message_preference(&mut self, preference: NavMessagePreference) {
100        self.message_preference = preference;
101    }
102
103    /// The GPS/QZSS legacy-vs-CNAV selection preference.
104    pub const fn message_preference(&self) -> NavMessagePreference {
105        self.message_preference
106    }
107
108    /// The broadcast ionosphere coefficients parsed from the navigation header
109    /// or RINEX 4 body `> ION` frames (GPS `GPSA`/`GPSB`, BeiDou `BDSA`/`BDSB`,
110    /// and Galileo `GAL`). Empty for a store built with [`new`](Self::new).
111    pub fn iono_corrections(&self) -> IonoCorrections {
112        self.iono
113    }
114
115    /// The held GLONASS records.
116    pub fn glonass_records(&self) -> &[GlonassRecord] {
117        &self.glonass
118    }
119
120    /// The GLONASS FDMA frequency channels carried by the held broadcast
121    /// records, keyed by satellite PRN/slot (`[-7, 6]`).
122    ///
123    /// Lets a consumer source the per-satellite channel numbers - needed to
124    /// scale the GLONASS ionospheric delay per carrier - from the broadcast
125    /// navigation message when an observation file carries no `GLONASS SLOT /
126    /// FRQ #` header records. Each GLONASS satellite broadcasts one channel, so
127    /// the map has at most one entry per slot. The result keys/values match the
128    /// `glonass_slots` layout of [`crate::rinex_obs::ObsHeader`], so a consumer
129    /// can use this map directly where an OBS file would otherwise supply one.
130    pub fn glonass_frequency_channels(&self) -> std::collections::BTreeMap<u8, i8> {
131        self.glonass
132            .iter()
133            .map(|r| (r.satellite_id.prn, r.freq_channel as i8))
134            .collect()
135    }
136
137    /// The default usability policy: healthy and single-frequency-appropriate
138    /// messages. CNAV-family records with no URA prediction are excluded.
139    fn is_default_usable(r: &BroadcastRecord) -> bool {
140        r.sv_health == 0.0
141            && matches!(
142                r.message,
143                NavMessage::GpsLnav
144                    | NavMessage::GpsCnav
145                    | NavMessage::GpsCnav2
146                    | NavMessage::QzssLnav
147                    | NavMessage::QzssCnav
148                    | NavMessage::QzssCnav2
149                    | NavMessage::GalileoInav
150                    | NavMessage::BeidouD1
151                    | NavMessage::BeidouD2
152            )
153            && (!r.message.is_cnav_family()
154                || r.cnav
155                    .map(|cnav| cnav_ura_nominal_m(cnav.ura_ed_index).is_some())
156                    .unwrap_or(false))
157    }
158
159    /// The held records.
160    pub fn records(&self) -> &[BroadcastRecord] {
161        &self.records
162    }
163
164    /// Select the broadcast record used for `sat` at `t_j2000_s`.
165    ///
166    /// The selection uses the same message preference, native-system time
167    /// mapping, and validity-window checks as
168    /// [`EphemerisSource::position_clock_at_j2000_s`]. Non-Keplerian systems
169    /// return `None`.
170    pub fn select_record_at(
171        &self,
172        sat: GnssSatelliteId,
173        t_j2000_s: f64,
174    ) -> Option<&BroadcastRecord> {
175        let (t_continuous_s, _) = query_continuous_time(sat, t_j2000_s)?;
176        self.select(sat, t_continuous_s)
177    }
178
179    /// Select the valid record for `sat` with a matching GPS issue byte at `t`.
180    pub fn select_by_iode_at(
181        &self,
182        sat: GnssSatelliteId,
183        iode: u8,
184        t_j2000_s: f64,
185    ) -> Option<&BroadcastRecord> {
186        let (t_continuous, _) = query_continuous_time(sat, t_j2000_s)?;
187        self.records
188            .iter()
189            .filter(|r| r.satellite_id == sat)
190            .filter(|r| r.issue_of_data.message == NavMessage::GpsLnav)
191            .filter(|r| r.issue_of_data.issue == u32::from(iode))
192            .filter(|r| (t_continuous - Self::toe_continuous_s(r)).abs() <= Self::half_window_s(r))
193            .min_by(|a, b| {
194                let da = (t_continuous - Self::toe_continuous_s(a)).abs();
195                let db = (t_continuous - Self::toe_continuous_s(b)).abs();
196                da.partial_cmp(&db).unwrap_or(core::cmp::Ordering::Equal)
197            })
198    }
199
200    /// Evaluate a matching issue-specific broadcast record at `t`.
201    pub fn state_by_iode_at(
202        &self,
203        sat: GnssSatelliteId,
204        iode: u8,
205        t_j2000_s: f64,
206    ) -> Option<([f64; 3], f64)> {
207        let (t_continuous, is_geo) = query_continuous_time(sat, t_j2000_s)?;
208        let rec = self.select_by_iode_at(sat, iode, t_j2000_s)?;
209        let sow = t_continuous.rem_euclid(SECONDS_PER_WEEK);
210        let state = evaluate_record_unchecked(rec, sow, is_geo);
211        let position = state.orbit.position().ok()?;
212        Some((position.as_array(), state.clock.dt_clock_total_s))
213    }
214
215    /// Keep only the records matching a predicate (e.g. a custom message/health
216    /// policy on a store built with [`new`](BroadcastStore::new)).
217    pub fn retain(&mut self, keep: impl FnMut(&BroadcastRecord) -> bool) {
218        self.records.retain(keep);
219    }
220
221    /// Continuous native broadcast time of a record's `toe`
222    /// (`week * 604800 + tow` in the record's own scale).
223    fn toe_continuous_s(rec: &BroadcastRecord) -> f64 {
224        f64::from(rec.toe.week) * SECONDS_PER_WEEK + rec.toe.tow_s
225    }
226
227    /// The half-validity window (seconds either side of `toe`) for a record: half
228    /// the broadcast GPS fit interval, or [`MAX_EPHEMERIS_AGE_S`] when no fit
229    /// interval is broadcast (Galileo, BeiDou).
230    fn half_window_s(rec: &BroadcastRecord) -> f64 {
231        match rec.fit_interval_s {
232            Some(fit) => fit / 2.0,
233            None => MAX_EPHEMERIS_AGE_S,
234        }
235    }
236
237    /// The record for `sat` whose native-system `toe` is nearest
238    /// `t_continuous_s` **among those whose validity window covers the
239    /// query** (see [`half_window_s`](Self::half_window_s)). Filtering by
240    /// validity before choosing the nearest means a query just past one record's
241    /// fit interval can still be served by a farther record whose own window is
242    /// wide enough, rather than being rejected outright.
243    fn select(&self, sat: GnssSatelliteId, t_continuous_s: f64) -> Option<&BroadcastRecord> {
244        let mut preferred = None;
245        let mut fallback = None;
246        for record in self.records.iter().filter(|r| r.satellite_id == sat) {
247            if (t_continuous_s - Self::toe_continuous_s(record)).abs() > Self::half_window_s(record)
248            {
249                continue;
250            }
251            if self.is_preferred_family(record) {
252                select_better_candidate(&mut preferred, record, t_continuous_s);
253            } else {
254                select_better_candidate(&mut fallback, record, t_continuous_s);
255            }
256        }
257        preferred.or(fallback)
258    }
259
260    fn is_preferred_family(&self, record: &BroadcastRecord) -> bool {
261        if !matches!(
262            record.satellite_id.system,
263            GnssSystem::Gps | GnssSystem::Qzss
264        ) {
265            return true;
266        }
267        match self.message_preference {
268            NavMessagePreference::PreferLegacy => !record.message.is_cnav_family(),
269            NavMessagePreference::PreferModern => record.message.is_cnav_family(),
270        }
271    }
272
273    /// Select the broadcast record matching a specific issue and message at the
274    /// query epoch.
275    pub fn select_by_issue_at(
276        &self,
277        sat: GnssSatelliteId,
278        issue: BroadcastIssue,
279        nav_message: NavMessage,
280        t_j2000_s: f64,
281    ) -> Option<&BroadcastRecord> {
282        if issue.message != nav_message {
283            return None;
284        }
285        let (t_continuous_s, _) = query_continuous_time(sat, t_j2000_s)?;
286        self.records
287            .iter()
288            .filter(|r| {
289                r.satellite_id == sat
290                    && r.message == nav_message
291                    && r.issue_of_data == issue
292                    && (t_continuous_s - Self::toe_continuous_s(r)).abs() <= Self::half_window_s(r)
293            })
294            .min_by(|a, b| {
295                let da = (t_continuous_s - Self::toe_continuous_s(a)).abs();
296                let db = (t_continuous_s - Self::toe_continuous_s(b)).abs();
297                da.partial_cmp(&db).unwrap_or(core::cmp::Ordering::Equal)
298            })
299    }
300
301    /// The GLONASS record for `sat` nearest the GPST-aligned query `t_j2000_s`
302    /// (within [`GLONASS_MAX_AGE_S`]), with `tk` = query − the record's reference
303    /// epoch in GPS time. Returns `None` if no leap-second offset was parsed (the
304    /// GLONASS UTC epoch then cannot be placed on the GPST timeline).
305    fn select_glonass(
306        &self,
307        sat: GnssSatelliteId,
308        t_j2000_s: f64,
309    ) -> Option<(&GlonassRecord, f64)> {
310        let leap = self.leap_seconds?;
311        let toe_gpst = |r: &GlonassRecord| r.toe_utc_j2000_s + leap;
312        let rec = self
313            .glonass
314            .iter()
315            .filter(|r| r.satellite_id == sat)
316            .min_by(|a, b| {
317                let da = (t_j2000_s - toe_gpst(a)).abs();
318                let db = (t_j2000_s - toe_gpst(b)).abs();
319                da.partial_cmp(&db).unwrap_or(core::cmp::Ordering::Equal)
320            })?;
321        let tk = t_j2000_s - toe_gpst(rec);
322        if tk.abs() <= GLONASS_MAX_AGE_S {
323            Some((rec, tk))
324        } else {
325            None
326        }
327    }
328}
329
330fn validate_manual_record(record: &BroadcastRecord) -> CoreResult<()> {
331    validate_finite(record.toe.tow_s, "record.toe.tow_s")?;
332    validate_finite(record.toc.tow_s, "record.toc.tow_s")?;
333    validate_finite(record.sv_health, "record.sv_health")?;
334    validate_finite(record.sv_accuracy_m, "record.sv_accuracy_m")?;
335    if let Some(fit) = record.fit_interval_s {
336        validate_finite(fit, "record.fit_interval_s")?;
337        if fit <= 0.0 {
338            return Err(invalid_input("record.fit_interval_s", "not positive"));
339        }
340    }
341    validate_group_delays(record.group_delays)?;
342    validate_cnav_presence(record)?;
343    if let Some(cnav) = record.cnav {
344        validate_cnav_parameters(cnav)?;
345    }
346
347    if let Some(cnav) = record.cnav {
348        satellite_state_cnav(
349            &record.elements,
350            &cnav_rates(cnav),
351            &record.clock,
352            &record.constants(),
353            record.elements.toe_sow,
354            record.broadcast_clock_group_delay_s(),
355        )
356        .map(|_| ())
357    } else {
358        satellite_state(
359            &record.elements,
360            &record.clock,
361            &record.constants(),
362            record.elements.toe_sow,
363            record.broadcast_clock_group_delay_s(),
364            is_beidou_geo(record.satellite_id),
365        )
366        .map(|_| ())
367    }
368}
369
370fn validate_group_delays(delays: BroadcastGroupDelays) -> CoreResult<()> {
371    for (field, value) in [
372        ("group_delays.gps_tgd_s", delays.gps_tgd_s),
373        (
374            "group_delays.galileo_bgd_e5a_e1_s",
375            delays.galileo_bgd_e5a_e1_s,
376        ),
377        (
378            "group_delays.galileo_bgd_e5b_e1_s",
379            delays.galileo_bgd_e5b_e1_s,
380        ),
381        ("group_delays.beidou_tgd1_s", delays.beidou_tgd1_s),
382        ("group_delays.beidou_tgd2_s", delays.beidou_tgd2_s),
383        ("group_delays.cnav_isc_l1ca_s", delays.cnav_isc_l1ca_s),
384        ("group_delays.cnav_isc_l2c_s", delays.cnav_isc_l2c_s),
385        ("group_delays.cnav_isc_l5i5_s", delays.cnav_isc_l5i5_s),
386        ("group_delays.cnav_isc_l5q5_s", delays.cnav_isc_l5q5_s),
387        ("group_delays.cnav_isc_l1cd_s", delays.cnav_isc_l1cd_s),
388        ("group_delays.cnav_isc_l1cp_s", delays.cnav_isc_l1cp_s),
389    ] {
390        if let Some(value) = value {
391            validate_finite(value, field)?;
392        }
393    }
394    Ok(())
395}
396
397fn validate_cnav_presence(record: &BroadcastRecord) -> CoreResult<()> {
398    if record.message.is_cnav_family() != record.cnav.is_some() {
399        return Err(invalid_input(
400            "record.cnav",
401            "must be present only for CNAV-family messages",
402        ));
403    }
404    Ok(())
405}
406
407fn validate_cnav_parameters(params: CnavParameters) -> CoreResult<()> {
408    validate_finite(params.adot_m_s, "record.cnav.adot_m_s")?;
409    validate_finite(
410        params.delta_n0_dot_rad_s2,
411        "record.cnav.delta_n0_dot_rad_s2",
412    )?;
413    validate_finite(params.top.tow_s, "record.cnav.top.tow_s")?;
414    validate_finite(
415        params.transmission_time_sow,
416        "record.cnav.transmission_time_sow",
417    )?;
418    if !(-16..=15).contains(&params.ura_ed_index) {
419        return Err(invalid_input("record.cnav.ura_ed_index", "out of range"));
420    }
421    if !(-16..=15).contains(&params.ura_ned0_index) {
422        return Err(invalid_input("record.cnav.ura_ned0_index", "out of range"));
423    }
424    if params.ura_ned1_index > 7 {
425        return Err(invalid_input("record.cnav.ura_ned1_index", "out of range"));
426    }
427    if params.ura_ned2_index > 7 {
428        return Err(invalid_input("record.cnav.ura_ned2_index", "out of range"));
429    }
430    Ok(())
431}
432
433fn cnav_rates(params: CnavParameters) -> CnavRates {
434    CnavRates {
435        adot_m_s: params.adot_m_s,
436        delta_n0_dot_rad_s2: params.delta_n0_dot_rad_s2,
437    }
438}
439
440fn evaluate_record_unchecked(rec: &BroadcastRecord, sow: f64, is_geo: bool) -> SatelliteState {
441    if let Some(cnav) = rec.cnav {
442        satellite_state_cnav_unchecked(
443            &rec.elements,
444            &cnav_rates(cnav),
445            &rec.clock,
446            &rec.constants(),
447            sow,
448            rec.broadcast_clock_group_delay_s(),
449        )
450    } else {
451        satellite_state_unchecked(
452            &rec.elements,
453            &rec.clock,
454            &rec.constants(),
455            sow,
456            rec.broadcast_clock_group_delay_s(),
457            is_geo,
458        )
459    }
460}
461
462fn select_better_candidate<'a>(
463    best: &mut Option<&'a BroadcastRecord>,
464    candidate: &'a BroadcastRecord,
465    t_continuous_s: f64,
466) {
467    let Some(current) = *best else {
468        *best = Some(candidate);
469        return;
470    };
471    if candidate_is_better(candidate, current, t_continuous_s) {
472        *best = Some(candidate);
473    }
474}
475
476fn candidate_is_better(
477    candidate: &BroadcastRecord,
478    current: &BroadcastRecord,
479    t_continuous_s: f64,
480) -> bool {
481    let da = (t_continuous_s - BroadcastStore::toe_continuous_s(candidate)).abs();
482    let db = (t_continuous_s - BroadcastStore::toe_continuous_s(current)).abs();
483    match da.partial_cmp(&db).unwrap_or(core::cmp::Ordering::Equal) {
484        core::cmp::Ordering::Less => true,
485        core::cmp::Ordering::Greater => false,
486        core::cmp::Ordering::Equal => {
487            cnav_tie_rank(candidate.message) < cnav_tie_rank(current.message)
488        }
489    }
490}
491
492const fn cnav_tie_rank(message: NavMessage) -> u8 {
493    match message {
494        NavMessage::GpsCnav | NavMessage::QzssCnav => 0,
495        NavMessage::GpsCnav2 | NavMessage::QzssCnav2 => 1,
496        NavMessage::QzssLnav => 0,
497        _ => 0,
498    }
499}
500
501fn validate_finite(value: f64, field: &'static str) -> CoreResult<()> {
502    if value.is_finite() {
503        Ok(())
504    } else {
505        Err(invalid_input(field, "not finite"))
506    }
507}
508
509fn invalid_input(field: &'static str, reason: &'static str) -> Error {
510    Error::InvalidInput(format!("{field} {reason}"))
511}
512
513impl core::str::FromStr for BroadcastStore {
514    type Err = NavParseError;
515
516    fn from_str(s: &str) -> Result<Self, Self::Err> {
517        Self::from_nav(s)
518    }
519}
520
521impl EphemerisSource for BroadcastStore {
522    fn position_clock_at_j2000_s(
523        &self,
524        sat: GnssSatelliteId,
525        t_j2000_s: f64,
526    ) -> Option<([f64; 3], f64)> {
527        // GLONASS is not Keplerian: integrate its broadcast state vector with the
528        // RK4 propagator. Its reference epoch is UTC, mapped onto the GPST-aligned
529        // query via the parsed leap-second offset.
530        if sat.system == GnssSystem::Glonass {
531            let (rec, tk) = self.select_glonass(sat, t_j2000_s)?;
532            let state0 = [
533                rec.pos_m[0],
534                rec.pos_m[1],
535                rec.pos_m[2],
536                rec.vel_m_s[0],
537                rec.vel_m_s[1],
538                rec.vel_m_s[2],
539            ];
540            let state = glonass::propagate(state0, rec.acc_m_s2, tk).ok()?;
541            let clock = glonass::clock_offset_s(rec.clk_bias, rec.gamma_n, tk);
542            return Some(([state[0], state[1], state[2]], clock));
543        }
544
545        // Supported Keplerian systems only; a record from any other system (for
546        // example SBAS or NavIC) reports no ephemeris rather than being evaluated
547        // with the wrong model. (`from_nav` already restricts records, but `new`
548        // accepts arbitrary ones.)
549        // Map the receive instant (J2000, GPST-aligned) onto the satellite
550        // system's continuous time and seconds of week. BeiDou runs on BDT
551        // (= GPST - 14 s) with its week epoch 1356 weeks after the GPS epoch, and
552        // its geostationary satellites take the GEO orbit branch.
553        // `query_continuous_time` returns None for non-Keplerian systems.
554        let (t_continuous, is_geo) = query_continuous_time(sat, t_j2000_s)?;
555
556        let rec = self.select(sat, t_continuous)?;
557        let sow = t_continuous.rem_euclid(SECONDS_PER_WEEK);
558        let state = evaluate_record_unchecked(rec, sow, is_geo);
559        let position = state.orbit.position().ok()?;
560        Some((position.as_array(), state.clock.dt_clock_total_s))
561    }
562}
563
564fn query_continuous_time(sat: GnssSatelliteId, t_j2000_s: f64) -> Option<(f64, bool)> {
565    if !matches!(
566        sat.system,
567        GnssSystem::Gps | GnssSystem::Galileo | GnssSystem::BeiDou | GnssSystem::Qzss
568    ) {
569        return None;
570    }
571    let gpst_continuous = t_j2000_s + GPS_EPOCH_TO_J2000_S;
572    if sat.system == GnssSystem::BeiDou {
573        Some((
574            gpst_continuous - GPST_MINUS_BDT_S - BDS_EPOCH_MINUS_GPS_EPOCH_S,
575            is_beidou_geo(sat),
576        ))
577    } else {
578        Some((gpst_continuous, false))
579    }
580}
sidereon_core/rinex_nav/store.rs

sidereon_core/rinex_nav/
store.rs
