predict_rs/
orbit.rs

1use chrono::{Datelike, Timelike};
2use core::f64::consts::{PI, TAU};
3use sgp4::{Constants, Elements};
4
5use crate::consts::{
6    AE, CK2, EARTH_RADIUS_KM_WGS84, GEOSYNCHRONOUS_ECCENTRICITY_THRESHOLD,
7    GEOSYNCHRONOUS_INCLINATION_THRESHOLD_DEGREES, GEOSYNCHRONOUS_LOWER_MEAN_MOTION,
8    GEOSYNCHRONOUS_UPPER_MEAN_MOTION, JULIAN_TIME_DIFF, MINUTES_PER_DAY, SOLAR_RADIUS_KM,
9    TWO_THIRD, XKE,
10};
11use crate::geodetic::calculate_lat_lon_alt;
12use crate::geodetic::Geodetic;
13use crate::julian_date::{daynum, julian_date_of_epoch, predict_to_julian_double};
14use crate::math::{
15    acos_clamped, asin_clamped, vec3_dot, vec3_length, vec3_mul_scalar, vec3_sub, Vec3, Vec4,
16};
17use crate::observer::calculate_user_posvel;
18use crate::predict::PredictPosition;
19use crate::sun::sun_predict;
20
21#[non_exhaustive]
22#[derive(Debug)]
23pub enum OrbitPredictionError {
24    Propagation(sgp4::Error),
25}
26
27#[must_use]
28pub fn predict_is_geosynchronous(m: &Elements) -> bool {
29    (m.mean_motion >= GEOSYNCHRONOUS_LOWER_MEAN_MOTION)
30        && (m.mean_motion <= GEOSYNCHRONOUS_UPPER_MEAN_MOTION)
31        && ((m.eccentricity).abs() <= GEOSYNCHRONOUS_ECCENTRICITY_THRESHOLD)
32        && ((m.inclination).abs() <= GEOSYNCHRONOUS_INCLINATION_THRESHOLD_DEGREES)
33}
34
35#[must_use]
36pub fn predict_apogee(m: &Elements) -> f64 {
37    let sma = 331.25 * ((1440.0 / m.mean_motion).ln() * (2.0 / 3.0)).exp();
38    sma * (1.0 + m.eccentricity) - EARTH_RADIUS_KM_WGS84
39}
40
41#[must_use]
42#[allow(clippy::similar_names)]
43pub fn predict_perigee(m: &Elements) -> f64 {
44    let xno = m.mean_motion * TAU / MINUTES_PER_DAY;
45    let a1 = (XKE / xno).powf(TWO_THIRD);
46    let cosio = (m.inclination * PI / 180.0).cos();
47    let theta2 = cosio * cosio;
48    let x3thm1 = 3.0 * theta2 - 1.0;
49    let eosq = m.eccentricity * m.eccentricity;
50    let betao2 = 1.0 - eosq;
51    let betao = betao2.sqrt();
52    let del1 = 1.5 * CK2 * x3thm1 / (a1 * a1 * betao * betao2);
53    let ao = a1 * (1.0 - del1 * (0.5 * TWO_THIRD + del1 * (1.0 + 134.0 / 81.0 * del1)));
54    let delo = 1.5 * CK2 * x3thm1 / (ao * ao * betao * betao2);
55    let aodp = ao / (1.0 - delo);
56
57    (aodp * (1.0 - m.eccentricity) - AE) * EARTH_RADIUS_KM_WGS84
58}
59
60/// Returns true if the satellite pointed to by "x" can ever rise above the horizon of the ground station
61#[must_use]
62pub fn predict_aos_happens(m: &Elements, latitude: f64) -> bool {
63    if m.mean_motion == 0.0 {
64        false
65    } else {
66        let mut lin = m.inclination;
67
68        if lin >= 90.0 {
69            lin = 180.0 - lin;
70        }
71        let apogee = predict_apogee(m);
72
73        ((EARTH_RADIUS_KM_WGS84 / (apogee + EARTH_RADIUS_KM_WGS84)).acos() + (lin * PI / 180.0))
74            > latitude.abs()
75    }
76}
77
78/// This is the stuff we need to do repetitively while tracking
79/// This is the old `Calc()` function
80/// # Errors
81///
82/// Can return `OrbitPredictionError`
83pub fn predict_orbit(
84    elements: &Elements,
85    constants: &Constants,
86    unix_time: f64,
87) -> Result<PredictPosition, OrbitPredictionError> {
88    #[allow(clippy::cast_precision_loss)]
89    let tsince =
90        (unix_time - (elements.datetime.and_utc().timestamp_micros() as f64 / 1_000_000.0)) / 60.0;
91    let jultime = predict_to_julian_double(unix_time);
92    let prediction = constants
93        .propagate(sgp4::MinutesSinceEpoch(tsince))
94        .map_err(OrbitPredictionError::Propagation)?;
95
96    let position = Vec3(
97        prediction.position[0],
98        prediction.position[1],
99        prediction.position[2],
100    );
101    let velocity = Vec3(
102        prediction.velocity[0],
103        prediction.velocity[1],
104        prediction.velocity[2],
105    );
106
107    let sat_geo = calculate_lat_lon_alt(jultime, position);
108
109    let latitude = sat_geo.lat;
110    let longitude = sat_geo.lon;
111    let altitude = sat_geo.alt;
112
113    // Calculate solar position
114    let solar_vector = sun_predict(jultime);
115
116    // Find eclipse depth and if sat is eclipsed
117    let (eclipse_depth, eclipsed) = is_eclipsed(position, solar_vector);
118
119    // Calculate footprint
120    let footprint = 2.0
121        * EARTH_RADIUS_KM_WGS84
122        * (EARTH_RADIUS_KM_WGS84 / (EARTH_RADIUS_KM_WGS84 + altitude)).acos();
123
124    // Calculate current number of revolutions around Earth
125    let temp = TAU / MINUTES_PER_DAY / MINUTES_PER_DAY;
126    let epoch_day = f64::from(
127        elements.datetime.ordinal() * 86400
128            + elements.datetime.hour() * 3600
129            + elements.datetime.minute() * 60
130            + elements.datetime.second(),
131    ) / 86400.0;
132    let epoch_year = elements.datetime.year() - 2000;
133    let epoch = 1000.0 * f64::from(epoch_year) + epoch_day;
134    let jul_epoch = julian_date_of_epoch(epoch);
135    let age = jultime - jul_epoch + JULIAN_TIME_DIFF;
136    let mean_motion = elements.mean_motion * temp * MINUTES_PER_DAY;
137    let mean_anomaly = elements.mean_anomaly * PI / 180.0;
138    #[allow(clippy::cast_precision_loss)]
139    let revolutions = ((mean_motion * MINUTES_PER_DAY / (PI * 2.0) + age * elements.drag_term)
140        * age
141        + mean_anomaly / (2.0 * PI))
142        + elements.revolution_number as f64;
143
144    //calculate whether orbit is decayed
145    let decayed = predict_decayed(elements, jultime);
146    Ok(PredictPosition {
147        position,
148        velocity,
149        latitude,
150        longitude,
151        altitude,
152        eclipsed,
153        footprint,
154        decayed,
155        eclipse_depth,
156        revolutions,
157        time: unix_time,
158    })
159}
160
161#[must_use]
162pub fn predict_decayed(elements: &Elements, time: f64) -> bool {
163    let satepoch = daynum(1, 0, elements.datetime.year()) + i64::from(elements.datetime.day());
164    let mut has_decayed = false;
165    #[allow(clippy::cast_precision_loss)]
166    if satepoch as f64
167        + ((16.666_666 - elements.mean_motion) / (10.0 * (elements.mean_motion_dot).abs()))
168        < time
169    {
170        has_decayed = true;
171    }
172    has_decayed
173}
174
175/// Calculates if a position is eclipsed.
176#[must_use]
177pub fn is_eclipsed(pos: Vec3, sol: Vec3) -> (f64, bool) {
178    // Determine partial eclipse
179    let sd_earth = asin_clamped(EARTH_RADIUS_KM_WGS84 / vec3_length(pos));
180    let rho = vec3_sub(sol, pos);
181    let sd_sun = asin_clamped(SOLAR_RADIUS_KM / vec3_length(rho));
182    let earth = vec3_mul_scalar(pos, -1.0);
183    let delta = acos_clamped(vec3_dot(sol, earth) / vec3_length(sol) / vec3_length(earth));
184    let depth = sd_earth - sd_sun - delta;
185
186    let is_eclipsed = if sd_earth < sd_sun {
187        false
188    } else {
189        depth >= 0.0
190    };
191    (depth, is_eclipsed)
192}
193
194/// The procedures `calculate_obs` calculates the *topocentric* coordinates of the object with ECI position,
195/// {pos}, and velocity, {vel}, from location {geodetic} at a given {time}.
196///
197/// The function `calculate_obs` returns a Vec4 containingthe azimuth,
198/// elevation, range, and range rate (in that order) with units of
199/// radians, radians, kilometers, and kilometers/second, respectively.
200/// The WGS '72 geoid is used and the effect of atmospheric refraction
201/// (under standard temperature and pressure) is incorporated into the
202/// elevation calculation; the effect of atmospheric refraction on
203/// range and range rate has not yet been quantified.
204#[must_use]
205pub fn calculate_obs(time: f64, pos: Vec3, vel: Vec3, mut geodetic: Geodetic) -> Vec4 {
206    let (obs_pos, obs_vel) = calculate_user_posvel(time, &mut geodetic);
207
208    let range = vec3_sub(pos, obs_pos);
209    let rgvel = vec3_sub(vel, obs_vel);
210
211    let range_length = vec3_length(range);
212
213    let sin_lat = geodetic.lat.sin();
214    let cos_lat = geodetic.lat.cos();
215    let sin_theta = geodetic.theta.sin();
216    let cos_theta = geodetic.theta.cos();
217    let top_s = sin_lat * cos_theta * range.0 + sin_lat * sin_theta * range.1 - cos_lat * range.2;
218    let top_e = -sin_theta * range.0 + cos_theta * range.1;
219    let top_z = cos_lat * cos_theta * range.0 + cos_lat * sin_theta * range.1 + sin_lat * range.2;
220    let mut azim = (-top_e / top_s).atan(); /* Azimuth */
221
222    if top_s > 0.0 {
223        azim += PI;
224    }
225
226    if azim < 0.0 {
227        azim += 2.0 * PI;
228    }
229
230    let elevation = asin_clamped(top_z / range_length);
231
232    Vec4(
233        azim,
234        elevation,
235        range_length, /* Range (kilometers)  */
236        /* Range Rate (kilometers/second) */
237        vec3_dot(range, rgvel) / vec3_length(range),
238    )
239}
predict_rs/orbit.rs

predict_rs/
orbit.rs
