1use 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
30pub use crate::constants::HALF_WEEK_S;
32pub use crate::constants::SECONDS_PER_WEEK;
34
35pub const KEPLER_TOL: f64 = 1.0e-12;
37pub const KEPLER_MAX_ITER: usize = 30;
39pub const CLOCK_MAX_ITER: usize = 2;
41
42#[derive(Debug, Clone, Copy, PartialEq)]
47pub struct ConstellationConstants {
48 pub gm_m3_s2: f64,
50 pub omega_e_rad_s: f64,
52 pub dtr_f: f64,
54}
55
56impl ConstellationConstants {
57 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 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 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#[derive(Debug, Clone, Copy, PartialEq)]
80pub struct KeplerianElements {
81 pub sqrt_a: f64,
83 pub e: f64,
85 pub m0: f64,
87 pub delta_n: f64,
89 pub omega0: f64,
91 pub i0: f64,
93 pub omega: f64,
95 pub omega_dot: f64,
97 pub idot: f64,
99 pub cuc: f64,
101 pub cus: f64,
103 pub crc: f64,
105 pub crs: f64,
107 pub cic: f64,
109 pub cis: f64,
111 pub toe_sow: f64,
113}
114
115#[derive(Debug, Clone, Copy, PartialEq)]
117pub struct ClockPolynomial {
118 pub af0: f64,
120 pub af1: f64,
122 pub af2: f64,
124 pub toc_sow: f64,
126}
127
128#[derive(Debug, Clone, Copy, PartialEq)]
130pub struct EccentricAnomaly {
131 pub value: f64,
133 pub iterations: usize,
135}
136
137#[derive(Debug, Clone, Copy, PartialEq)]
142pub struct OrbitState {
143 pub a: f64,
145 pub n0: f64,
147 pub n: f64,
149 pub tk: f64,
151 pub mk: f64,
153 pub eccentric_anomaly: f64,
155 pub kepler_iterations: usize,
157 pub sin_e: f64,
159 pub cos_e: f64,
161 pub nu: f64,
163 pub phi: f64,
165 pub s2: f64,
167 pub c2: f64,
169 pub du: f64,
171 pub dr: f64,
173 pub di: f64,
175 pub u: f64,
177 pub r: f64,
179 pub i: f64,
181 pub xp: f64,
183 pub yp: f64,
185 pub omega_k: f64,
187 pub x_m: f64,
189 pub y_m: f64,
191 pub z_m: f64,
193}
194
195impl OrbitState {
196 pub const fn position(&self) -> core::result::Result<ItrfPositionM, FrameValueError> {
198 ItrfPositionM::new(self.x_m, self.y_m, self.z_m)
199 }
200}
201
202#[derive(Debug, Clone, Copy, PartialEq)]
204pub struct ClockOffset {
205 pub dt_clock_poly_s: f64,
207 pub dt_rel_s: f64,
209 pub tgd_s: f64,
211 pub dt_clock_total_s: f64,
213}
214
215pub fn time_from_reference_s(t_sow_s: f64, t_ref_sow_s: f64) -> f64 {
220 let mut dt = t_sow_s - t_ref_sow_s;
221 if dt > HALF_WEEK_S {
222 dt -= SECONDS_PER_WEEK;
223 }
224 if dt < -HALF_WEEK_S {
225 dt += SECONDS_PER_WEEK;
226 }
227 dt
228}
229
230pub fn eccentric_anomaly(mean_anomaly_rad: f64, eccentricity: f64) -> Result<EccentricAnomaly> {
233 validate_finite(mean_anomaly_rad, "mean_anomaly_rad")?;
234 validate_eccentricity(eccentricity)?;
235
236 Ok(eccentric_anomaly_unchecked(mean_anomaly_rad, eccentricity))
237}
238
239pub(crate) fn eccentric_anomaly_unchecked(
240 mean_anomaly_rad: f64,
241 eccentricity: f64,
242) -> EccentricAnomaly {
243 let mut e_k = mean_anomaly_rad;
244 let mut iterations = 0usize;
245 while iterations < KEPLER_MAX_ITER {
246 let e_prev = e_k;
247 e_k = mean_anomaly_rad + eccentricity * e_prev.sin();
248 iterations += 1;
249 let delta = (e_k - e_prev).abs();
250 if delta <= KEPLER_TOL {
251 break;
252 }
253 }
254 EccentricAnomaly {
255 value: e_k,
256 iterations,
257 }
258}
259
260pub fn satellite_position_ecef(
267 elements: &KeplerianElements,
268 consts: &ConstellationConstants,
269 t_sow_s: f64,
270 is_geo: bool,
271) -> Result<OrbitState> {
272 validate_elements(elements)?;
273 validate_constants(consts)?;
274 validate_finite(t_sow_s, "t_sow_s")?;
275
276 let state = satellite_position_ecef_unchecked(elements, consts, t_sow_s, is_geo);
277 validate_orbit_state(&state)?;
278 Ok(state)
279}
280
281pub(crate) fn satellite_position_ecef_unchecked(
282 elements: &KeplerianElements,
283 consts: &ConstellationConstants,
284 t_sow_s: f64,
285 is_geo: bool,
286) -> OrbitState {
287 let sqrt_a = elements.sqrt_a;
288 let e = elements.e;
289 let gm = consts.gm_m3_s2;
290 let omega_e = consts.omega_e_rad_s;
291
292 let a = sqrt_a * sqrt_a;
294 let n0 = (gm / (a * a * a)).sqrt();
295 let n = n0 + elements.delta_n;
296
297 let tk = time_from_reference_s(t_sow_s, elements.toe_sow);
299
300 let mk = elements.m0 + n * tk;
302 let kepler = eccentric_anomaly_unchecked(mk, e);
303 let ecc_anom = kepler.value;
304 let sin_e = ecc_anom.sin();
305 let cos_e = ecc_anom.cos();
306
307 let e2 = e * e;
309 let nu = ((1.0 - e2).sqrt() * sin_e).atan2(cos_e - e);
310 let phi = nu + elements.omega;
311
312 let two_phi = 2.0 * phi;
314 let s2 = two_phi.sin();
315 let c2 = two_phi.cos();
316 let du = elements.cus * s2 + elements.cuc * c2;
317 let dr = elements.crs * s2 + elements.crc * c2;
318 let di = elements.cis * s2 + elements.cic * c2;
319
320 let u = phi + du;
322 let r = a * (1.0 - e * cos_e) + dr;
323 let i = elements.i0 + di + elements.idot * tk;
324
325 let xp = r * u.cos();
327 let yp = r * u.sin();
328
329 let omega_k = if is_geo {
332 elements.omega0 + elements.omega_dot * tk - omega_e * elements.toe_sow
333 } else {
334 elements.omega0 + (elements.omega_dot - omega_e) * tk - omega_e * elements.toe_sow
335 };
336
337 let sin_o = omega_k.sin();
339 let cos_o = omega_k.cos();
340 let sin_i = i.sin();
341 let cos_i = i.cos();
342 let xg = xp * cos_o - yp * cos_i * sin_o;
343 let yg = xp * sin_o + yp * cos_i * cos_o;
344 let zg = yp * sin_i;
345
346 let (x, y, z) = if is_geo {
349 let deg5 = 5.0_f64.to_radians();
350 let cos_phi = deg5.cos();
351 let sin_phi = -deg5.sin();
352 let z_ang = omega_e * tk;
353 let cos_z = z_ang.cos();
354 let sin_z = z_ang.sin();
355 let yr = yg * cos_phi + zg * sin_phi;
356 let zr = -yg * sin_phi + zg * cos_phi;
357 (xg * cos_z + yr * sin_z, -xg * sin_z + yr * cos_z, zr)
358 } else {
359 (xg, yg, zg)
360 };
361
362 OrbitState {
363 a,
364 n0,
365 n,
366 tk,
367 mk,
368 eccentric_anomaly: ecc_anom,
369 kepler_iterations: kepler.iterations,
370 sin_e,
371 cos_e,
372 nu,
373 phi,
374 s2,
375 c2,
376 du,
377 dr,
378 di,
379 u,
380 r,
381 i,
382 xp,
383 yp,
384 omega_k,
385 x_m: x,
386 y_m: y,
387 z_m: z,
388 }
389}
390
391pub fn satellite_clock_offset_s(
397 clock: &ClockPolynomial,
398 consts: &ConstellationConstants,
399 elements: &KeplerianElements,
400 sin_e: f64,
401 t_sow_s: f64,
402 tgd_s: f64,
403) -> Result<ClockOffset> {
404 validate_clock(clock)?;
405 validate_constants(consts)?;
406 validate_elements(elements)?;
407 validate_finite(sin_e, "sin_e")?;
408 validate_finite(t_sow_s, "t_sow_s")?;
409 validate_finite(tgd_s, "tgd_s")?;
410
411 let offset = satellite_clock_offset_s_unchecked(clock, consts, elements, sin_e, t_sow_s, tgd_s);
412 validate_clock_offset(&offset)?;
413 Ok(offset)
414}
415
416pub(crate) fn satellite_clock_offset_s_unchecked(
417 clock: &ClockPolynomial,
418 consts: &ConstellationConstants,
419 elements: &KeplerianElements,
420 sin_e: f64,
421 t_sow_s: f64,
422 tgd_s: f64,
423) -> ClockOffset {
424 let af0 = clock.af0;
425 let af1 = clock.af1;
426 let af2 = clock.af2;
427
428 let dt0 = time_from_reference_s(t_sow_s, clock.toc_sow);
430 let mut dt = dt0;
431 let mut refine = 0usize;
432 while refine < CLOCK_MAX_ITER {
433 dt = dt0 - (af0 + af1 * dt + af2 * dt * dt);
434 refine += 1;
435 }
436 let dt_poly = af0 + af1 * dt + af2 * dt * dt;
437
438 let dt_rel = consts.dtr_f * elements.e * elements.sqrt_a * sin_e;
440
441 let dt_total = dt_poly + dt_rel - tgd_s;
442
443 ClockOffset {
444 dt_clock_poly_s: dt_poly,
445 dt_rel_s: dt_rel,
446 tgd_s,
447 dt_clock_total_s: dt_total,
448 }
449}
450
451#[derive(Debug, Clone, Copy, PartialEq)]
453pub struct SatelliteState {
454 pub orbit: OrbitState,
456 pub clock: ClockOffset,
458}
459
460pub fn satellite_state(
468 elements: &KeplerianElements,
469 clock: &ClockPolynomial,
470 consts: &ConstellationConstants,
471 t_sow_s: f64,
472 tgd_s: f64,
473 is_geo: bool,
474) -> Result<SatelliteState> {
475 validate_elements(elements)?;
476 validate_clock(clock)?;
477 validate_constants(consts)?;
478 validate_finite(t_sow_s, "t_sow_s")?;
479 validate_finite(tgd_s, "tgd_s")?;
480
481 let state = satellite_state_unchecked(elements, clock, consts, t_sow_s, tgd_s, is_geo);
482 validate_orbit_state(&state.orbit)?;
483 validate_clock_offset(&state.clock)?;
484 Ok(state)
485}
486
487pub(crate) fn satellite_state_unchecked(
488 elements: &KeplerianElements,
489 clock: &ClockPolynomial,
490 consts: &ConstellationConstants,
491 t_sow_s: f64,
492 tgd_s: f64,
493 is_geo: bool,
494) -> SatelliteState {
495 let orbit = satellite_position_ecef_unchecked(elements, consts, t_sow_s, is_geo);
496 let clock =
497 satellite_clock_offset_s_unchecked(clock, consts, elements, orbit.sin_e, t_sow_s, tgd_s);
498 SatelliteState { orbit, clock }
499}
500
501fn validate_elements(elements: &KeplerianElements) -> Result<()> {
502 validate_positive(elements.sqrt_a, "elements.sqrt_a")?;
503 validate_eccentricity(elements.e)?;
504 validate_finite(elements.m0, "elements.m0")?;
505 validate_finite(elements.delta_n, "elements.delta_n")?;
506 validate_finite(elements.omega0, "elements.omega0")?;
507 validate_finite(elements.i0, "elements.i0")?;
508 validate_finite(elements.omega, "elements.omega")?;
509 validate_finite(elements.omega_dot, "elements.omega_dot")?;
510 validate_finite(elements.idot, "elements.idot")?;
511 validate_finite(elements.cuc, "elements.cuc")?;
512 validate_finite(elements.cus, "elements.cus")?;
513 validate_finite(elements.crc, "elements.crc")?;
514 validate_finite(elements.crs, "elements.crs")?;
515 validate_finite(elements.cic, "elements.cic")?;
516 validate_finite(elements.cis, "elements.cis")?;
517 validate_sow(elements.toe_sow, "elements.toe_sow")
518}
519
520fn validate_clock(clock: &ClockPolynomial) -> Result<()> {
521 validate_finite(clock.af0, "clock.af0")?;
522 validate_finite(clock.af1, "clock.af1")?;
523 validate_finite(clock.af2, "clock.af2")?;
524 validate_sow(clock.toc_sow, "clock.toc_sow")
525}
526
527fn validate_constants(consts: &ConstellationConstants) -> Result<()> {
528 validate_positive(consts.gm_m3_s2, "consts.gm_m3_s2")?;
529 validate_finite(consts.omega_e_rad_s, "consts.omega_e_rad_s")?;
530 validate_finite(consts.dtr_f, "consts.dtr_f")
531}
532
533fn validate_orbit_state(state: &OrbitState) -> Result<()> {
534 validate_finite(state.a, "orbit.a")?;
535 validate_finite(state.n0, "orbit.n0")?;
536 validate_finite(state.n, "orbit.n")?;
537 validate_finite(state.tk, "orbit.tk")?;
538 validate_finite(state.mk, "orbit.mk")?;
539 validate_finite(state.eccentric_anomaly, "orbit.eccentric_anomaly")?;
540 validate_finite(state.sin_e, "orbit.sin_e")?;
541 validate_finite(state.cos_e, "orbit.cos_e")?;
542 validate_finite(state.nu, "orbit.nu")?;
543 validate_finite(state.phi, "orbit.phi")?;
544 validate_finite(state.s2, "orbit.s2")?;
545 validate_finite(state.c2, "orbit.c2")?;
546 validate_finite(state.du, "orbit.du")?;
547 validate_finite(state.dr, "orbit.dr")?;
548 validate_finite(state.di, "orbit.di")?;
549 validate_finite(state.u, "orbit.u")?;
550 validate_finite(state.r, "orbit.r")?;
551 validate_finite(state.i, "orbit.i")?;
552 validate_finite(state.xp, "orbit.xp")?;
553 validate_finite(state.yp, "orbit.yp")?;
554 validate_finite(state.omega_k, "orbit.omega_k")?;
555 validate_finite(state.x_m, "orbit.x_m")?;
556 validate_finite(state.y_m, "orbit.y_m")?;
557 validate_finite(state.z_m, "orbit.z_m")
558}
559
560fn validate_clock_offset(clock: &ClockOffset) -> Result<()> {
561 validate_finite(clock.dt_clock_poly_s, "clock.dt_clock_poly_s")?;
562 validate_finite(clock.dt_rel_s, "clock.dt_rel_s")?;
563 validate_finite(clock.tgd_s, "clock.tgd_s")?;
564 validate_finite(clock.dt_clock_total_s, "clock.dt_clock_total_s")
565}
566
567fn validate_eccentricity(eccentricity: f64) -> Result<()> {
568 validate_finite(eccentricity, "eccentricity")?;
569 if (0.0..1.0).contains(&eccentricity) {
570 Ok(())
571 } else {
572 Err(invalid_input("eccentricity", "out of range"))
573 }
574}
575
576fn validate_sow(value: f64, field: &'static str) -> Result<()> {
577 validate_finite(value, field)?;
578 if (0.0..SECONDS_PER_WEEK).contains(&value) {
579 Ok(())
580 } else {
581 Err(invalid_input(field, "out of range"))
582 }
583}
584
585fn validate_positive(value: f64, field: &'static str) -> Result<()> {
586 validate_finite(value, field)?;
587 if value > 0.0 {
588 Ok(())
589 } else {
590 Err(invalid_input(field, "not positive"))
591 }
592}
593
594fn validate_finite(value: f64, field: &'static str) -> Result<()> {
595 if value.is_finite() {
596 Ok(())
597 } else {
598 Err(invalid_input(field, "not finite"))
599 }
600}
601
602fn invalid_input(field: &'static str, reason: &'static str) -> Error {
603 Error::InvalidInput(format!("{field} {reason}"))
604}
605
606#[cfg(all(test, sidereon_repo_tests))]
607mod tests;