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

sidereon_core/broadcast/
mod.rs
