gistools/space/propagation/
sgp4.rs

1use crate::space::{
2    EARTH_RADIUS_KM, Method, Satellite,
3    propagation::{DpperOptions, DspaceOptions, dpper, dspace},
4    util::constants::{J2, J3_J2, VKMPERSEC, X2_3, XKE},
5};
6use alloc::{format, string::String};
7use core::f64::consts::{PI, TAU};
8use libm::{atan2, cos, fabs, pow, sin, sqrt};
9use s2json::VectorPoint;
10
11/// An error output from an sgp4 computation
12#[derive(Debug, Default, Clone, PartialEq)]
13pub struct SGP4ErrorOutput {
14    /// The type of error
15    pub r#type: u64,
16    /// The error
17    pub error: String,
18}
19
20/// A successful output from an sgp4 computation
21#[derive(Debug, Default, Clone, PartialEq)]
22pub struct SGP4Output {
23    /// The position of the satellite
24    pub position: VectorPoint,
25    /// The velocity of the satellite
26    pub velocity: VectorPoint,
27}
28
29/// procedure sgp4
30///
31/// this procedure is the sgp4 prediction model from space command. this is an
32/// updated and combined version of sgp4 and sdp4, which were originally
33/// published separately in spacetrack report //3. this version follows the
34/// methodology from the aiaa paper (2006) describing the history and
35/// development of the code.
36///
37/// author         david vallado                  719-573-2600   28 jun 2005
38///
39/// references
40/// - hoots, roehrich, norad spacetrack report //3 1980
41/// - hoots, norad spacetrack report //6 1986
42/// - hoots, schumacher and glover 2004
43/// - vallado, crawford, hujsak, kelso  2006
44///
45/// ## Parameters
46/// - `sat`: the satellite object to propagate
47/// - `tsince`: the time since the epoch
48///
49/// ## Returns
50/// The position and velocity of the satellite or an error report
51pub fn sgp4(sat: &Satellite, tsince: f64) -> Result<SGP4Output, SGP4ErrorOutput> {
52    let Satellite {
53        anomaly,
54        motion,
55        eccentricity,
56        inclination,
57        method,
58        drag,
59        mdot,
60        perigee,
61        argpdot,
62        ascension,
63        nodedot,
64        nodecf,
65        cc1,
66        cc4,
67        cc5,
68        t2cof,
69        isimp,
70        omgcof,
71        eta,
72        xmcof,
73        delmo,
74        d2,
75        d3,
76        d4,
77        sinmao,
78        t3cof,
79        t4cof,
80        t5cof,
81        irez,
82        d2201,
83        d2211,
84        d3210,
85        d3222,
86        d4410,
87        d4422,
88        d5220,
89        d5232,
90        d5421,
91        d5433,
92        dedt,
93        del1,
94        del2,
95        del3,
96        didt,
97        dmdt,
98        dnodt,
99        domdt,
100        // opsmode,
101        gsto,
102        xfact,
103        xlamo,
104        atime,
105        xli,
106        xni,
107        mut aycof,
108        mut xlcof,
109        mut con41,
110        mut x1mth2,
111        mut x7thm1,
112        ..
113    } = *sat;
114
115    let mut coseo1 = 0.;
116    let mut sineo1 = 0.;
117    let mut cosip: f64;
118    let mut sinip: f64;
119    let cosisq: f64;
120    let delm: f64;
121    let delomg: f64;
122    let mut eo1: f64;
123    let mut argpm: f64;
124    let mut argpp: f64;
125    let mut su: f64;
126    let t3: f64;
127    let t4: f64;
128    let tc: f64;
129    let mut tem5: f64;
130    let mut temp: f64;
131    let mut tempa: f64;
132    let mut tempe: f64;
133    let mut templ: f64;
134    let mut inclm: f64;
135    let mut mm: f64;
136    let mut nm: f64;
137    let mut nodem: f64;
138    let mut xincp: f64;
139    let mut xlm: f64;
140    let mut mp: f64;
141    let mut nodep: f64;
142
143    /* ------------------ set mathematical constants --------------- */
144    // sgp4fix divisor for divide by zero check on inclination
145    // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
146    // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
147
148    let temp4 = 1.5e-12;
149
150    //  ------- update for secular gravity and atmospheric drag -----
151    let xmdf = anomaly + mdot * tsince;
152    let argpdf = perigee + argpdot * tsince;
153    let nodedf = ascension + nodedot * tsince;
154    argpm = argpdf;
155    mm = xmdf;
156    let t2 = tsince * tsince;
157    nodem = nodedf + nodecf * t2;
158    tempa = 1.0 - cc1 * tsince;
159    tempe = drag * cc4 * tsince;
160    templ = t2cof * t2;
161
162    if isimp != 1. {
163        delomg = omgcof * tsince;
164        //  sgp4fix use mutliply for speed instead of pow
165        let delmtemp = 1.0 + eta * cos(xmdf);
166        delm = xmcof * (delmtemp * delmtemp * delmtemp - delmo);
167        temp = delomg + delm;
168        mm = xmdf + temp;
169        argpm = argpdf - temp;
170        t3 = t2 * tsince;
171        t4 = t3 * tsince;
172        tempa = tempa - d2 * t2 - d3 * t3 - d4 * t4;
173        tempe += drag * cc5 * (sin(mm) - sinmao);
174        templ = templ + t3cof * t3 + t4 * (t4cof + tsince * t5cof);
175    }
176    nm = motion;
177    let mut em = eccentricity;
178    inclm = inclination;
179    if method == Method::D {
180        tc = tsince;
181
182        let dspace_options = DspaceOptions {
183            irez,
184            d2201,
185            d2211,
186            d3210,
187            d3222,
188            d4410,
189            d4422,
190            d5220,
191            d5232,
192            d5421,
193            d5433,
194            dedt,
195            del1,
196            del2,
197            del3,
198            didt,
199            dmdt,
200            dnodt,
201            domdt,
202            argpo: perigee,
203            argpdot,
204            tc,
205            gsto,
206            xfact,
207            xlamo,
208            no: motion,
209            atime,
210            em,
211            argpm,
212            inclm,
213            xli,
214            mm,
215            xni,
216            nodem,
217            nm,
218        };
219
220        let dspace_result = dspace(dspace_options, tsince);
221        em = dspace_result.em;
222        argpm = dspace_result.argpm;
223        inclm = dspace_result.inclm;
224        mm = dspace_result.mm;
225        nodem = dspace_result.nodem;
226        nm = dspace_result.nm;
227    }
228
229    if nm <= 0.0 {
230        // sgp4fix add return
231        return Err(SGP4ErrorOutput { r#type: 2, error: format!("error nm {}", nm) });
232    }
233
234    // let am = (XKE / nm) ** X2_3 * tempa * tempa;
235    let am = pow(XKE / nm, X2_3) * tempa * tempa;
236    // nm = XKE / am ** 1.5;
237    nm = XKE / pow(am, 1.5);
238    em -= tempe;
239
240    // fix tolerance for error recognition
241    // sgp4fix am is fixed from the previous nm check
242    if !(-0.001..1.0).contains(&em) {
243        // || (am < 0.95)
244        // sgp4fix to return if there is an error in eccentricity
245        return Err(SGP4ErrorOutput { r#type: 1, error: format!("error em {}", em) });
246    }
247
248    //  sgp4fix fix tolerance to avoid a divide by zero
249    if em < 1.0e-6 {
250        em = 1.0e-6;
251    }
252    mm += motion * templ;
253    xlm = mm + argpm + nodem;
254
255    nodem %= TAU;
256    argpm %= TAU;
257    xlm %= TAU;
258    mm = (xlm - argpm - nodem) % TAU;
259
260    // ----------------- compute extra mean quantities -------------
261    let sinim = sin(inclm);
262    let cosim = cos(inclm);
263
264    // -------------------- add lunar-solar periodics --------------
265    let mut ep = em;
266    xincp = inclm;
267    argpp = argpm;
268    nodep = nodem;
269    mp = mm;
270    sinip = sinim;
271    cosip = cosim;
272    if method == Method::D {
273        let dpper_parameters = DpperOptions { init: false, ep, inclp: xincp, nodep, argpp, mp };
274        let dpper_result = dpper(sat, dpper_parameters, tsince);
275        ep = dpper_result.ep;
276        nodep = dpper_result.nodep;
277        argpp = dpper_result.argpp;
278        mp = dpper_result.mp;
279
280        xincp = dpper_result.inclp;
281
282        if xincp < 0.0 {
283            xincp = -xincp;
284            nodep += PI;
285            argpp -= PI;
286        }
287        if !(0.0..=1.0).contains(&ep) {
288            //  sgp4fix add return
289            return Err(SGP4ErrorOutput { r#type: 3, error: format!("error ep {}", ep) });
290        }
291    }
292
293    //  -------------------- long period periodics ------------------
294    if method == Method::D {
295        sinip = sin(xincp);
296        cosip = cos(xincp);
297        aycof = -0.5 * J3_J2 * sinip;
298
299        //  sgp4fix for divide by zero for xincp = 180 deg
300        if fabs(cosip + 1.0) > 1.5e-12 {
301            xlcof = (-0.25 * J3_J2 * sinip * (3.0 + 5.0 * cosip)) / (1.0 + cosip);
302        } else {
303            xlcof = (-0.25 * J3_J2 * sinip * (3.0 + 5.0 * cosip)) / temp4;
304        }
305    }
306
307    let axnl = ep * cos(argpp);
308    temp = 1.0 / (am * (1.0 - ep * ep));
309    let aynl = ep * sin(argpp) + temp * aycof;
310    let xl = mp + argpp + nodep + temp * xlcof * axnl;
311
312    // --------------------- solve kepler's equation ---------------
313    let u = (xl - nodep) % TAU;
314    eo1 = u;
315    tem5 = 9999.9;
316    let mut ktr = 1;
317
318    //    sgp4fix for kepler iteration
319    //    the following iteration needs better limits on corrections
320    while fabs(tem5) >= 1.0e-12 && ktr <= 10 {
321        sineo1 = sin(eo1);
322        coseo1 = cos(eo1);
323        tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl;
324        tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5;
325        if fabs(tem5) >= 0.95 {
326            if tem5 > 0.0 {
327                tem5 = 0.95;
328            } else {
329                tem5 = -0.95;
330            }
331        }
332        eo1 += tem5;
333        ktr += 1;
334    }
335
336    //  ------------- short period preliminary quantities -----------
337    let ecose = axnl * coseo1 + aynl * sineo1;
338    let esine = axnl * sineo1 - aynl * coseo1;
339    let el2 = axnl * axnl + aynl * aynl;
340    let pl = am * (1.0 - el2);
341    if pl < 0.0 {
342        //  sgp4fix add return
343        return Err(SGP4ErrorOutput { r#type: 4, error: format!("error pl {}", pl) });
344    }
345
346    let rl = am * (1.0 - ecose);
347    let rdotl = (sqrt(am) * esine) / rl;
348    let rvdotl = sqrt(pl) / rl;
349    let betal = sqrt(1.0 - el2);
350    temp = esine / (1.0 + betal);
351    let sinu = (am / rl) * (sineo1 - aynl - axnl * temp);
352    let cosu = (am / rl) * (coseo1 - axnl + aynl * temp);
353    su = atan2(sinu, cosu);
354    let sin2u = (cosu + cosu) * sinu;
355    let cos2u = 1.0 - 2.0 * sinu * sinu;
356    temp = 1.0 / pl;
357    let temp1 = 0.5 * J2 * temp;
358    let temp2 = temp1 * temp;
359
360    // -------------- update for short period periodics ------------
361    if method == Method::D {
362        cosisq = cosip * cosip;
363        con41 = 3.0 * cosisq - 1.0;
364        x1mth2 = 1.0 - cosisq;
365        x7thm1 = 7.0 * cosisq - 1.0;
366    }
367
368    let mrt = rl * (1.0 - 1.5 * temp2 * betal * con41) + 0.5 * temp1 * x1mth2 * cos2u;
369
370    // sgp4fix for decaying satellites
371    if mrt < 1.0 {
372        return Err(SGP4ErrorOutput { r#type: 6, error: format!("decay condition {}", mrt) });
373    }
374
375    su -= 0.25 * temp2 * x7thm1 * sin2u;
376    let xnode = nodep + 1.5 * temp2 * cosip * sin2u;
377    let xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u;
378    let mvt = rdotl - (nm * temp1 * x1mth2 * sin2u) / XKE;
379    let rvdot = rvdotl + (nm * temp1 * (x1mth2 * cos2u + 1.5 * con41)) / XKE;
380
381    // --------------------- orientation vectors -------------------
382    let sinsu = sin(su);
383    let cossu = cos(su);
384    let snod = sin(xnode);
385    let cnod = cos(xnode);
386    let sini = sin(xinc);
387    let cosi = cos(xinc);
388    let xmx = -snod * cosi;
389    let xmy = cnod * cosi;
390    let ux = xmx * sinsu + cnod * cossu;
391    let uy = xmy * sinsu + snod * cossu;
392    let uz = sini * sinsu;
393    let vx = xmx * cossu - cnod * sinsu;
394    let vy = xmy * cossu - snod * sinsu;
395    let vz = sini * cossu;
396
397    // --------- position and velocity (in km and km/sec) ----------
398    Ok(SGP4Output {
399        position: VectorPoint::new_xyz(
400            mrt * ux * EARTH_RADIUS_KM,
401            mrt * uy * EARTH_RADIUS_KM,
402            mrt * uz * EARTH_RADIUS_KM,
403            None,
404        ),
405        velocity: VectorPoint::new_xyz(
406            (mvt * ux + rvdot * vx) * VKMPERSEC,
407            (mvt * uy + rvdot * vy) * VKMPERSEC,
408            (mvt * uz + rvdot * vz) * VKMPERSEC,
409            None,
410        ),
411    })
412}
gistools/space/propagation/sgp4.rs

gistools/space/propagation/
sgp4.rs
