sidereon_core/broadcast/
mod.rs

1//! Broadcast-ephemeris orbit and clock evaluation (GPS LNAV/CNAV/CNAV-2,
2//! QZSS CNAV/CNAV-2, Galileo I/NAV, BeiDou D1/D2).
3//!
4//! Evaluates a broadcast navigation message into an ECEF satellite position and
5//! a satellite clock offset, by the standard Keplerian construction of
6//! IS-GPS-200 (Section 20.3.3.4.3.1, Table 20-IV) and the equivalent Galileo OS
7//! and BeiDou SIS ICD sections. The constellations share the algorithm and
8//! differ in the gravitational constant, the Earth-rotation rate, and the
9//! relativistic clock constant ([`ConstellationConstants`]). BeiDou
10//! geostationary satellites additionally take a custom-frame-to-ECEF rotation
11//! (the `is_geo` path of [`satellite_position_ecef`]); GPS, Galileo, and BeiDou
12//! MEO/IGSO satellites use the direct rotation.
13//!
14//! This is a 0-ULP parity target for the legacy evaluator: the operation order
15//! reproduces the canonical reference recipe (`parity/generator/broadcast_eval.py`)
16//! bit-for-bit. The CNAV evaluator is pinned separately by
17//! `fixtures-generators/broadcast_eval_cnav.py`, using the same scalar libm,
18//! no-FMA, explicit-multiply discipline. Separate `.sin()` and `.cos()` calls
19//! are used deliberately (never `.sin_cos()`, whose fused evaluation can differ
20//! in the last bit). Integer powers are explicit repeated multiplies and there
21//! is no fused multiply-add (Rust does not auto-contract `a * b + c`).
22
23use crate::astro::constants::models::broadcast::{
24    BEIDOU_OMEGA_E_RAD_S, GALILEO_BEIDOU_DTR_F, GALILEO_GM_M3_S2, GPS_DTR_F,
25    GPS_GALILEO_OMEGA_E_RAD_S, GPS_GM_M3_S2,
26};
27use crate::error::{Error, Result};
28use crate::frame::{FrameValueError, ItrfPositionM};
29
30/// Half a week, the fold threshold for a time difference against `toe`/`toc`.
31pub use crate::constants::HALF_WEEK_S;
32/// Seconds in one GPS/Galileo week.
33pub use crate::constants::SECONDS_PER_WEEK;
34
35/// Eccentric-anomaly fixed-point convergence threshold (radians).
36pub const KEPLER_TOL: f64 = 1.0e-12;
37/// Maximum eccentric-anomaly fixed-point iterations.
38pub const KEPLER_MAX_ITER: usize = 30;
39/// Satellite-clock time-argument refinement count (RTKLIB `eph2clk` convention).
40pub const CLOCK_MAX_ITER: usize = 2;
41
42/// Per-constellation physical constants used by the broadcast evaluation.
43///
44/// The literals match the values the broadcast reference recipe and the `rinex`
45/// crate use, so the Python and Rust sides share identical `f64` bit patterns.
46#[derive(Debug, Clone, Copy, PartialEq)]
47pub struct ConstellationConstants {
48    /// Gravitational constant GM (m^3 / s^2).
49    pub gm_m3_s2: f64,
50    /// Earth rotation rate used by the longitude-of-node (Sagnac) term (rad/s).
51    pub omega_e_rad_s: f64,
52    /// Relativistic clock constant `F = -2 * sqrt(GM) / c^2` (s / sqrt(m)).
53    pub dtr_f: f64,
54}
55
56impl ConstellationConstants {
57    /// GPS constants (IS-GPS-200).
58    pub const GPS: Self = Self {
59        gm_m3_s2: GPS_GM_M3_S2,
60        omega_e_rad_s: GPS_GALILEO_OMEGA_E_RAD_S,
61        dtr_f: GPS_DTR_F,
62    };
63    /// Galileo constants (OS SIS ICD); shares the GPS rotation rate.
64    pub const GALILEO: Self = Self {
65        gm_m3_s2: GALILEO_GM_M3_S2,
66        omega_e_rad_s: GPS_GALILEO_OMEGA_E_RAD_S,
67        dtr_f: GALILEO_BEIDOU_DTR_F,
68    };
69    /// BeiDou constants (BDS-SIS-ICD); its own Earth-rotation rate.
70    pub const BEIDOU: Self = Self {
71        gm_m3_s2: GALILEO_GM_M3_S2,
72        omega_e_rad_s: BEIDOU_OMEGA_E_RAD_S,
73        dtr_f: GALILEO_BEIDOU_DTR_F,
74    };
75}
76
77/// Broadcast Keplerian orbital elements (SI units; angles in radians; `toe_sow`
78/// in seconds of the constellation's week).
79#[derive(Debug, Clone, Copy, PartialEq)]
80pub struct KeplerianElements {
81    /// Square root of the semi-major axis (sqrt(m)).
82    pub sqrt_a: f64,
83    /// Eccentricity (dimensionless).
84    pub e: f64,
85    /// Mean anomaly at reference time (rad).
86    pub m0: f64,
87    /// Mean motion difference from computed value (rad/s).
88    pub delta_n: f64,
89    /// Longitude of ascending node at weekly epoch (rad).
90    pub omega0: f64,
91    /// Inclination at reference time (rad).
92    pub i0: f64,
93    /// Argument of perigee (rad).
94    pub omega: f64,
95    /// Rate of right ascension (rad/s).
96    pub omega_dot: f64,
97    /// Rate of inclination (rad/s).
98    pub idot: f64,
99    /// Latitude argument cosine correction (rad).
100    pub cuc: f64,
101    /// Latitude argument sine correction (rad).
102    pub cus: f64,
103    /// Orbit radius cosine correction (m).
104    pub crc: f64,
105    /// Orbit radius sine correction (m).
106    pub crs: f64,
107    /// Inclination cosine correction (rad).
108    pub cic: f64,
109    /// Inclination sine correction (rad).
110    pub cis: f64,
111    /// Ephemeris reference time, seconds of week.
112    pub toe_sow: f64,
113}
114
115/// Broadcast satellite-clock polynomial about `toc_sow`.
116#[derive(Debug, Clone, Copy, PartialEq)]
117pub struct ClockPolynomial {
118    /// Clock bias (s).
119    pub af0: f64,
120    /// Clock drift (s/s).
121    pub af1: f64,
122    /// Clock drift rate (s/s^2).
123    pub af2: f64,
124    /// Clock reference time, seconds of week.
125    pub toc_sow: f64,
126}
127
128/// CNAV ephemeris rate terms absent from the legacy parameterization.
129#[derive(Debug, Clone, Copy, PartialEq)]
130pub struct CnavRates {
131    /// Semi-major axis rate ADOT (m/s).
132    pub adot_m_s: f64,
133    /// Rate of the mean-motion difference (rad/s^2).
134    pub delta_n0_dot_rad_s2: f64,
135}
136
137/// A solved eccentric anomaly and the iteration count that produced it.
138#[derive(Debug, Clone, Copy, PartialEq)]
139pub struct EccentricAnomaly {
140    /// Eccentric anomaly (rad).
141    pub value: f64,
142    /// Fixed-point iterations performed.
143    pub iterations: usize,
144}
145
146/// The full intermediate substrate of a broadcast orbit evaluation.
147///
148/// Every field is exposed so a 0-ULP parity test can localize a mismatch to a
149/// single operation. [`OrbitState::position`] returns the ECEF position.
150#[derive(Debug, Clone, Copy, PartialEq)]
151pub struct OrbitState {
152    /// Semi-major axis (m).
153    pub a: f64,
154    /// Computed mean motion (rad/s).
155    pub n0: f64,
156    /// Corrected mean motion (rad/s).
157    pub n: f64,
158    /// Time from ephemeris reference epoch, half-week folded (s).
159    pub tk: f64,
160    /// Mean anomaly (rad).
161    pub mk: f64,
162    /// Eccentric anomaly (rad).
163    pub eccentric_anomaly: f64,
164    /// Number of Kepler iterations.
165    pub kepler_iterations: usize,
166    /// sin(E).
167    pub sin_e: f64,
168    /// cos(E).
169    pub cos_e: f64,
170    /// True anomaly (rad).
171    pub nu: f64,
172    /// Argument of latitude before correction (rad).
173    pub phi: f64,
174    /// sin(2*phi).
175    pub s2: f64,
176    /// cos(2*phi).
177    pub c2: f64,
178    /// Argument-of-latitude correction (rad).
179    pub du: f64,
180    /// Radius correction (m).
181    pub dr: f64,
182    /// Inclination correction (rad).
183    pub di: f64,
184    /// Corrected argument of latitude (rad).
185    pub u: f64,
186    /// Corrected radius (m).
187    pub r: f64,
188    /// Corrected inclination (rad).
189    pub i: f64,
190    /// Orbital-plane x (m).
191    pub xp: f64,
192    /// Orbital-plane y (m).
193    pub yp: f64,
194    /// Corrected longitude of ascending node (rad).
195    pub omega_k: f64,
196    /// ECEF x (m).
197    pub x_m: f64,
198    /// ECEF y (m).
199    pub y_m: f64,
200    /// ECEF z (m).
201    pub z_m: f64,
202}
203
204impl OrbitState {
205    /// The Earth-fixed (ITRF/ECEF) satellite position in meters.
206    pub const fn position(&self) -> core::result::Result<ItrfPositionM, FrameValueError> {
207        ItrfPositionM::new(self.x_m, self.y_m, self.z_m)
208    }
209}
210
211/// The satellite clock offset, split into its components.
212#[derive(Debug, Clone, Copy, PartialEq)]
213pub struct ClockOffset {
214    /// Polynomial term (s).
215    pub dt_clock_poly_s: f64,
216    /// Relativistic eccentricity term (s).
217    pub dt_rel_s: f64,
218    /// Group delay subtracted for the single-frequency user (s).
219    pub tgd_s: f64,
220    /// Total satellite clock offset (s).
221    pub dt_clock_total_s: f64,
222}
223
224/// Broadcast relativistic satellite-clock correction, seconds.
225///
226/// Evaluates the periodic eccentric-orbit term
227/// `F * e * sqrt(A) * sin(E)` from IS-GPS-200 and the equivalent Galileo/BeiDou
228/// broadcast-clock models. Use [`ConstellationConstants::dtr_f`] for `F` when
229/// evaluating a full broadcast record.
230pub fn relativistic_clock_correction_s(
231    dtr_f_s_sqrt_m: f64,
232    eccentricity: f64,
233    sqrt_a_m_sqrt: f64,
234    eccentric_anomaly_sin: f64,
235) -> Result<f64> {
236    validate_finite(dtr_f_s_sqrt_m, "dtr_f_s_sqrt_m")?;
237    validate_eccentricity(eccentricity)?;
238    validate_positive(sqrt_a_m_sqrt, "sqrt_a_m_sqrt")?;
239    validate_finite(eccentric_anomaly_sin, "eccentric_anomaly_sin")?;
240    let correction = relativistic_clock_correction_s_unchecked(
241        dtr_f_s_sqrt_m,
242        eccentricity,
243        sqrt_a_m_sqrt,
244        eccentric_anomaly_sin,
245    );
246    validate_finite(correction, "relativistic_clock_correction_s")?;
247    Ok(correction)
248}
249
250#[inline]
251pub(crate) fn relativistic_clock_correction_s_unchecked(
252    dtr_f_s_sqrt_m: f64,
253    eccentricity: f64,
254    sqrt_a_m_sqrt: f64,
255    eccentric_anomaly_sin: f64,
256) -> f64 {
257    dtr_f_s_sqrt_m * eccentricity * sqrt_a_m_sqrt * eccentric_anomaly_sin
258}
259
260/// Time difference `t - t_ref` (seconds), folded into the +/- half-week range.
261///
262/// Used for both `tk = t - toe` (orbit) and `t - toc` (clock); the fold handles
263/// a query that straddles the week rollover relative to the reference.
264pub fn time_from_reference_s(t_sow_s: f64, t_ref_sow_s: f64) -> f64 {
265    let mut dt = t_sow_s - t_ref_sow_s;
266    if dt > HALF_WEEK_S {
267        dt -= SECONDS_PER_WEEK;
268    }
269    if dt < -HALF_WEEK_S {
270        dt += SECONDS_PER_WEEK;
271    }
272    dt
273}
274
275/// Solve Kepler's equation by fixed-point iteration `E = M + e*sin(E)`, seeded
276/// at `E = M`; stops on `|dE| <= KEPLER_TOL` or after `KEPLER_MAX_ITER` steps.
277pub fn eccentric_anomaly(mean_anomaly_rad: f64, eccentricity: f64) -> Result<EccentricAnomaly> {
278    validate_finite(mean_anomaly_rad, "mean_anomaly_rad")?;
279    validate_eccentricity(eccentricity)?;
280
281    Ok(eccentric_anomaly_unchecked(mean_anomaly_rad, eccentricity))
282}
283
284pub(crate) fn eccentric_anomaly_unchecked(
285    mean_anomaly_rad: f64,
286    eccentricity: f64,
287) -> EccentricAnomaly {
288    let mut e_k = mean_anomaly_rad;
289    let mut iterations = 0usize;
290    while iterations < KEPLER_MAX_ITER {
291        let e_prev = e_k;
292        e_k = mean_anomaly_rad + eccentricity * e_prev.sin();
293        iterations += 1;
294        let delta = (e_k - e_prev).abs();
295        if delta <= KEPLER_TOL {
296            break;
297        }
298    }
299    EccentricAnomaly {
300        value: e_k,
301        iterations,
302    }
303}
304
305/// Evaluate the broadcast Keplerian orbit at `t_sow_s` (seconds of week).
306///
307/// `is_geo` selects the BeiDou geostationary path (the node omits the
308/// Earth-rotation-during-`tk` term and the position is rotated to ECEF by
309/// `Rz(omega_e*tk) . Rx(-5deg)`); GPS, Galileo, and BeiDou MEO/IGSO use `false`.
310/// The statement order reproduces `broadcast_eval.satellite_position_ecef`.
311pub fn satellite_position_ecef(
312    elements: &KeplerianElements,
313    consts: &ConstellationConstants,
314    t_sow_s: f64,
315    is_geo: bool,
316) -> Result<OrbitState> {
317    validate_elements(elements)?;
318    validate_constants(consts)?;
319    validate_finite(t_sow_s, "t_sow_s")?;
320
321    let state = satellite_position_ecef_unchecked(elements, consts, t_sow_s, is_geo);
322    validate_orbit_state(&state)?;
323    Ok(state)
324}
325
326pub(crate) fn satellite_position_ecef_unchecked(
327    elements: &KeplerianElements,
328    consts: &ConstellationConstants,
329    t_sow_s: f64,
330    is_geo: bool,
331) -> OrbitState {
332    satellite_position_ecef_impl(elements, None, consts, t_sow_s, is_geo)
333}
334
335fn satellite_position_ecef_impl(
336    elements: &KeplerianElements,
337    cnav_rates: Option<&CnavRates>,
338    consts: &ConstellationConstants,
339    t_sow_s: f64,
340    is_geo: bool,
341) -> OrbitState {
342    let sqrt_a = elements.sqrt_a;
343    let e = elements.e;
344    let gm = consts.gm_m3_s2;
345    let omega_e = consts.omega_e_rad_s;
346
347    let (a, n0, n, tk) = if let Some(rates) = cnav_rates {
348        let a0 = sqrt_a * sqrt_a;
349        let n0 = (gm / (a0 * a0 * a0)).sqrt();
350        let tk = time_from_reference_s(t_sow_s, elements.toe_sow);
351        let a = a0 + rates.adot_m_s * tk;
352        let delta_n_a = elements.delta_n + 0.5 * rates.delta_n0_dot_rad_s2 * tk;
353        let n = n0 + delta_n_a;
354        (a, n0, n, tk)
355    } else {
356        // 1. Semi-major axis and mean motion. a^3 as an explicit multiply chain.
357        let a = sqrt_a * sqrt_a;
358        let n0 = (gm / (a * a * a)).sqrt();
359        let n = n0 + elements.delta_n;
360
361        // 2. Time from ephemeris reference epoch (half-week folded).
362        let tk = time_from_reference_s(t_sow_s, elements.toe_sow);
363        (a, n0, n, tk)
364    };
365
366    // 3. Mean anomaly and eccentric anomaly.
367    let mk = elements.m0 + n * tk;
368    let kepler = eccentric_anomaly_unchecked(mk, e);
369    let ecc_anom = kepler.value;
370    let sin_e = ecc_anom.sin();
371    let cos_e = ecc_anom.cos();
372
373    // 4. True anomaly (atan2 form) and argument of latitude.
374    let e2 = e * e;
375    let nu = ((1.0 - e2).sqrt() * sin_e).atan2(cos_e - e);
376    let phi = nu + elements.omega;
377
378    // 5. Second-harmonic corrections (sine term first).
379    let two_phi = 2.0 * phi;
380    let s2 = two_phi.sin();
381    let c2 = two_phi.cos();
382    let du = elements.cus * s2 + elements.cuc * c2;
383    let dr = elements.crs * s2 + elements.crc * c2;
384    let di = elements.cis * s2 + elements.cic * c2;
385
386    // 6. Corrected argument of latitude, radius, inclination.
387    let u = phi + du;
388    let r = a * (1.0 - e * cos_e) + dr;
389    let i = elements.i0 + di + elements.idot * tk;
390
391    // 7. Position in the orbital plane.
392    let xp = r * u.cos();
393    let yp = r * u.sin();
394
395    // 8. Corrected longitude of ascending node. The BeiDou GEO node omits the
396    // Earth-rotation-during-tk term (applied by the final rotation instead).
397    let omega_k = if is_geo {
398        elements.omega0 + elements.omega_dot * tk - omega_e * elements.toe_sow
399    } else {
400        elements.omega0 + (elements.omega_dot - omega_e) * tk - omega_e * elements.toe_sow
401    };
402
403    // 9. Coordinates in the (custom) frame from the node rotation.
404    let sin_o = omega_k.sin();
405    let cos_o = omega_k.cos();
406    let sin_i = i.sin();
407    let cos_i = i.cos();
408    let xg = xp * cos_o - yp * cos_i * sin_o;
409    let yg = xp * sin_o + yp * cos_i * cos_o;
410    let zg = yp * sin_i;
411
412    // 10. Earth-fixed coordinates. The standard path is the identity; the BeiDou
413    // GEO path applies Rz(omega_e*tk) . Rx(-5deg) (BDS-SIS-ICD).
414    let (x, y, z) = if is_geo {
415        let deg5 = 5.0_f64.to_radians();
416        let cos_phi = deg5.cos();
417        let sin_phi = -deg5.sin();
418        let z_ang = omega_e * tk;
419        let cos_z = z_ang.cos();
420        let sin_z = z_ang.sin();
421        let yr = yg * cos_phi + zg * sin_phi;
422        let zr = -yg * sin_phi + zg * cos_phi;
423        (xg * cos_z + yr * sin_z, -xg * sin_z + yr * cos_z, zr)
424    } else {
425        (xg, yg, zg)
426    };
427
428    OrbitState {
429        a,
430        n0,
431        n,
432        tk,
433        mk,
434        eccentric_anomaly: ecc_anom,
435        kepler_iterations: kepler.iterations,
436        sin_e,
437        cos_e,
438        nu,
439        phi,
440        s2,
441        c2,
442        du,
443        dr,
444        di,
445        u,
446        r,
447        i,
448        xp,
449        yp,
450        omega_k,
451        x_m: x,
452        y_m: y,
453        z_m: z,
454    }
455}
456
457/// Evaluate a GPS/QZSS CNAV-family broadcast orbit at `t_sow_s`.
458///
459/// CNAV uses a time-varying semi-major axis and mean-motion correction:
460/// `Ak = A0 + ADOT * tk` and
461/// `delta_nA = delta_n0 + 0.5 * delta_n0_dot * tk`. The downstream Keplerian
462/// construction is the same as the legacy path, with the standard non-GEO
463/// rotation for both GPS and QZSS.
464pub fn satellite_position_ecef_cnav(
465    elements: &KeplerianElements,
466    rates: &CnavRates,
467    consts: &ConstellationConstants,
468    t_sow_s: f64,
469) -> Result<OrbitState> {
470    validate_elements(elements)?;
471    validate_cnav_rates(rates)?;
472    validate_constants(consts)?;
473    validate_finite(t_sow_s, "t_sow_s")?;
474
475    let state = satellite_position_ecef_cnav_unchecked(elements, rates, consts, t_sow_s);
476    validate_orbit_state(&state)?;
477    Ok(state)
478}
479
480pub(crate) fn satellite_position_ecef_cnav_unchecked(
481    elements: &KeplerianElements,
482    rates: &CnavRates,
483    consts: &ConstellationConstants,
484    t_sow_s: f64,
485) -> OrbitState {
486    satellite_position_ecef_impl(elements, Some(rates), consts, t_sow_s, false)
487}
488
489/// Evaluate the broadcast satellite clock offset (seconds).
490///
491/// `sin_e` is the eccentric-anomaly sine from the position evaluation at the
492/// same instant; `tgd_s` is the single-frequency group delay. The statement
493/// order reproduces `broadcast_eval.satellite_clock_offset_s`.
494pub fn satellite_clock_offset_s(
495    clock: &ClockPolynomial,
496    consts: &ConstellationConstants,
497    elements: &KeplerianElements,
498    sin_e: f64,
499    t_sow_s: f64,
500    tgd_s: f64,
501) -> Result<ClockOffset> {
502    validate_clock(clock)?;
503    validate_constants(consts)?;
504    validate_elements(elements)?;
505    validate_finite(sin_e, "sin_e")?;
506    validate_finite(t_sow_s, "t_sow_s")?;
507    validate_finite(tgd_s, "tgd_s")?;
508
509    let offset = satellite_clock_offset_s_unchecked(clock, consts, elements, sin_e, t_sow_s, tgd_s);
510    validate_clock_offset(&offset)?;
511    Ok(offset)
512}
513
514pub(crate) fn satellite_clock_offset_s_unchecked(
515    clock: &ClockPolynomial,
516    consts: &ConstellationConstants,
517    elements: &KeplerianElements,
518    sin_e: f64,
519    t_sow_s: f64,
520    tgd_s: f64,
521) -> ClockOffset {
522    let af0 = clock.af0;
523    let af1 = clock.af1;
524    let af2 = clock.af2;
525
526    // Time from clock reference, folded; then refine out the SV clock itself.
527    let dt0 = time_from_reference_s(t_sow_s, clock.toc_sow);
528    let mut dt = dt0;
529    let mut refine = 0usize;
530    while refine < CLOCK_MAX_ITER {
531        dt = dt0 - (af0 + af1 * dt + af2 * dt * dt);
532        refine += 1;
533    }
534    let dt_poly = af0 + af1 * dt + af2 * dt * dt;
535
536    // Relativistic eccentricity term (sqrt_a is the broadcast sqrt(A)).
537    let dt_rel =
538        relativistic_clock_correction_s_unchecked(consts.dtr_f, elements.e, elements.sqrt_a, sin_e);
539
540    let dt_total = dt_poly + dt_rel - tgd_s;
541
542    ClockOffset {
543        dt_clock_poly_s: dt_poly,
544        dt_rel_s: dt_rel,
545        tgd_s,
546        dt_clock_total_s: dt_total,
547    }
548}
549
550/// A satellite's broadcast orbit and clock evaluated together at one instant.
551#[derive(Debug, Clone, Copy, PartialEq)]
552pub struct SatelliteState {
553    /// The orbit evaluation (ECEF position and all intermediates).
554    pub orbit: OrbitState,
555    /// The clock offset evaluation.
556    pub clock: ClockOffset,
557}
558
559/// Evaluate the broadcast orbit and clock at the same instant.
560///
561/// This is the intended public entry point: it solves the orbit and feeds that
562/// solution's eccentric-anomaly sine into the clock's relativistic term, so a
563/// caller cannot accidentally pair a clock evaluation with `sin(E)` from a
564/// different epoch. [`satellite_position_ecef`] and [`satellite_clock_offset_s`]
565/// remain available for component-level parity testing.
566pub fn satellite_state(
567    elements: &KeplerianElements,
568    clock: &ClockPolynomial,
569    consts: &ConstellationConstants,
570    t_sow_s: f64,
571    tgd_s: f64,
572    is_geo: bool,
573) -> Result<SatelliteState> {
574    validate_elements(elements)?;
575    validate_clock(clock)?;
576    validate_constants(consts)?;
577    validate_finite(t_sow_s, "t_sow_s")?;
578    validate_finite(tgd_s, "tgd_s")?;
579
580    let state = satellite_state_unchecked(elements, clock, consts, t_sow_s, tgd_s, is_geo);
581    validate_orbit_state(&state.orbit)?;
582    validate_clock_offset(&state.clock)?;
583    Ok(state)
584}
585
586pub(crate) fn satellite_state_unchecked(
587    elements: &KeplerianElements,
588    clock: &ClockPolynomial,
589    consts: &ConstellationConstants,
590    t_sow_s: f64,
591    tgd_s: f64,
592    is_geo: bool,
593) -> SatelliteState {
594    let orbit = satellite_position_ecef_unchecked(elements, consts, t_sow_s, is_geo);
595    let clock =
596        satellite_clock_offset_s_unchecked(clock, consts, elements, orbit.sin_e, t_sow_s, tgd_s);
597    SatelliteState { orbit, clock }
598}
599
600/// Evaluate a GPS/QZSS CNAV-family broadcast orbit and clock at one instant.
601pub fn satellite_state_cnav(
602    elements: &KeplerianElements,
603    rates: &CnavRates,
604    clock: &ClockPolynomial,
605    consts: &ConstellationConstants,
606    t_sow_s: f64,
607    tgd_s: f64,
608) -> Result<SatelliteState> {
609    validate_elements(elements)?;
610    validate_cnav_rates(rates)?;
611    validate_clock(clock)?;
612    validate_constants(consts)?;
613    validate_finite(t_sow_s, "t_sow_s")?;
614    validate_finite(tgd_s, "tgd_s")?;
615
616    let state = satellite_state_cnav_unchecked(elements, rates, clock, consts, t_sow_s, tgd_s);
617    validate_orbit_state(&state.orbit)?;
618    validate_clock_offset(&state.clock)?;
619    Ok(state)
620}
621
622pub(crate) fn satellite_state_cnav_unchecked(
623    elements: &KeplerianElements,
624    rates: &CnavRates,
625    clock: &ClockPolynomial,
626    consts: &ConstellationConstants,
627    t_sow_s: f64,
628    tgd_s: f64,
629) -> SatelliteState {
630    let orbit = satellite_position_ecef_cnav_unchecked(elements, rates, consts, t_sow_s);
631    let clock =
632        satellite_clock_offset_s_unchecked(clock, consts, elements, orbit.sin_e, t_sow_s, tgd_s);
633    SatelliteState { orbit, clock }
634}
635
636fn validate_elements(elements: &KeplerianElements) -> Result<()> {
637    validate_positive(elements.sqrt_a, "elements.sqrt_a")?;
638    validate_eccentricity(elements.e)?;
639    validate_finite(elements.m0, "elements.m0")?;
640    validate_finite(elements.delta_n, "elements.delta_n")?;
641    validate_finite(elements.omega0, "elements.omega0")?;
642    validate_finite(elements.i0, "elements.i0")?;
643    validate_finite(elements.omega, "elements.omega")?;
644    validate_finite(elements.omega_dot, "elements.omega_dot")?;
645    validate_finite(elements.idot, "elements.idot")?;
646    validate_finite(elements.cuc, "elements.cuc")?;
647    validate_finite(elements.cus, "elements.cus")?;
648    validate_finite(elements.crc, "elements.crc")?;
649    validate_finite(elements.crs, "elements.crs")?;
650    validate_finite(elements.cic, "elements.cic")?;
651    validate_finite(elements.cis, "elements.cis")?;
652    validate_sow(elements.toe_sow, "elements.toe_sow")
653}
654
655fn validate_cnav_rates(rates: &CnavRates) -> Result<()> {
656    validate_finite(rates.adot_m_s, "rates.adot_m_s")?;
657    validate_finite(rates.delta_n0_dot_rad_s2, "rates.delta_n0_dot_rad_s2")
658}
659
660fn validate_clock(clock: &ClockPolynomial) -> Result<()> {
661    validate_finite(clock.af0, "clock.af0")?;
662    validate_finite(clock.af1, "clock.af1")?;
663    validate_finite(clock.af2, "clock.af2")?;
664    validate_sow(clock.toc_sow, "clock.toc_sow")
665}
666
667fn validate_constants(consts: &ConstellationConstants) -> Result<()> {
668    validate_positive(consts.gm_m3_s2, "consts.gm_m3_s2")?;
669    validate_finite(consts.omega_e_rad_s, "consts.omega_e_rad_s")?;
670    validate_finite(consts.dtr_f, "consts.dtr_f")
671}
672
673fn validate_orbit_state(state: &OrbitState) -> Result<()> {
674    validate_finite(state.a, "orbit.a")?;
675    validate_finite(state.n0, "orbit.n0")?;
676    validate_finite(state.n, "orbit.n")?;
677    validate_finite(state.tk, "orbit.tk")?;
678    validate_finite(state.mk, "orbit.mk")?;
679    validate_finite(state.eccentric_anomaly, "orbit.eccentric_anomaly")?;
680    validate_finite(state.sin_e, "orbit.sin_e")?;
681    validate_finite(state.cos_e, "orbit.cos_e")?;
682    validate_finite(state.nu, "orbit.nu")?;
683    validate_finite(state.phi, "orbit.phi")?;
684    validate_finite(state.s2, "orbit.s2")?;
685    validate_finite(state.c2, "orbit.c2")?;
686    validate_finite(state.du, "orbit.du")?;
687    validate_finite(state.dr, "orbit.dr")?;
688    validate_finite(state.di, "orbit.di")?;
689    validate_finite(state.u, "orbit.u")?;
690    validate_finite(state.r, "orbit.r")?;
691    validate_finite(state.i, "orbit.i")?;
692    validate_finite(state.xp, "orbit.xp")?;
693    validate_finite(state.yp, "orbit.yp")?;
694    validate_finite(state.omega_k, "orbit.omega_k")?;
695    validate_finite(state.x_m, "orbit.x_m")?;
696    validate_finite(state.y_m, "orbit.y_m")?;
697    validate_finite(state.z_m, "orbit.z_m")
698}
699
700fn validate_clock_offset(clock: &ClockOffset) -> Result<()> {
701    validate_finite(clock.dt_clock_poly_s, "clock.dt_clock_poly_s")?;
702    validate_finite(clock.dt_rel_s, "clock.dt_rel_s")?;
703    validate_finite(clock.tgd_s, "clock.tgd_s")?;
704    validate_finite(clock.dt_clock_total_s, "clock.dt_clock_total_s")
705}
706
707fn validate_eccentricity(eccentricity: f64) -> Result<()> {
708    validate_finite(eccentricity, "eccentricity")?;
709    if (0.0..1.0).contains(&eccentricity) {
710        Ok(())
711    } else {
712        Err(invalid_input("eccentricity", "out of range"))
713    }
714}
715
716fn validate_sow(value: f64, field: &'static str) -> Result<()> {
717    validate_finite(value, field)?;
718    if (0.0..SECONDS_PER_WEEK).contains(&value) {
719        Ok(())
720    } else {
721        Err(invalid_input(field, "out of range"))
722    }
723}
724
725fn validate_positive(value: f64, field: &'static str) -> Result<()> {
726    validate_finite(value, field)?;
727    if value > 0.0 {
728        Ok(())
729    } else {
730        Err(invalid_input(field, "not positive"))
731    }
732}
733
734fn validate_finite(value: f64, field: &'static str) -> Result<()> {
735    if value.is_finite() {
736        Ok(())
737    } else {
738        Err(invalid_input(field, "not finite"))
739    }
740}
741
742fn invalid_input(field: &'static str, reason: &'static str) -> Error {
743    Error::InvalidInput(format!("{field} {reason}"))
744}
745
746#[cfg(test)]
747mod public_api_tests {
748    use super::*;
749
750    #[test]
751    fn relativistic_clock_correction_exposes_broadcast_formula() {
752        let dtr_f = ConstellationConstants::GPS.dtr_f;
753        let eccentricity = 0.013_456_789;
754        let sqrt_a = 5_153.795_477_5;
755        let sin_e = -0.625;
756        let got = relativistic_clock_correction_s(dtr_f, eccentricity, sqrt_a, sin_e)
757            .expect("valid relativistic correction");
758        let want = dtr_f * eccentricity * sqrt_a * sin_e;
759        assert_eq!(got.to_bits(), want.to_bits());
760    }
761
762    #[test]
763    fn relativistic_clock_correction_rejects_invalid_inputs() {
764        assert!(relativistic_clock_correction_s(f64::NAN, 0.01, 5_153.0, 0.5).is_err());
765        assert!(relativistic_clock_correction_s(
766            ConstellationConstants::GPS.dtr_f,
767            1.0,
768            5_153.0,
769            0.5
770        )
771        .is_err());
772        assert!(
773            relativistic_clock_correction_s(ConstellationConstants::GPS.dtr_f, 0.01, 0.0, 0.5)
774                .is_err()
775        );
776    }
777}
778
779#[cfg(all(test, sidereon_repo_tests))]
780mod tests;
sidereon_core/broadcast/mod.rs

sidereon_core/broadcast/
mod.rs
