gistools/space/propagation/
sgp4init.rs

1use crate::space::{
2    EARTH_RADIUS_KM, Method, Satellite,
3    propagation::{
4        DpperOptions, DsInitOptions, DscomOptions, DscomOutput, InitlOptions, InitlOutput, dpper,
5        dscom, dsinit, initl,
6    },
7    util::constants::{J2, J3_J2, J4, X2_3},
8};
9use core::f64::consts::PI;
10use libm::{cos, fabs, pow, sin};
11
12/// procedure sgp4init
13///
14/// this procedure initializes variables for sgp4.
15///
16/// author        david vallado                  719-573-2600   28 jun 2005
17///
18/// references
19/// hoots, roehrich, norad spacetrack report #3 1980
20/// hoots, norad spacetrack report #6 1986
21/// hoots, schumacher and glover 2004
22/// vallado, crawford, hujsak, kelso  2006
23///
24/// ## Parameters
25/// - `sat`: Satellite object
26pub fn sgp4init(sat: &mut Satellite) {
27    let epoch = sat.jdsatepoch - 2433281.5;
28
29    let cc1sq: f64;
30    let cc2: f64;
31    let mut cc3: f64;
32    let coef: f64;
33    let coef1: f64;
34    let cosio4: f64;
35    let eeta: f64;
36    let etasq: f64;
37    let argpm: f64;
38    let nodem: f64;
39    let inclm: f64;
40    let mm: f64;
41    let perige: f64;
42    let pinvsq: f64;
43    let psisq: f64;
44    let mut qzms24: f64;
45    let mut sfour: f64;
46    let tc: f64;
47    let temp: f64;
48    let temp1: f64;
49    let temp2: f64;
50    let temp3: f64;
51    let tsi: f64;
52    let xpidot: f64;
53    let xhdot1: f64;
54
55    /* ------------------------ initialization --------------------- */
56    // sgp4fix divisor for divide by zero check on inclination
57    // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
58    // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
59    let temp4 = 1.5e-12;
60
61    // sgp4fix - note the following variables are also passed directly via sat.
62    // it is possible to streamline the sgp4init call by deleting the "x"
63    // variables, but the user would need to set the sat.* values first. we
64    // include the additional assignments in case twoline2rv is not used.
65
66    // ------------------------ earth constants -----------------------
67    // sgp4fix identify constants and allow alternate values
68
69    let ss = 78.0 / EARTH_RADIUS_KM + 1.0;
70    // sgp4fix use multiply for speed instead of pow
71    let qzms2ttemp = (120.0 - 78.0) / EARTH_RADIUS_KM;
72    let qzms2t = qzms2ttemp * qzms2ttemp * qzms2ttemp * qzms2ttemp;
73
74    sat.init = true;
75
76    let initl_options = InitlOptions {
77        // satn,
78        ecco: sat.eccentricity,
79
80        epoch,
81        inclo: sat.inclination,
82        no: sat.motion,
83
84        opsmode: sat.opsmode,
85    };
86
87    let initl_result = initl(initl_options);
88    let InitlOutput { ao, con42, cosio, cosio2, eccsq, omeosq, posq, rp, rteosq, sinio, .. } =
89        initl_result;
90    sat.motion = initl_result.no;
91    sat.con41 = initl_result.con41;
92    sat.gsto = initl_result.gsto;
93    // const a = (sat.motion * tumin) ** (-2.0 / 3.0);
94    // const alta = a * (1.0 + sat.eccentricity) - 1.0;
95    // const altp = a * (1.0 - sat.eccentricity) - 1.0;
96
97    // sgp4fix remove this check as it is unnecessary
98    // the mrt check in sgp4 handles decaying satellite cases even if the starting
99    // condition is below the surface of te earth
100    // if (rp < 1.0)
101    // {
102    //   printf("// *** satn%d epoch elts sub-orbital ***\n", satn);
103    //   sat.error = 5;
104    // }
105
106    if omeosq >= 0.0 || sat.motion >= 0.0 {
107        sat.isimp = 0.;
108        if rp < 220.0 / EARTH_RADIUS_KM + 1.0 {
109            sat.isimp = 1.;
110        }
111        sfour = ss;
112        qzms24 = qzms2t;
113        perige = (rp - 1.0) * EARTH_RADIUS_KM;
114
115        // - for perigees below 156 km, s and qoms2t are altered -
116        if perige < 156.0 {
117            sfour = perige - 78.0;
118            if perige < 98.0 {
119                sfour = 20.0;
120            }
121
122            // sgp4fix use multiply for speed instead of pow
123            let qzms24temp = (120.0 - sfour) / EARTH_RADIUS_KM;
124            qzms24 = qzms24temp * qzms24temp * qzms24temp * qzms24temp;
125            sfour = sfour / EARTH_RADIUS_KM + 1.0;
126        }
127        pinvsq = 1.0 / posq;
128
129        tsi = 1.0 / (ao - sfour);
130        sat.eta = ao * sat.eccentricity * tsi;
131        etasq = sat.eta * sat.eta;
132        eeta = sat.eccentricity * sat.eta;
133        psisq = fabs(1.0 - etasq);
134        coef = pow(qzms24 * tsi, 4.0);
135        coef1 = coef / pow(psisq, 1.5);
136        cc2 = coef1
137            * sat.motion
138            * (ao * (1.0 + 1.5 * etasq + eeta * (4.0 + etasq))
139                + ((0.375 * J2 * tsi) / psisq) * sat.con41 * (8.0 + 3.0 * etasq * (8.0 + etasq)));
140        sat.cc1 = sat.drag * cc2;
141        cc3 = 0.0;
142        if sat.eccentricity > 1.0e-4 {
143            cc3 = (-2.0 * coef * tsi * J3_J2 * sat.motion * sinio) / sat.eccentricity;
144        }
145        sat.x1mth2 = 1.0 - cosio2;
146        sat.cc4 = 2.0
147            * sat.motion
148            * coef1
149            * ao
150            * omeosq
151            * (sat.eta * (2.0 + 0.5 * etasq) + sat.eccentricity * (0.5 + 2.0 * etasq)
152                - ((J2 * tsi) / (ao * psisq))
153                    * (-3.0 * sat.con41 * (1.0 - 2.0 * eeta + etasq * (1.5 - 0.5 * eeta))
154                        + 0.75
155                            * sat.x1mth2
156                            * (2.0 * etasq - eeta * (1.0 + etasq))
157                            * cos(2.0 * sat.perigee)));
158        sat.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * (etasq + eeta) + eeta * etasq);
159        cosio4 = cosio2 * cosio2;
160        temp1 = 1.5 * J2 * pinvsq * sat.motion;
161        temp2 = 0.5 * temp1 * J2 * pinvsq;
162        temp3 = -0.46875 * J4 * pinvsq * pinvsq * sat.motion;
163        sat.mdot = sat.motion
164            + 0.5 * temp1 * rteosq * sat.con41
165            + 0.0625 * temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4);
166        sat.argpdot = -0.5 * temp1 * con42
167            + 0.0625 * temp2 * (7.0 - 114.0 * cosio2 + 395.0 * cosio4)
168            + temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4);
169        xhdot1 = -temp1 * cosio;
170        sat.nodedot = xhdot1
171            + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio;
172        xpidot = sat.argpdot + sat.nodedot;
173        sat.omgcof = sat.drag * cc3 * cos(sat.perigee);
174        sat.xmcof = 0.0;
175        if sat.eccentricity > 1.0e-4 {
176            sat.xmcof = (-X2_3 * coef * sat.drag) / eeta;
177        }
178        sat.nodecf = 3.5 * omeosq * xhdot1 * sat.cc1;
179        sat.t2cof = 1.5 * sat.cc1;
180
181        // sgp4fix for divide by zero with xinco = 180 deg
182        if fabs(cosio + 1.0) > 1.5e-12 {
183            sat.xlcof = (-0.25 * J3_J2 * sinio * (3.0 + 5.0 * cosio)) / (1.0 + cosio);
184        } else {
185            sat.xlcof = (-0.25 * J3_J2 * sinio * (3.0 + 5.0 * cosio)) / temp4;
186        }
187        sat.aycof = -0.5 * J3_J2 * sinio;
188
189        // sgp4fix use multiply for speed instead of pow
190        let delmotemp = 1.0 + sat.eta * cos(sat.anomaly);
191        sat.delmo = delmotemp * delmotemp * delmotemp;
192        sat.sinmao = sin(sat.anomaly);
193        sat.x7thm1 = 7.0 * cosio2 - 1.0;
194
195        // --------------- deep space initialization -------------
196        if (2. * PI) / sat.motion >= 225.0 {
197            sat.method = Method::D;
198            sat.isimp = 1.;
199            tc = 0.0;
200            inclm = sat.inclination;
201
202            let dscom_options = DscomOptions {
203                epoch,
204                ep: sat.eccentricity,
205                argpp: sat.perigee,
206                tc,
207                inclp: sat.inclination,
208                nodep: sat.ascension,
209                np: sat.motion,
210            };
211
212            let dscom_result = dscom(dscom_options);
213
214            sat.e3 = dscom_result.e3;
215            sat.ee2 = dscom_result.ee2;
216
217            sat.peo = dscom_result.peo;
218            sat.pgho = dscom_result.pgho;
219            sat.pho = dscom_result.pho;
220
221            sat.pinco = dscom_result.pinco;
222            sat.plo = dscom_result.plo;
223            sat.se2 = dscom_result.se2;
224            sat.se3 = dscom_result.se3;
225
226            sat.sgh2 = dscom_result.sgh2;
227            sat.sgh3 = dscom_result.sgh3;
228            sat.sgh4 = dscom_result.sgh4;
229            sat.sh2 = dscom_result.sh2;
230            sat.sh3 = dscom_result.sh3;
231
232            sat.si2 = dscom_result.si2;
233            sat.si3 = dscom_result.si3;
234            sat.sl2 = dscom_result.sl2;
235            sat.sl3 = dscom_result.sl3;
236            sat.sl4 = dscom_result.sl4;
237
238            let DscomOutput {
239                sinim,
240                cosim,
241                em,
242                emsq,
243                s1,
244                s2,
245                s3,
246                s4,
247                s5,
248                ss1,
249                ss2,
250                ss3,
251                ss4,
252                ss5,
253                sz1,
254                sz3,
255                sz11,
256                sz13,
257                sz21,
258                sz23,
259                sz31,
260                sz33,
261                nm,
262                z1,
263                z3,
264                z11,
265                z13,
266                z21,
267                z23,
268                z31,
269                z33,
270                ..
271            } = dscom_result;
272
273            sat.xgh2 = dscom_result.xgh2;
274            sat.xgh3 = dscom_result.xgh3;
275            sat.xgh4 = dscom_result.xgh4;
276            sat.xh2 = dscom_result.xh2;
277            sat.xh3 = dscom_result.xh3;
278            sat.xi2 = dscom_result.xi2;
279            sat.xi3 = dscom_result.xi3;
280            sat.xl2 = dscom_result.xl2;
281            sat.xl3 = dscom_result.xl3;
282            sat.xl4 = dscom_result.xl4;
283            sat.zmol = dscom_result.zmol;
284            sat.zmos = dscom_result.zmos;
285
286            let dpper_options = DpperOptions {
287                // inclo: inclm,
288                init: sat.init,
289                ep: sat.eccentricity,
290                inclp: sat.inclination,
291                nodep: sat.ascension,
292                argpp: sat.perigee,
293                mp: sat.anomaly,
294                // opsmode: sat.opsmode,
295            };
296
297            let dpper_result = dpper(sat, dpper_options, 0.);
298
299            sat.eccentricity = dpper_result.ep;
300            sat.inclination = dpper_result.inclp;
301            sat.ascension = dpper_result.nodep;
302            sat.perigee = dpper_result.argpp;
303            sat.anomaly = dpper_result.mp;
304
305            argpm = 0.0;
306            nodem = 0.0;
307            mm = 0.0;
308
309            let dsinit_options = DsInitOptions {
310                cosim,
311                emsq,
312                argpo: sat.perigee,
313                s1,
314                s2,
315                s3,
316                s4,
317                s5,
318                sinim,
319                ss1,
320                ss2,
321                ss3,
322                ss4,
323                ss5,
324                sz1,
325                sz3,
326                sz11,
327                sz13,
328                sz21,
329                sz23,
330                sz31,
331                sz33,
332                tc,
333                gsto: sat.gsto,
334                mo: sat.anomaly,
335                mdot: sat.mdot,
336                no: sat.motion,
337                nodeo: sat.ascension,
338                nodedot: sat.nodedot,
339                xpidot,
340                z1,
341                z3,
342                z11,
343                z13,
344                z21,
345                z23,
346                z31,
347                z33,
348                ecco: sat.eccentricity,
349                eccsq,
350                em,
351                argpm,
352                inclm,
353                mm,
354                nm,
355                nodem,
356                irez: sat.irez,
357                atime: sat.atime,
358                d2201: sat.d2201,
359                d2211: sat.d2211,
360                d3210: sat.d3210,
361                d3222: sat.d3222,
362                d4410: sat.d4410,
363                d4422: sat.d4422,
364                d5220: sat.d5220,
365                d5232: sat.d5232,
366                d5421: sat.d5421,
367                d5433: sat.d5433,
368                dedt: sat.dedt,
369                didt: sat.didt,
370                dmdt: sat.dmdt,
371                dnodt: sat.dnodt,
372                domdt: sat.domdt,
373                del1: sat.del1,
374                del2: sat.del2,
375                del3: sat.del3,
376                xfact: sat.xfact,
377                xlamo: sat.xlamo,
378                xli: sat.xli,
379                xni: sat.xni,
380            };
381
382            let dsinit_result = dsinit(dsinit_options, 0.);
383
384            sat.irez = dsinit_result.irez;
385            sat.atime = dsinit_result.atime;
386            sat.d2201 = dsinit_result.d2201;
387            sat.d2211 = dsinit_result.d2211;
388
389            sat.d3210 = dsinit_result.d3210;
390            sat.d3222 = dsinit_result.d3222;
391            sat.d4410 = dsinit_result.d4410;
392            sat.d4422 = dsinit_result.d4422;
393            sat.d5220 = dsinit_result.d5220;
394
395            sat.d5232 = dsinit_result.d5232;
396            sat.d5421 = dsinit_result.d5421;
397            sat.d5433 = dsinit_result.d5433;
398            sat.dedt = dsinit_result.dedt;
399            sat.didt = dsinit_result.didt;
400
401            sat.dmdt = dsinit_result.dmdt;
402            sat.dnodt = dsinit_result.dnodt;
403            sat.domdt = dsinit_result.domdt;
404            sat.del1 = dsinit_result.del1;
405
406            sat.del2 = dsinit_result.del2;
407            sat.del3 = dsinit_result.del3;
408            sat.xfact = dsinit_result.xfact;
409            sat.xlamo = dsinit_result.xlamo;
410            sat.xli = dsinit_result.xli;
411
412            sat.xni = dsinit_result.xni;
413        }
414
415        // ----------- set variables if not deep space -----------
416        if sat.isimp != 1. {
417            cc1sq = sat.cc1 * sat.cc1;
418            sat.d2 = 4.0 * ao * tsi * cc1sq;
419            temp = (sat.d2 * tsi * sat.cc1) / 3.0;
420            sat.d3 = (17.0 * ao + sfour) * temp;
421            sat.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * sat.cc1;
422            sat.t3cof = sat.d2 + 2.0 * cc1sq;
423            sat.t4cof = 0.25 * (3.0 * sat.d3 + sat.cc1 * (12.0 * sat.d2 + 10.0 * cc1sq));
424            sat.t5cof = 0.2
425                * (3.0 * sat.d4
426                    + 12.0 * sat.cc1 * sat.d3
427                    + 6.0 * sat.d2 * sat.d2
428                    + 15.0 * cc1sq * (2.0 * sat.d2 + cc1sq));
429        }
430    }
431}
gistools/space/propagation/sgp4init.rs

gistools/space/propagation/
sgp4init.rs
