1 /* ----------------------------------------------------------------
2 *
3 * sgp4unit.cpp
4 *
5 * this file contains the sgp4 procedures for analytical propagation
6 * of a satellite. the code was originally released in the 1980 and 1986
7 * spacetrack papers. a detailed discussion of the theory and history
8 * may be found in the 2006 aiaa paper by vallado, crawford, hujsak,
9 * and kelso.
10 *
11 * companion code for
12 * fundamentals of astrodynamics and applications
13 * 2007
14 * by david vallado
15 *
16 * (w) 719-573-2600, email dvallado@agi.com
17 *
18 * current :
19 * 3 Nov 08 david vallado
20 * put returns in for error codes
21 * changes :
22 * 29 sep 08 david vallado
23 * fix atime for faster operation in dspace
24 * add operationmode for afspc (a) or improved (i)
25 * performance mode
26 * 16 jun 08 david vallado
27 * update small eccentricity check
28 * 16 nov 07 david vallado
29 * misc fixes for better compliance
30 * 20 apr 07 david vallado
31 * misc fixes for constants
32 * 11 aug 06 david vallado
33 * chg lyddane choice back to strn3, constants, misc doc
34 * 15 dec 05 david vallado
35 * misc fixes
36 * 26 jul 05 david vallado
37 * fixes for paper
38 * note that each fix is preceded by a
39 * comment with "sgp4fix" and an explanation of
40 * what was changed
41 * 10 aug 04 david vallado
42 * 2nd printing baseline working
43 * 14 may 01 david vallado
44 * 2nd edition baseline
45 * 80 norad
46 * original baseline
47 * ---------------------------------------------------------------- */
48
49 #include "sgp4unit.h"
50 #include <cmath>
51
52 //FILE *dbgfile;
53
54
55 /* ----------- local functions - only ever used internally by sgp4 ---------- */
56 static void dpper(double e3, double ee2, double peo, double pgho, double pho,
57 double pinco, double plo, double se2, double se3, double sgh2,
58 double sgh3, double sgh4, double sh2, double sh3, double si2,
59 double si3, double sl2, double sl3, double sl4, double t,
60 double xgh2, double xgh3, double xgh4, double xh2, double xh3,
61 double xi2, double xi3, double xl2, double xl3, double xl4,
62 double zmol, double zmos, double inclo,
63 char init,
64 double& ep, double& inclp, double& nodep, double& argpp, double& mp,
65 char opsmode);
66
67 static void dscom(double epoch, double ep, double argpp, double tc, double inclp,
68 double nodep, double np,
69 double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm,
70 double& cosomm,double& day, double& e3, double& ee2, double& em,
71 double& emsq, double& gam, double& peo, double& pgho, double& pho,
72 double& pinco, double& plo, double& rtemsq, double& se2, double& se3,
73 double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3,
74 double& si2, double& si3, double& sl2, double& sl3, double& sl4,
75 double& s1, double& s2, double& s3, double& s4, double& s5,
76 double& s6, double& s7, double& ss1, double& ss2, double& ss3,
77 double& ss4, double& ss5, double& ss6, double& ss7, double& sz1,
78 double& sz2, double& sz3, double& sz11, double& sz12, double& sz13,
79 double& sz21, double& sz22, double& sz23, double& sz31, double& sz32,
80 double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2,
81 double& xh3, double& xi2, double& xi3, double& xl2, double& xl3,
82 double& xl4, double& nm, double& z1, double& z2, double& z3,
83 double& z11, double& z12, double& z13, double& z21, double& z22,
84 double& z23, double& z31, double& z32, double& z33, double& zmol,
85 double& zmos);
86
87 static void dsinit(gravconsttype whichconst,
88 double cosim, double emsq, double argpo, double s1, double s2,
89 double s3, double s4, double s5, double sinim, double ss1,
90 double ss2, double ss3, double ss4, double ss5, double sz1,
91 double sz3, double sz11, double sz13, double sz21, double sz23,
92 double sz31, double sz33, double t, double tc, double gsto,
93 double mo, double mdot, double no, double nodeo, double nodedot,
94 double xpidot, double z1, double z3, double z11, double z13,
95 double z21, double z23, double z31, double z33, double ecco,
96 double eccsq, double& em, double& argpm, double& inclm, double& mm,
97 double& nm, double& nodem,
98 int& irez,
99 double& atime, double& d2201, double& d2211, double& d3210, double& d3222,
100 double& d4410, double& d4422, double& d5220, double& d5232, double& d5421,
101 double& d5433, double& dedt, double& didt, double& dmdt, double& dndt,
102 double& dnodt, double& domdt, double& del1, double& del2, double& del3,
103 double& xfact, double& xlamo, double& xli, double& xni);
104
105 static void dspace(int irez,
106 double d2201, double d2211, double d3210, double d3222, double d4410,
107 double d4422, double d5220, double d5232, double d5421, double d5433,
108 double dedt, double del1, double del2, double del3, double didt,
109 double dmdt, double dnodt, double domdt, double argpo, double argpdot,
110 double t, double tc, double gsto, double xfact, double xlamo,
111 double no,
112 double& atime, double& em, double& argpm, double& inclm, double& xli,
113 double& mm, double& xni, double& nodem, double& dndt, double& nm);
114
115 static void initl(int satn, gravconsttype whichconst,
116 double ecco, double epoch, double inclo, double& no,
117 char& method,
118 double& ainv, double& ao, double& con41, double& con42, double& cosio,
119 double& cosio2,double& eccsq, double& omeosq, double& posq,
120 double& rp, double& rteosq,double& sinio , double& gsto, char opsmode);
121
122 /* -----------------------------------------------------------------------------
123 *
124 * procedure dpper
125 *
126 * this procedure provides deep space long period periodic contributions
127 * to the mean elements. by design, these periodics are zero at epoch.
128 * this used to be dscom which included initialization, but it's really a
129 * recurring function.
130 *
131 * author : david vallado 719-573-2600 28 jun 2005
132 *
133 * inputs :
134 * e3 -
135 * ee2 -
136 * peo -
137 * pgho -
138 * pho -
139 * pinco -
140 * plo -
141 * se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 -
142 * t -
143 * xh2, xh3, xi2, xi3, xl2, xl3, xl4 -
144 * zmol -
145 * zmos -
146 * ep - eccentricity 0.0 - 1.0
147 * inclo - inclination - needed for lyddane modification
148 * nodep - right ascension of ascending node
149 * argpp - argument of perigee
150 * mp - mean anomaly
151 *
152 * outputs :
153 * ep - eccentricity 0.0 - 1.0
154 * inclp - inclination
155 * nodep - right ascension of ascending node
156 * argpp - argument of perigee
157 * mp - mean anomaly
158 *
159 * locals :
160 * alfdp -
161 * betdp -
162 * cosip , sinip , cosop , sinop ,
163 * dalf -
164 * dbet -
165 * dls -
166 * f2, f3 -
167 * pe -
168 * pgh -
169 * ph -
170 * pinc -
171 * pl -
172 * sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis ,
173 * sll , sls
174 * xls -
175 * xnoh -
176 * zf -
177 * zm -
178 *
179 * coupling :
180 * none.
181 *
182 * references :
183 * hoots, roehrich, norad spacetrack report #3 1980
184 * hoots, norad spacetrack report #6 1986
185 * hoots, schumacher and glover 2004
186 * vallado, crawford, hujsak, kelso 2006
187 ----------------------------------------------------------------------------*/
188
dpper(double e3,double ee2,double peo,double pgho,double pho,double pinco,double plo,double se2,double se3,double sgh2,double sgh3,double sgh4,double sh2,double sh3,double si2,double si3,double sl2,double sl3,double sl4,double t,double xgh2,double xgh3,double xgh4,double xh2,double xh3,double xi2,double xi3,double xl2,double xl3,double xl4,double zmol,double zmos,double inclo,char init,double & ep,double & inclp,double & nodep,double & argpp,double & mp,char opsmode)189 static void dpper(double e3, double ee2, double peo, double pgho, double pho,
190 double pinco, double plo, double se2, double se3, double sgh2,
191 double sgh3, double sgh4, double sh2, double sh3, double si2,
192 double si3, double sl2, double sl3, double sl4, double t,
193 double xgh2, double xgh3, double xgh4, double xh2, double xh3,
194 double xi2, double xi3, double xl2, double xl3, double xl4,
195 double zmol, double zmos, double inclo,
196 char init,
197 double& ep, double& inclp, double& nodep, double& argpp, double& mp,
198 char opsmode)
199 {
200 /* --------------------- local variables ------------------------ */
201 const double twopi = 2.0 * M_PI;
202 double alfdp, betdp, cosip, cosop, dalf, dbet, dls,
203 f2, f3, pe, pgh, ph, pinc, pl ,
204 sel, ses, sghl, sghs, shll, shs, sil,
205 sinip, sinop, sinzf, sis, sll, sls, xls,
206 xnoh, zf, zm, zel, zes, znl, zns;
207
208 /* kill warning */
209 inclo = 0.;
210 static_cast<void>(inclo);
211
212 /* ---------------------- constants ----------------------------- */
213 zns = 1.19459e-5;
214 zes = 0.01675;
215 znl = 1.5835218e-4;
216 zel = 0.05490;
217
218 /* --------------- calculate time varying periodics ----------- */
219 zm = zmos + zns * t;
220 // be sure that the initial call has time set to zero
221 if (init == 'y')
222 zm = zmos;
223 zf = zm + 2.0 * zes * std::sin(zm);
224 sinzf = std::sin(zf);
225 f2 = 0.5 * sinzf * sinzf - 0.25;
226 f3 = -0.5 * sinzf * std::cos(zf);
227 ses = se2* f2 + se3 * f3;
228 sis = si2 * f2 + si3 * f3;
229 sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf;
230 sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf;
231 shs = sh2 * f2 + sh3 * f3;
232 zm = zmol + znl * t;
233 if (init == 'y')
234 zm = zmol;
235 zf = zm + 2.0 * zel * std::sin(zm);
236 sinzf = std::sin(zf);
237 f2 = 0.5 * sinzf * sinzf - 0.25;
238 f3 = -0.5 * sinzf * std::cos(zf);
239 sel = ee2 * f2 + e3 * f3;
240 sil = xi2 * f2 + xi3 * f3;
241 sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf;
242 sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf;
243 shll = xh2 * f2 + xh3 * f3;
244 pe = ses + sel;
245 pinc = sis + sil;
246 pl = sls + sll;
247 pgh = sghs + sghl;
248 ph = shs + shll;
249
250 if (init == 'n')
251 {
252 pe = pe - peo;
253 pinc = pinc - pinco;
254 pl = pl - plo;
255 pgh = pgh - pgho;
256 ph = ph - pho;
257 inclp = inclp + pinc;
258 ep = ep + pe;
259 sinip = std::sin(inclp);
260 cosip = std::cos(inclp);
261
262 /* ----------------- apply periodics directly ------------ */
263 // sgp4fix for lyddane choice
264 // strn3 used original inclination - this is technically feasible
265 // gsfc used perturbed inclination - also technically feasible
266 // probably best to readjust the 0.2 limit value and limit discontinuity
267 // 0.2 rad = 11.45916 deg
268 // use next line for original strn3 approach and original inclination
269 // if (inclo >= 0.2)
270 // use next line for gsfc version and perturbed inclination
271 if (inclp >= 0.2)
272 {
273 ph = ph / sinip;
274 pgh = pgh - cosip * ph;
275 argpp = argpp + pgh;
276 nodep = nodep + ph;
277 mp = mp + pl;
278 }
279 else
280 {
281 /* ---- apply periodics with lyddane modification ---- */
282 sinop = std::sin(nodep);
283 cosop = std::cos(nodep);
284 alfdp = sinip * sinop;
285 betdp = sinip * cosop;
286 dalf = ph * cosop + pinc * cosip * sinop;
287 dbet = -ph * sinop + pinc * cosip * cosop;
288 alfdp = alfdp + dalf;
289 betdp = betdp + dbet;
290 nodep = std::fmod(nodep, twopi);
291 // sgp4fix for afspc written intrinsic functions
292 // nodep used without a trigonometric function ahead
293 if ((nodep < 0.0) && (opsmode == 'a'))
294 nodep = nodep + twopi;
295 xls = mp + argpp + cosip * nodep;
296 dls = pl + pgh - pinc * nodep * sinip;
297 xls = xls + dls;
298 xnoh = nodep;
299 nodep = std::atan2(alfdp, betdp);
300 // sgp4fix for afspc written intrinsic functions
301 // nodep used without a trigonometric function ahead
302 if ((nodep < 0.0) && (opsmode == 'a'))
303 nodep = nodep + twopi;
304 if (std::fabs(xnoh - nodep) > M_PI)
305 {
306 if (nodep < xnoh)
307 nodep = nodep + twopi;
308 else
309 nodep = nodep - twopi;
310 }
311 mp = mp + pl;
312 argpp = xls - mp - cosip * nodep;
313 }
314 } // if init == 'n'
315 } // end dpper
316
317 /*-----------------------------------------------------------------------------
318 *
319 * procedure dscom
320 *
321 * this procedure provides deep space common items used by both the secular
322 * and periodics subroutines. input is provided as shown. this routine
323 * used to be called dpper, but the functions inside weren't well organized.
324 *
325 * author : david vallado 719-573-2600 28 jun 2005
326 *
327 * inputs :
328 * epoch -
329 * ep - eccentricity
330 * argpp - argument of perigee
331 * tc -
332 * inclp - inclination
333 * nodep - right ascension of ascending node
334 * np - mean motion
335 *
336 * outputs :
337 * sinim , cosim , sinomm , cosomm , snodm , cnodm
338 * day -
339 * e3 -
340 * ee2 -
341 * em - eccentricity
342 * emsq - eccentricity squared
343 * gam -
344 * peo -
345 * pgho -
346 * pho -
347 * pinco -
348 * plo -
349 * rtemsq -
350 * se2, se3 -
351 * sgh2, sgh3, sgh4 -
352 * sh2, sh3, si2, si3, sl2, sl3, sl4 -
353 * s1, s2, s3, s4, s5, s6, s7 -
354 * ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 -
355 * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 -
356 * xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 -
357 * nm - mean motion
358 * z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 -
359 * zmol -
360 * zmos -
361 *
362 * locals :
363 * a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 -
364 * betasq -
365 * cc -
366 * ctem, stem -
367 * x1, x2, x3, x4, x5, x6, x7, x8 -
368 * xnodce -
369 * xnoi -
370 * zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl ,
371 * zcosi , zsini , zcosil , zsinil ,
372 * zx -
373 * zy -
374 *
375 * coupling :
376 * none.
377 *
378 * references :
379 * hoots, roehrich, norad spacetrack report #3 1980
380 * hoots, norad spacetrack report #6 1986
381 * hoots, schumacher and glover 2004
382 * vallado, crawford, hujsak, kelso 2006
383 ----------------------------------------------------------------------------*/
384
dscom(double epoch,double ep,double argpp,double tc,double inclp,double nodep,double np,double & snodm,double & cnodm,double & sinim,double & cosim,double & sinomm,double & cosomm,double & day,double & e3,double & ee2,double & em,double & emsq,double & gam,double & peo,double & pgho,double & pho,double & pinco,double & plo,double & rtemsq,double & se2,double & se3,double & sgh2,double & sgh3,double & sgh4,double & sh2,double & sh3,double & si2,double & si3,double & sl2,double & sl3,double & sl4,double & s1,double & s2,double & s3,double & s4,double & s5,double & s6,double & s7,double & ss1,double & ss2,double & ss3,double & ss4,double & ss5,double & ss6,double & ss7,double & sz1,double & sz2,double & sz3,double & sz11,double & sz12,double & sz13,double & sz21,double & sz22,double & sz23,double & sz31,double & sz32,double & sz33,double & xgh2,double & xgh3,double & xgh4,double & xh2,double & xh3,double & xi2,double & xi3,double & xl2,double & xl3,double & xl4,double & nm,double & z1,double & z2,double & z3,double & z11,double & z12,double & z13,double & z21,double & z22,double & z23,double & z31,double & z32,double & z33,double & zmol,double & zmos)385 static void dscom(double epoch, double ep, double argpp, double tc, double inclp,
386 double nodep, double np,
387 double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm,
388 double& cosomm,double& day, double& e3, double& ee2, double& em,
389 double& emsq, double& gam, double& peo, double& pgho, double& pho,
390 double& pinco, double& plo, double& rtemsq, double& se2, double& se3,
391 double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3,
392 double& si2, double& si3, double& sl2, double& sl3, double& sl4,
393 double& s1, double& s2, double& s3, double& s4, double& s5,
394 double& s6, double& s7, double& ss1, double& ss2, double& ss3,
395 double& ss4, double& ss5, double& ss6, double& ss7, double& sz1,
396 double& sz2, double& sz3, double& sz11, double& sz12, double& sz13,
397 double& sz21, double& sz22, double& sz23, double& sz31, double& sz32,
398 double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2,
399 double& xh3, double& xi2, double& xi3, double& xl2, double& xl3,
400 double& xl4, double& nm, double& z1, double& z2, double& z3,
401 double& z11, double& z12, double& z13, double& z21, double& z22,
402 double& z23, double& z31, double& z32, double& z33, double& zmol,
403 double& zmos)
404 {
405 /* -------------------------- constants ------------------------- */
406 const double zes = 0.01675;
407 const double zel = 0.05490;
408 const double c1ss = 2.9864797e-6;
409 const double c1l = 4.7968065e-7;
410 const double zsinis = 0.39785416;
411 const double zcosis = 0.91744867;
412 const double zcosgs = 0.1945905;
413 const double zsings = -0.98088458;
414 const double twopi = 2.0 * M_PI;
415
416 /* --------------------- local variables ------------------------ */
417 int lsflg;
418 double a1 , a2 , a3 , a4 , a5 , a6 , a7 ,
419 a8 , a9 , a10 , betasq, cc , ctem , stem ,
420 x1 , x2 , x3 , x4 , x5 , x6 , x7 ,
421 x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl,
422 zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini ,
423 zsinil, zx , zy;
424
425 nm = np;
426 em = ep;
427 snodm = std::sin(nodep);
428 cnodm = std::cos(nodep);
429 sinomm = std::sin(argpp);
430 cosomm = std::cos(argpp);
431 sinim = std::sin(inclp);
432 cosim = std::cos(inclp);
433 emsq = em * em;
434 betasq = 1.0 - emsq;
435 rtemsq = std::sqrt(betasq);
436
437 /* ----------------- initialize lunar solar terms --------------- */
438 peo = 0.0;
439 pinco = 0.0;
440 plo = 0.0;
441 pgho = 0.0;
442 pho = 0.0;
443 day = epoch + 18261.5 + tc / 1440.0;
444 xnodce = std::fmod(4.5236020 - 9.2422029e-4 * day, twopi);
445 stem = std::sin(xnodce);
446 ctem = std::cos(xnodce);
447 zcosil = 0.91375164 - 0.03568096 * ctem;
448 zsinil = std::sqrt(1.0 - zcosil * zcosil);
449 zsinhl = 0.089683511 * stem / zsinil;
450 zcoshl = std::sqrt(1.0 - zsinhl * zsinhl);
451 gam = 5.8351514 + 0.0019443680 * day;
452 zx = 0.39785416 * stem / zsinil;
453 zy = zcoshl * ctem + 0.91744867 * zsinhl * stem;
454 zx = std::atan2(zx, zy);
455 zx = gam + zx - xnodce;
456 zcosgl = std::cos(zx);
457 zsingl = std::sin(zx);
458
459 /* ------------------------- do solar terms --------------------- */
460 zcosg = zcosgs;
461 zsing = zsings;
462 zcosi = zcosis;
463 zsini = zsinis;
464 zcosh = cnodm;
465 zsinh = snodm;
466 cc = c1ss;
467 xnoi = 1.0 / nm;
468
469 for (lsflg = 1; lsflg <= 2; lsflg++)
470 {
471 a1 = zcosg * zcosh + zsing * zcosi * zsinh;
472 a3 = -zsing * zcosh + zcosg * zcosi * zsinh;
473 a7 = -zcosg * zsinh + zsing * zcosi * zcosh;
474 a8 = zsing * zsini;
475 a9 = zsing * zsinh + zcosg * zcosi * zcosh;
476 a10 = zcosg * zsini;
477 a2 = cosim * a7 + sinim * a8;
478 a4 = cosim * a9 + sinim * a10;
479 a5 = -sinim * a7 + cosim * a8;
480 a6 = -sinim * a9 + cosim * a10;
481
482 x1 = a1 * cosomm + a2 * sinomm;
483 x2 = a3 * cosomm + a4 * sinomm;
484 x3 = -a1 * sinomm + a2 * cosomm;
485 x4 = -a3 * sinomm + a4 * cosomm;
486 x5 = a5 * sinomm;
487 x6 = a6 * sinomm;
488 x7 = a5 * cosomm;
489 x8 = a6 * cosomm;
490
491 z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3;
492 z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4;
493 z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4;
494 z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq;
495 z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq;
496 z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq;
497 z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5);
498 z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5));
499 z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6);
500 z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7);
501 z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8));
502 z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8);
503 z1 = z1 + z1 + betasq * z31;
504 z2 = z2 + z2 + betasq * z32;
505 z3 = z3 + z3 + betasq * z33;
506 s3 = cc * xnoi;
507 s2 = -0.5 * s3 / rtemsq;
508 s4 = s3 * rtemsq;
509 s1 = -15.0 * em * s4;
510 s5 = x1 * x3 + x2 * x4;
511 s6 = x2 * x3 + x1 * x4;
512 s7 = x2 * x4 - x1 * x3;
513
514 /* ----------------------- do lunar terms ------------------- */
515 if (lsflg == 1)
516 {
517 ss1 = s1;
518 ss2 = s2;
519 ss3 = s3;
520 ss4 = s4;
521 ss5 = s5;
522 ss6 = s6;
523 ss7 = s7;
524 sz1 = z1;
525 sz2 = z2;
526 sz3 = z3;
527 sz11 = z11;
528 sz12 = z12;
529 sz13 = z13;
530 sz21 = z21;
531 sz22 = z22;
532 sz23 = z23;
533 sz31 = z31;
534 sz32 = z32;
535 sz33 = z33;
536 zcosg = zcosgl;
537 zsing = zsingl;
538 zcosi = zcosil;
539 zsini = zsinil;
540 zcosh = zcoshl * cnodm + zsinhl * snodm;
541 zsinh = snodm * zcoshl - cnodm * zsinhl;
542 cc = c1l;
543 }
544 }
545
546 zmol = std::fmod(4.7199672 + 0.22997150 * day - gam, twopi);
547 zmos = std::fmod(6.2565837 + 0.017201977 * day, twopi);
548
549 /* ------------------------ do solar terms ---------------------- */
550 se2 = 2.0 * ss1 * ss6;
551 se3 = 2.0 * ss1 * ss7;
552 si2 = 2.0 * ss2 * sz12;
553 si3 = 2.0 * ss2 * (sz13 - sz11);
554 sl2 = -2.0 * ss3 * sz2;
555 sl3 = -2.0 * ss3 * (sz3 - sz1);
556 sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes;
557 sgh2 = 2.0 * ss4 * sz32;
558 sgh3 = 2.0 * ss4 * (sz33 - sz31);
559 sgh4 = -18.0 * ss4 * zes;
560 sh2 = -2.0 * ss2 * sz22;
561 sh3 = -2.0 * ss2 * (sz23 - sz21);
562
563 /* ------------------------ do lunar terms ---------------------- */
564 ee2 = 2.0 * s1 * s6;
565 e3 = 2.0 * s1 * s7;
566 xi2 = 2.0 * s2 * z12;
567 xi3 = 2.0 * s2 * (z13 - z11);
568 xl2 = -2.0 * s3 * z2;
569 xl3 = -2.0 * s3 * (z3 - z1);
570 xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel;
571 xgh2 = 2.0 * s4 * z32;
572 xgh3 = 2.0 * s4 * (z33 - z31);
573 xgh4 = -18.0 * s4 * zel;
574 xh2 = -2.0 * s2 * z22;
575 xh3 = -2.0 * s2 * (z23 - z21);
576 } // end dscom
577
578 /*-----------------------------------------------------------------------------
579 *
580 * procedure dsinit
581 *
582 * this procedure provides deep space contributions to mean motion dot due
583 * to geopotential resonance with half day and one day orbits.
584 *
585 * author : david vallado 719-573-2600 28 jun 2005
586 *
587 * inputs :
588 * cosim, sinim-
589 * emsq - eccentricity squared
590 * argpo - argument of perigee
591 * s1, s2, s3, s4, s5 -
592 * ss1, ss2, ss3, ss4, ss5 -
593 * sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 -
594 * t - time
595 * tc -
596 * gsto - greenwich sidereal time rad
597 * mo - mean anomaly
598 * mdot - mean anomaly dot (rate)
599 * no - mean motion
600 * nodeo - right ascension of ascending node
601 * nodedot - right ascension of ascending node dot (rate)
602 * xpidot -
603 * z1, z3, z11, z13, z21, z23, z31, z33 -
604 * eccm - eccentricity
605 * argpm - argument of perigee
606 * inclm - inclination
607 * mm - mean anomaly
608 * xn - mean motion
609 * nodem - right ascension of ascending node
610 *
611 * outputs :
612 * em - eccentricity
613 * argpm - argument of perigee
614 * inclm - inclination
615 * mm - mean anomaly
616 * nm - mean motion
617 * nodem - right ascension of ascending node
618 * irez - flag for resonance 0-none, 1-one day, 2-half day
619 * atime -
620 * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 -
621 * dedt -
622 * didt -
623 * dmdt -
624 * dndt -
625 * dnodt -
626 * domdt -
627 * del1, del2, del3 -
628 * ses , sghl , sghs , sgs , shl , shs , sis , sls
629 * theta -
630 * xfact -
631 * xlamo -
632 * xli -
633 * xni
634 *
635 * locals :
636 * ainv2 -
637 * aonv -
638 * cosisq -
639 * eoc -
640 * f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 -
641 * g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 -
642 * sini2 -
643 * temp -
644 * temp1 -
645 * theta -
646 * xno2 -
647 *
648 * coupling :
649 * getgravconst
650 *
651 * references :
652 * hoots, roehrich, norad spacetrack report #3 1980
653 * hoots, norad spacetrack report #6 1986
654 * hoots, schumacher and glover 2004
655 * vallado, crawford, hujsak, kelso 2006
656 ----------------------------------------------------------------------------*/
657
dsinit(gravconsttype whichconst,double cosim,double emsq,double argpo,double s1,double s2,double s3,double s4,double s5,double sinim,double ss1,double ss2,double ss3,double ss4,double ss5,double sz1,double sz3,double sz11,double sz13,double sz21,double sz23,double sz31,double sz33,double t,double tc,double gsto,double mo,double mdot,double no,double nodeo,double nodedot,double xpidot,double z1,double z3,double z11,double z13,double z21,double z23,double z31,double z33,double ecco,double eccsq,double & em,double & argpm,double & inclm,double & mm,double & nm,double & nodem,int & irez,double & atime,double & d2201,double & d2211,double & d3210,double & d3222,double & d4410,double & d4422,double & d5220,double & d5232,double & d5421,double & d5433,double & dedt,double & didt,double & dmdt,double & dndt,double & dnodt,double & domdt,double & del1,double & del2,double & del3,double & xfact,double & xlamo,double & xli,double & xni)658 static void dsinit(gravconsttype whichconst,
659 double cosim, double emsq, double argpo, double s1, double s2,
660 double s3, double s4, double s5, double sinim, double ss1,
661 double ss2, double ss3, double ss4, double ss5, double sz1,
662 double sz3, double sz11, double sz13, double sz21, double sz23,
663 double sz31, double sz33, double t, double tc, double gsto,
664 double mo, double mdot, double no, double nodeo, double nodedot,
665 double xpidot, double z1, double z3, double z11, double z13,
666 double z21, double z23, double z31, double z33, double ecco,
667 double eccsq, double& em, double& argpm, double& inclm, double& mm,
668 double& nm, double& nodem,
669 int& irez,
670 double& atime, double& d2201, double& d2211, double& d3210, double& d3222,
671 double& d4410, double& d4422, double& d5220, double& d5232, double& d5421,
672 double& d5433, double& dedt, double& didt, double& dmdt, double& dndt,
673 double& dnodt, double& domdt, double& del1, double& del2, double& del3,
674 double& xfact, double& xlamo, double& xli, double& xni)
675 {
676 /* --------------------- local variables ------------------------ */
677 const double twopi = 2.0 * M_PI;
678
679 double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 ,
680 f321 , f322 , f330 , f441 , f442 , f522 , f523 ,
681 f542 , f543 , g200 , g201 , g211 , g300 , g310 ,
682 g322 , g410 , g422 , g520 , g521 , g532 , g533 ,
683 ses , sgs , sghl , sghs , shs , shll , sis ,
684 sini2 , sls , temp , temp1 , theta , xno2 , q22 ,
685 q31 , q33 , root22, root44, root54, rptim , root32,
686 root52, x2o3 , xke=0 , znl , emo , zns , emsqo,
687 tumin, mu, radiusearthkm, j2, j3, j4, j3oj2;
688
689 q22 = 1.7891679e-6;
690 q31 = 2.1460748e-6;
691 q33 = 2.2123015e-7;
692 root22 = 1.7891679e-6;
693 root44 = 7.3636953e-9;
694 root54 = 2.1765803e-9;
695 rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
696 root32 = 3.7393792e-7;
697 root52 = 1.1428639e-7;
698 x2o3 = 2.0 / 3.0;
699 znl = 1.5835218e-4;
700 zns = 1.19459e-5;
701
702 // sgp4fix identify constants and allow alternate values
703 getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
704
705 /* -------------------- deep space initialization ------------ */
706 irez = 0;
707 if ((nm < 0.0052359877) && (nm > 0.0034906585))
708 irez = 1;
709 if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5))
710 irez = 2;
711
712 /* ------------------------ do solar terms ------------------- */
713 ses = ss1 * zns * ss5;
714 sis = ss2 * zns * (sz11 + sz13);
715 sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq);
716 sghs = ss4 * zns * (sz31 + sz33 - 6.0);
717 shs = -zns * ss2 * (sz21 + sz23);
718 // sgp4fix for 180 deg incl
719 if ((inclm < 5.2359877e-2) || (inclm > M_PI - 5.2359877e-2))
720 shs = 0.0;
721 if (sinim != 0.0)
722 shs = shs / sinim;
723 sgs = sghs - cosim * shs;
724
725 /* ------------------------- do lunar terms ------------------ */
726 dedt = ses + s1 * znl * s5;
727 didt = sis + s2 * znl * (z11 + z13);
728 dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq);
729 sghl = s4 * znl * (z31 + z33 - 6.0);
730 shll = -znl * s2 * (z21 + z23);
731 // sgp4fix for 180 deg incl
732 if ((inclm < 5.2359877e-2) || (inclm > M_PI - 5.2359877e-2))
733 shll = 0.0;
734 domdt = sgs + sghl;
735 dnodt = shs;
736 if (sinim != 0.0)
737 {
738 domdt = domdt - cosim / sinim * shll;
739 dnodt = dnodt + shll / sinim;
740 }
741
742 /* ----------- calculate deep space resonance effects -------- */
743 dndt = 0.0;
744 theta = std::fmod(gsto + tc * rptim, twopi);
745 em = em + dedt * t;
746 inclm = inclm + didt * t;
747 argpm = argpm + domdt * t;
748 nodem = nodem + dnodt * t;
749 mm = mm + dmdt * t;
750 // sgp4fix for negative inclinations
751 // the following if statement should be commented out
752 //if (inclm < 0.0)
753 // {
754 // inclm = -inclm;
755 // argpm = argpm - M_PI;
756 // nodem = nodem + M_PI;
757 // }
758
759 /* -------------- initialize the resonance terms ------------- */
760 if (irez != 0)
761 {
762 aonv = std::pow(nm / xke, x2o3);
763
764 /* ---------- geopotential resonance for 12 hour orbits ------ */
765 if (irez == 2)
766 {
767 cosisq = cosim * cosim;
768 emo = em;
769 em = ecco;
770 emsqo = emsq;
771 emsq = eccsq;
772 eoc = em * emsq;
773 g201 = -0.306 - (em - 0.64) * 0.440;
774
775 if (em <= 0.65)
776 {
777 g211 = 3.616 - 13.2470 * em + 16.2900 * emsq;
778 g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc;
779 g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc;
780 g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc;
781 g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc;
782 g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc;
783 }
784 else
785 {
786 g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc;
787 g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc;
788 g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc;
789 g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc;
790 g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc;
791 if (em > 0.715)
792 g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc;
793 else
794 g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq;
795 }
796
797 if (em < 0.7)
798 {
799 g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc;
800 g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc;
801 g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc;
802 }
803 else
804 {
805 g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc;
806 g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc;
807 g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc;
808 }
809
810 sini2= sinim * sinim;
811 f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq);
812 f221 = 1.5 * sini2;
813 f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq);
814 f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq);
815 f441 = 35.0 * sini2 * f220;
816 f442 = 39.3750 * sini2 * sini2;
817 f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) +
818 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) );
819 f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim +
820 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq));
821 f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq *
822 (-12.0 + 8.0 * cosim + 10.0 * cosisq));
823 f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq *
824 (12.0 + 8.0 * cosim - 10.0 * cosisq));
825 xno2 = nm * nm;
826 ainv2 = aonv * aonv;
827 temp1 = 3.0 * xno2 * ainv2;
828 temp = temp1 * root22;
829 d2201 = temp * f220 * g201;
830 d2211 = temp * f221 * g211;
831 temp1 = temp1 * aonv;
832 temp = temp1 * root32;
833 d3210 = temp * f321 * g310;
834 d3222 = temp * f322 * g322;
835 temp1 = temp1 * aonv;
836 temp = 2.0 * temp1 * root44;
837 d4410 = temp * f441 * g410;
838 d4422 = temp * f442 * g422;
839 temp1 = temp1 * aonv;
840 temp = temp1 * root52;
841 d5220 = temp * f522 * g520;
842 d5232 = temp * f523 * g532;
843 temp = 2.0 * temp1 * root54;
844 d5421 = temp * f542 * g521;
845 d5433 = temp * f543 * g533;
846 xlamo = std::fmod(mo + nodeo + nodeo-theta - theta, twopi);
847 xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no;
848 em = emo;
849 emsq = emsqo;
850 }
851
852 /* ---------------- synchronous resonance terms -------------- */
853 if (irez == 1)
854 {
855 g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq);
856 g310 = 1.0 + 2.0 * emsq;
857 g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq);
858 f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim);
859 f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim);
860 f330 = 1.0 + cosim;
861 f330 = 1.875 * f330 * f330 * f330;
862 del1 = 3.0 * nm * nm * aonv * aonv;
863 del2 = 2.0 * del1 * f220 * g200 * q22;
864 del3 = 3.0 * del1 * f330 * g300 * q33 * aonv;
865 del1 = del1 * f311 * g310 * q31 * aonv;
866 xlamo = std::fmod(mo + nodeo + argpo - theta, twopi);
867 xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no;
868 }
869
870 /* ------------ for sgp4, initialize the integrator ---------- */
871 xli = xlamo;
872 xni = no;
873 atime = 0.0;
874 nm = no + dndt;
875 }
876 } // end dsinit
877
878 /*-----------------------------------------------------------------------------
879 *
880 * procedure dspace
881 *
882 * this procedure provides deep space contributions to mean elements for
883 * perturbing third body. these effects have been averaged over one
884 * revolution of the sun and moon. for earth resonance effects, the
885 * effects have been averaged over no revolutions of the satellite.
886 * (mean motion)
887 *
888 * author : david vallado 719-573-2600 28 jun 2005
889 *
890 * inputs :
891 * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 -
892 * dedt -
893 * del1, del2, del3 -
894 * didt -
895 * dmdt -
896 * dnodt -
897 * domdt -
898 * irez - flag for resonance 0-none, 1-one day, 2-half day
899 * argpo - argument of perigee
900 * argpdot - argument of perigee dot (rate)
901 * t - time
902 * tc -
903 * gsto - gst
904 * xfact -
905 * xlamo -
906 * no - mean motion
907 * atime -
908 * em - eccentricity
909 * ft -
910 * argpm - argument of perigee
911 * inclm - inclination
912 * xli -
913 * mm - mean anomaly
914 * xni - mean motion
915 * nodem - right ascension of ascending node
916 *
917 * outputs :
918 * atime -
919 * em - eccentricity
920 * argpm - argument of perigee
921 * inclm - inclination
922 * xli -
923 * mm - mean anomaly
924 * xni -
925 * nodem - right ascension of ascending node
926 * dndt -
927 * nm - mean motion
928 *
929 * locals :
930 * delt -
931 * ft -
932 * theta -
933 * x2li -
934 * x2omi -
935 * xl -
936 * xldot -
937 * xnddt -
938 * xndt -
939 * xomi -
940 *
941 * coupling :
942 * none -
943 *
944 * references :
945 * hoots, roehrich, norad spacetrack report #3 1980
946 * hoots, norad spacetrack report #6 1986
947 * hoots, schumacher and glover 2004
948 * vallado, crawford, hujsak, kelso 2006
949 ----------------------------------------------------------------------------*/
950
dspace(int irez,double d2201,double d2211,double d3210,double d3222,double d4410,double d4422,double d5220,double d5232,double d5421,double d5433,double dedt,double del1,double del2,double del3,double didt,double dmdt,double dnodt,double domdt,double argpo,double argpdot,double t,double tc,double gsto,double xfact,double xlamo,double no,double & atime,double & em,double & argpm,double & inclm,double & xli,double & mm,double & xni,double & nodem,double & dndt,double & nm)951 static void dspace(int irez,
952 double d2201, double d2211, double d3210, double d3222, double d4410,
953 double d4422, double d5220, double d5232, double d5421, double d5433,
954 double dedt, double del1, double del2, double del3, double didt,
955 double dmdt, double dnodt, double domdt, double argpo, double argpdot,
956 double t, double tc, double gsto, double xfact, double xlamo,
957 double no,
958 double& atime, double& em, double& argpm, double& inclm, double& xli,
959 double& mm, double& xni, double& nodem, double& dndt, double& nm)
960 {
961 const double twopi = 2.0 * M_PI;
962 int iretn , iret;
963 double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32,
964 g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp;
965
966 xldot = 0.0;
967 xnddt = 0.0;
968 xndt = 0.0;
969 fasx2 = 0.13130908;
970 fasx4 = 2.8843198;
971 fasx6 = 0.37448087;
972 g22 = 5.7686396;
973 g32 = 0.95240898;
974 g44 = 1.8014998;
975 g52 = 1.0508330;
976 g54 = 4.4108898;
977 rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
978 stepp = 720.0;
979 stepn = -720.0;
980 step2 = 259200.0;
981
982 /* ----------- calculate deep space resonance effects ----------- */
983 dndt = 0.0;
984 theta = std::fmod(gsto + tc * rptim, twopi);
985 em = em + dedt * t;
986
987 inclm = inclm + didt * t;
988 argpm = argpm + domdt * t;
989 nodem = nodem + dnodt * t;
990 mm = mm + dmdt * t;
991
992 // sgp4fix for negative inclinations
993 // the following if statement should be commented out
994 // if (inclm < 0.0)
995 // {
996 // inclm = -inclm;
997 // argpm = argpm - pi;
998 // nodem = nodem + pi;
999 // }
1000
1001 /* - update resonances : numerical (euler-maclaurin) integration - */
1002 /* ------------------------- epoch restart ---------------------- */
1003 // sgp4fix for propagator problems
1004 // the following integration works for negative time steps and periods
1005 // the specific changes are unknown because the original code was so convoluted
1006
1007 // sgp4fix take out atime = 0.0 and fix for faster operation
1008 ft = 0.0;
1009 if (irez != 0)
1010 {
1011 // sgp4fix streamline check
1012 if ((atime == 0.0) || (t * atime <= 0.0) || (std::fabs(t) < std::fabs(atime)) )
1013 {
1014 atime = 0.0;
1015 xni = no;
1016 xli = xlamo;
1017 }
1018 // sgp4fix move check outside loop
1019 if (t > 0.0)
1020 delt = stepp;
1021 else
1022 delt = stepn;
1023
1024 iretn = 381; // added for do loop
1025 iret = 0; // added for loop
1026 static_cast<void>(iret);
1027 while (iretn == 381)
1028 {
1029 /* ------------------- dot terms calculated ------------- */
1030 /* ----------- near - synchronous resonance terms ------- */
1031 if (irez != 2)
1032 {
1033 xndt = del1 * std::sin(xli - fasx2) + del2 * std::sin(2.0 * (xli - fasx4)) +
1034 del3 * std::sin(3.0 * (xli - fasx6));
1035 xldot = xni + xfact;
1036 xnddt = del1 * std::cos(xli - fasx2) +
1037 2.0 * del2 * std::cos(2.0 * (xli - fasx4)) +
1038 3.0 * del3 * std::cos(3.0 * (xli - fasx6));
1039 xnddt = xnddt * xldot;
1040 }
1041 else
1042 {
1043 /* --------- near - half-day resonance terms -------- */
1044 xomi = argpo + argpdot * atime;
1045 x2omi = xomi + xomi;
1046 x2li = xli + xli;
1047 xndt = d2201 * std::sin(x2omi + xli - g22) + d2211 * std::sin(xli - g22) +
1048 d3210 * std::sin(xomi + xli - g32) + d3222 * std::sin(-xomi + xli - g32)+
1049 d4410 * std::sin(x2omi + x2li - g44)+ d4422 * std::sin(x2li - g44) +
1050 d5220 * std::sin(xomi + xli - g52) + d5232 * std::sin(-xomi + xli - g52)+
1051 d5421 * std::sin(xomi + x2li - g54) + d5433 * std::sin(-xomi + x2li - g54);
1052 xldot = xni + xfact;
1053 xnddt = d2201 * std::cos(x2omi + xli - g22) + d2211 * std::cos(xli - g22) +
1054 d3210 * std::cos(xomi + xli - g32) + d3222 * std::cos(-xomi + xli - g32) +
1055 d5220 * std::cos(xomi + xli - g52) + d5232 * std::cos(-xomi + xli - g52) +
1056 2.0 * (d4410 * std::cos(x2omi + x2li - g44) +
1057 d4422 * std::cos(x2li - g44) + d5421 * std::cos(xomi + x2li - g54) +
1058 d5433 * std::cos(-xomi + x2li - g54));
1059 xnddt = xnddt * xldot;
1060 }
1061
1062 /* ----------------------- integrator ------------------- */
1063 // sgp4fix move end checks to end of routine
1064 if (std::fabs(t - atime) >= stepp)
1065 {
1066 // NOTE: Never read!
1067 // iret = 0;
1068 iretn = 381;
1069 }
1070 else // exit here
1071 {
1072 ft = t - atime;
1073 iretn = 0;
1074 }
1075
1076 if (iretn == 381)
1077 {
1078 xli = xli + xldot * delt + xndt * step2;
1079 xni = xni + xndt * delt + xnddt * step2;
1080 atime = atime + delt;
1081 }
1082 } // while iretn = 381
1083
1084 nm = xni + xndt * ft + xnddt * ft * ft * 0.5;
1085 xl = xli + xldot * ft + xndt * ft * ft * 0.5;
1086 if (irez != 1)
1087 {
1088 mm = xl - 2.0 * nodem + 2.0 * theta;
1089 dndt = nm - no;
1090 }
1091 else
1092 {
1093 mm = xl - nodem - argpm + theta;
1094 dndt = nm - no;
1095 }
1096 nm = no + dndt;
1097 }
1098 } // end dsspace
1099
1100 /*-----------------------------------------------------------------------------
1101 *
1102 * procedure initl
1103 *
1104 * this procedure initializes the spg4 propagator. all the initialization is
1105 * consolidated here instead of having multiple loops inside other routines.
1106 *
1107 * author : david vallado 719-573-2600 28 jun 2005
1108 *
1109 * inputs :
1110 * ecco - eccentricity 0.0 - 1.0
1111 * epoch - epoch time in days from jan 0, 1950. 0 hr
1112 * inclo - inclination of satellite
1113 * no - mean motion of satellite
1114 * satn - satellite number
1115 *
1116 * outputs :
1117 * ainv - 1.0 / a
1118 * ao - semi major axis
1119 * con41 -
1120 * con42 - 1.0 - 5.0 cos(i)
1121 * cosio - cosine of inclination
1122 * cosio2 - cosio squared
1123 * eccsq - eccentricity squared
1124 * method - flag for deep space 'd', 'n'
1125 * omeosq - 1.0 - ecco * ecco
1126 * posq - semi-parameter squared
1127 * rp - radius of perigee
1128 * rteosq - square root of (1.0 - ecco*ecco)
1129 * sinio - sine of inclination
1130 * gsto - gst at time of observation rad
1131 * no - mean motion of satellite
1132 *
1133 * locals :
1134 * ak -
1135 * d1 -
1136 * del -
1137 * adel -
1138 * po -
1139 *
1140 * coupling :
1141 * getgravconst
1142 * gstime - find greenwich sidereal time from the julian date
1143 *
1144 * references :
1145 * hoots, roehrich, norad spacetrack report #3 1980
1146 * hoots, norad spacetrack report #6 1986
1147 * hoots, schumacher and glover 2004
1148 * vallado, crawford, hujsak, kelso 2006
1149 ----------------------------------------------------------------------------*/
1150
initl(int satn,gravconsttype whichconst,double ecco,double epoch,double inclo,double & no,char & method,double & ainv,double & ao,double & con41,double & con42,double & cosio,double & cosio2,double & eccsq,double & omeosq,double & posq,double & rp,double & rteosq,double & sinio,double & gsto,char opsmode)1151 static void initl(int satn, gravconsttype whichconst,
1152 double ecco, double epoch, double inclo, double& no,
1153 char& method,
1154 double& ainv, double& ao, double& con41, double& con42, double& cosio,
1155 double& cosio2,double& eccsq, double& omeosq, double& posq,
1156 double& rp, double& rteosq,double& sinio , double& gsto,
1157 char opsmode)
1158 {
1159 /* --------------------- local variables ------------------------ */
1160 double ak, d1, del, adel, po, x2o3, j2=0, xke=0,
1161 tumin, mu, radiusearthkm, j3, j4, j3oj2;
1162
1163 /* kill warning */
1164 satn = 0;
1165 static_cast<void>(satn);
1166
1167 // sgp4fix use old way of finding gst
1168 double ds70;
1169 double ts70, tfrac, c1, thgr70, fk5r, c1p2p;
1170 const double twopi = 2.0 * M_PI;
1171
1172 /* ----------------------- earth constants ---------------------- */
1173 // sgp4fix identify constants and allow alternate values
1174 getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
1175 x2o3 = 2.0 / 3.0;
1176
1177 /* ------------- calculate auxillary epoch quantities ---------- */
1178 eccsq = ecco * ecco;
1179 omeosq = 1.0 - eccsq;
1180 rteosq = std::sqrt(omeosq);
1181 cosio = std::cos(inclo);
1182 cosio2 = cosio * cosio;
1183
1184 /* ------------------ un-kozai the mean motion ----------------- */
1185 ak = std::pow(xke / no, x2o3);
1186 d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq);
1187 del = d1 / (ak * ak);
1188 adel = ak * (1.0 - del * del - del * (1.0 / 3.0 + 134.0 * del * del / 81.0));
1189 del = d1/(adel * adel);
1190 no = no / (1.0 + del);
1191
1192 ao = std::pow(xke / no, x2o3);
1193 sinio = std::sin(inclo);
1194 po = ao * omeosq;
1195 con42 = 1.0 - 5.0 * cosio2;
1196 con41 = -con42-cosio2-cosio2;
1197 ainv = 1.0 / ao;
1198 posq = po * po;
1199 rp = ao * (1.0 - ecco);
1200 method = 'n';
1201
1202 // sgp4fix modern approach to finding sidereal time
1203 if (opsmode == 'a')
1204 {
1205 // sgp4fix use old way of finding gst
1206 // count integer number of days from 0 jan 1970
1207 ts70 = epoch - 7305.0;
1208 ds70 = std::floor(ts70 + 1.0e-8);
1209 tfrac = ts70 - ds70;
1210 // find greenwich location at epoch
1211 c1 = 1.72027916940703639e-2;
1212 thgr70= 1.7321343856509374;
1213 fk5r = 5.07551419432269442e-15;
1214 c1p2p = c1 + twopi;
1215 gsto = std::fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi);
1216 if ( gsto < 0.0 )
1217 gsto = gsto + twopi;
1218 }
1219 else
1220 gsto = gstime(epoch + 2433281.5);
1221 } // end initl
1222
1223 /*-----------------------------------------------------------------------------
1224 *
1225 * procedure sgp4init
1226 *
1227 * this procedure initializes variables for sgp4.
1228 *
1229 * author : david vallado 719-573-2600 28 jun 2005
1230 *
1231 * inputs :
1232 * opsmode - mode of operation afspc or improved 'a', 'i'
1233 * whichconst - which set of constants to use 72, 84
1234 * satn - satellite number
1235 * bstar - sgp4 type drag coefficient kg/m2er
1236 * ecco - eccentricity
1237 * epoch - epoch time in days from jan 0, 1950. 0 hr
1238 * argpo - argument of perigee (output if ds)
1239 * inclo - inclination
1240 * mo - mean anomaly (output if ds)
1241 * no - mean motion
1242 * nodeo - right ascension of ascending node
1243 *
1244 * outputs :
1245 * satrec - common values for subsequent calls
1246 * return code - non-zero on error.
1247 * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
1248 * 2 - mean motion less than 0.0
1249 * 3 - pert elements, ecc < 0.0 or ecc > 1.0
1250 * 4 - semi-latus rectum < 0.0
1251 * 5 - epoch elements are sub-orbital
1252 * 6 - satellite has decayed
1253 *
1254 * locals :
1255 * cnodm , snodm , cosim , sinim , cosomm , sinomm
1256 * cc1sq , cc2 , cc3
1257 * coef , coef1
1258 * cosio4 -
1259 * day -
1260 * dndt -
1261 * em - eccentricity
1262 * emsq - eccentricity squared
1263 * eeta -
1264 * etasq -
1265 * gam -
1266 * argpm - argument of perigee
1267 * nodem -
1268 * inclm - inclination
1269 * mm - mean anomaly
1270 * nm - mean motion
1271 * perige - perigee
1272 * pinvsq -
1273 * psisq -
1274 * qzms24 -
1275 * rtemsq -
1276 * s1, s2, s3, s4, s5, s6, s7 -
1277 * sfour -
1278 * ss1, ss2, ss3, ss4, ss5, ss6, ss7 -
1279 * sz1, sz2, sz3
1280 * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 -
1281 * tc -
1282 * temp -
1283 * temp1, temp2, temp3 -
1284 * tsi -
1285 * xpidot -
1286 * xhdot1 -
1287 * z1, z2, z3 -
1288 * z11, z12, z13, z21, z22, z23, z31, z32, z33 -
1289 *
1290 * coupling :
1291 * getgravconst-
1292 * initl -
1293 * dscom -
1294 * dpper -
1295 * dsinit -
1296 * sgp4 -
1297 *
1298 * references :
1299 * hoots, roehrich, norad spacetrack report #3 1980
1300 * hoots, norad spacetrack report #6 1986
1301 * hoots, schumacher and glover 2004
1302 * vallado, crawford, hujsak, kelso 2006
1303 ----------------------------------------------------------------------------*/
1304
sgp4init(gravconsttype whichconst,char opsmode,const int satn,const double epoch,const double xbstar,const double xecco,const double xargpo,const double xinclo,const double xmo,const double xno,const double xnodeo,elsetrec & satrec)1305 bool sgp4init(gravconsttype whichconst, char opsmode, const int satn, const double epoch,
1306 const double xbstar, const double xecco, const double xargpo,
1307 const double xinclo, const double xmo, const double xno,
1308 const double xnodeo, elsetrec& satrec)
1309 {
1310 /* --------------------- local variables ------------------------ */
1311 double ao , ainv , con42 , cosio , sinio , cosio2, eccsq ,
1312 omeosq, posq , rp , rteosq,
1313 cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq ,
1314 cc2 , cc3 , coef , coef1 , cosio4, day , dndt ,
1315 em , emsq , eeta , etasq , gam , argpm , nodem ,
1316 inclm , mm , nm , perige, pinvsq, psisq , qzms24,
1317 rtemsq, s1 , s2 , s3 , s4 , s5 , s6 ,
1318 s7 , sfour , ss1 , ss2 , ss3 , ss4 , ss5 ,
1319 ss6 , ss7 , sz1 , sz2 , sz3 , sz11 , sz12 ,
1320 sz13 , sz21 , sz22 , sz23 , sz31 , sz32 , sz33 ,
1321 tc , temp , temp1 , temp2 , temp3 , tsi , xpidot,
1322 xhdot1, z1 , z2 , z3 , z11 , z12 , z13 ,
1323 z21 , z22 , z23 , z31 , z32 , z33 , qzms2t,
1324 ss , j2 , j3oj2 , j4 , x2o3 , r[3] , v[3] ,
1325 tumin , mu , radiusearthkm, xke, j3;
1326
1327 /* ------------------------ initialization --------------------- */
1328 // sgp4fix divisor for divide by zero check on inclination
1329 // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
1330 // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
1331 const double temp4 = 1.5e-12;
1332
1333 ss1 = 0.0; ss2 = 0.0; ss3 = 0.0;
1334 ss4 = 0.0; ss5 = 0.0; ss6 = 0.0;
1335 ss6 = 0.0;
1336 sz1 = 0.0; sz2 = 0.0; sz3 = 0.0;
1337 sz11 = 0.0; sz12 = 0.0; sz13 = 0.0;
1338 sz21 = 0.0; sz22 = 0.0; sz23 = 0.0;
1339 sz31 = 0.0; sz32 = 0.0; sz33 = 0.0;
1340 j2 = 0; j3oj2 = 0; j4 = 0;
1341 radiusearthkm = 0;
1342
1343 /* ----------- set all near earth variables to zero ------------ */
1344 satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0;
1345 satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0;
1346 satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0;
1347 satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0;
1348 satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0;
1349 satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0;
1350 satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0;
1351 satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0;
1352 satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0;
1353
1354 /* ----------- set all deep space variables to zero ------------ */
1355 satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0;
1356 satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0;
1357 satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0;
1358 satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0;
1359 satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0;
1360 satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0;
1361 satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0;
1362 satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0;
1363 satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0;
1364 satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0;
1365 satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0;
1366 satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0;
1367 satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0;
1368 satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0;
1369 satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0;
1370 satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0;
1371 satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0;
1372 satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0;
1373 satrec.xli = 0.0; satrec.xni = 0.0;
1374
1375 // sgp4fix - note the following variables are also passed directly via satrec.
1376 // it is possible to streamline the sgp4init call by deleting the "x"
1377 // variables, but the user would need to set the satrec.* values first. we
1378 // include the additional assignments in case twoline2rv is not used.
1379 satrec.bstar = xbstar;
1380 satrec.ecco = xecco;
1381 satrec.argpo = xargpo;
1382 satrec.inclo = xinclo;
1383 satrec.mo = xmo;
1384 satrec.no = xno;
1385 satrec.nodeo = xnodeo;
1386
1387 // sgp4fix add opsmode
1388 satrec.operationmode = opsmode;
1389
1390 /* ------------------------ earth constants ----------------------- */
1391 // sgp4fix identify constants and allow alternate values
1392 getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
1393 ss = 78.0 / radiusearthkm + 1.0;
1394 qzms2t = std::pow(((120.0 - 78.0) / radiusearthkm), 4);
1395 x2o3 = 2.0 / 3.0;
1396
1397 satrec.init = 'y';
1398 satrec.t = 0.0;
1399
1400 initl(satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method,
1401 ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq,
1402 posq, rp, rteosq, sinio, satrec.gsto, satrec.operationmode);
1403 satrec.error = 0;
1404
1405 // sgp4fix remove this check as it is unnecessary
1406 // the mrt check in sgp4 handles decaying satellite cases even if the starting
1407 // condition is below the surface of te earth
1408 // if (rp < 1.0)
1409 // {
1410 // printf("# *** satn%d epoch elts sub-orbital ***\n", satn);
1411 // satrec.error = 5;
1412 // }
1413
1414 if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0))
1415 {
1416 satrec.isimp = 0;
1417 if (rp < (220.0 / radiusearthkm + 1.0))
1418 satrec.isimp = 1;
1419 sfour = ss;
1420 qzms24 = qzms2t;
1421 perige = (rp - 1.0) * radiusearthkm;
1422
1423 /* - for perigees below 156 km, s and qoms2t are altered - */
1424 if (perige < 156.0)
1425 {
1426 sfour = perige - 78.0;
1427 if (perige < 98.0)
1428 sfour = 20.0;
1429 qzms24 = std::pow(((120.0 - sfour) / radiusearthkm), 4.0);
1430 sfour = sfour / radiusearthkm + 1.0;
1431 }
1432 pinvsq = 1.0 / posq;
1433
1434 tsi = 1.0 / (ao - sfour);
1435 satrec.eta = ao * satrec.ecco * tsi;
1436 etasq = satrec.eta * satrec.eta;
1437 eeta = satrec.ecco * satrec.eta;
1438 psisq = std::fabs(1.0 - etasq);
1439 coef = qzms24 * std::pow(tsi, 4.0);
1440 coef1 = coef / std::pow(psisq, 3.5);
1441 cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta *
1442 (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 *
1443 (8.0 + 3.0 * etasq * (8.0 + etasq)));
1444 satrec.cc1 = satrec.bstar * cc2;
1445 cc3 = 0.0;
1446 if (satrec.ecco > 1.0e-4)
1447 cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco;
1448 satrec.x1mth2 = 1.0 - cosio2;
1449 satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq *
1450 (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco *
1451 (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) *
1452 (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq *
1453 (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 *
1454 (2.0 * etasq - eeta * (1.0 + etasq)) * std::cos(2.0 * satrec.argpo)));
1455 satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 *
1456 (etasq + eeta) + eeta * etasq);
1457
1458 cosio4 = cosio2 * cosio2;
1459 temp1 = 1.5 * j2 * pinvsq * satrec.no;
1460 temp2 = 0.5 * temp1 * j2 * pinvsq;
1461 temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no;
1462 satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 *
1463 temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4);
1464 satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 *
1465 (7.0 - 114.0 * cosio2 + 395.0 * cosio4) +
1466 temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4);
1467 xhdot1 = -temp1 * cosio;
1468 satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) +
1469 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio;
1470 xpidot = satrec.argpdot+ satrec.nodedot;
1471 satrec.omgcof = satrec.bstar * cc3 * std::cos(satrec.argpo);
1472 satrec.xmcof = 0.0;
1473 if (satrec.ecco > 1.0e-4)
1474 satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta;
1475 satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1;
1476 satrec.t2cof = 1.5 * satrec.cc1;
1477 // sgp4fix for divide by zero with xinco = 180 deg
1478 if (std::fabs(cosio+1.0) > 1.5e-12)
1479 satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio);
1480 else
1481 satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4;
1482 satrec.aycof = -0.5 * j3oj2 * sinio;
1483 satrec.delmo = std::pow((1.0 + satrec.eta * std::cos(satrec.mo)), 3);
1484 satrec.sinmao = std::sin(satrec.mo);
1485 satrec.x7thm1 = 7.0 * cosio2 - 1.0;
1486
1487 /* --------------- deep space initialization ------------- */
1488 if ((2*M_PI / satrec.no) >= 225.0)
1489 {
1490 satrec.method = 'd';
1491 satrec.isimp = 1;
1492 tc = 0.0;
1493 inclm = satrec.inclo;
1494
1495 dscom(epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo,
1496 satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm,
1497 day, satrec.e3, satrec.ee2, em, emsq, gam,
1498 satrec.peo, satrec.pgho, satrec.pho, satrec.pinco,
1499 satrec.plo, rtemsq, satrec.se2, satrec.se3,
1500 satrec.sgh2, satrec.sgh3, satrec.sgh4,
1501 satrec.sh2, satrec.sh3, satrec.si2, satrec.si3,
1502 satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5,
1503 s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3,
1504 sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33,
1505 satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2,
1506 satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2,
1507 satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11,
1508 z12, z13, z21, z22, z23, z31, z32, z33,
1509 satrec.zmol, satrec.zmos);
1510 dpper(satrec.e3, satrec.ee2, satrec.peo, satrec.pgho,
1511 satrec.pho, satrec.pinco, satrec.plo, satrec.se2,
1512 satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4,
1513 satrec.sh2, satrec.sh3, satrec.si2, satrec.si3,
1514 satrec.sl2, satrec.sl3, satrec.sl4, satrec.t,
1515 satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2,
1516 satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2,
1517 satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init,
1518 satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo,
1519 satrec.operationmode);
1520
1521 argpm = 0.0;
1522 nodem = 0.0;
1523 mm = 0.0;
1524
1525 dsinit(whichconst,
1526 cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4,
1527 ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc,
1528 satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo,
1529 satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33,
1530 satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem,
1531 satrec.irez, satrec.atime,
1532 satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 ,
1533 satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232,
1534 satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt,
1535 satrec.dmdt, dndt, satrec.dnodt, satrec.domdt ,
1536 satrec.del1, satrec.del2, satrec.del3, satrec.xfact,
1537 satrec.xlamo, satrec.xli, satrec.xni);
1538 }
1539
1540 /* ----------- set variables if not deep space ----------- */
1541 if (satrec.isimp != 1)
1542 {
1543 cc1sq = satrec.cc1 * satrec.cc1;
1544 satrec.d2 = 4.0 * ao * tsi * cc1sq;
1545 temp = satrec.d2 * tsi * satrec.cc1 / 3.0;
1546 satrec.d3 = (17.0 * ao + sfour) * temp;
1547 satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * satrec.cc1;
1548 satrec.t3cof = satrec.d2 + 2.0 * cc1sq;
1549 satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * (12.0 * satrec.d2 + 10.0 * cc1sq));
1550 satrec.t5cof = 0.2 * (3.0 * satrec.d4 + 12.0 * satrec.cc1 * satrec.d3 +
1551 6.0 * satrec.d2 * satrec.d2 + 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq));
1552 }
1553 } // if omeosq = 0 ...
1554
1555 /* finally propogate to zero epoch to initialize all others. */
1556 // sgp4fix take out check to let satellites process until they are actually below earth surface
1557 // if(satrec.error == 0)
1558 sgp4(whichconst, satrec, 0.0, r, v);
1559
1560 satrec.init = 'n';
1561
1562 //sgp4fix return boolean. satrec.error contains any error codes
1563 return true;
1564 } // end sgp4init
1565
1566 /*-----------------------------------------------------------------------------
1567 *
1568 * procedure sgp4
1569 *
1570 * this procedure is the sgp4 prediction model from space command. this is an
1571 * updated and combined version of sgp4 and sdp4, which were originally
1572 * published separately in spacetrack report #3. this version follows the
1573 * methodology from the aiaa paper (2006) describing the history and
1574 * development of the code.
1575 *
1576 * author : david vallado 719-573-2600 28 jun 2005
1577 *
1578 * inputs :
1579 * satrec - initialised structure from sgp4init() call.
1580 * tsince - time eince epoch (minutes)
1581 *
1582 * outputs :
1583 * r - position vector km
1584 * v - velocity km/sec
1585 * return code - non-zero on error.
1586 * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
1587 * 2 - mean motion less than 0.0
1588 * 3 - pert elements, ecc < 0.0 or ecc > 1.0
1589 * 4 - semi-latus rectum < 0.0
1590 * 5 - epoch elements are sub-orbital
1591 * 6 - satellite has decayed
1592 *
1593 * locals :
1594 * am -
1595 * axnl, aynl -
1596 * betal -
1597 * cosim , sinim , cosomm , sinomm , cnod , snod , cos2u ,
1598 * sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip ,
1599 * cosisq , cossu , sinsu , cosu , sinu
1600 * delm -
1601 * delomg -
1602 * dndt -
1603 * eccm -
1604 * emsq -
1605 * ecose -
1606 * el2 -
1607 * eo1 -
1608 * eccp -
1609 * esine -
1610 * argpm -
1611 * argpp -
1612 * omgadf -
1613 * pl -
1614 * r -
1615 * rtemsq -
1616 * rdotl -
1617 * rl -
1618 * rvdot -
1619 * rvdotl -
1620 * su -
1621 * t2 , t3 , t4 , tc
1622 * tem5, temp , temp1 , temp2 , tempa , tempe , templ
1623 * u , ux , uy , uz , vx , vy , vz
1624 * inclm - inclination
1625 * mm - mean anomaly
1626 * nm - mean motion
1627 * nodem - right asc of ascending node
1628 * xinc -
1629 * xincp -
1630 * xl -
1631 * xlm -
1632 * mp -
1633 * xmdf -
1634 * xmx -
1635 * xmy -
1636 * nodedf -
1637 * xnode -
1638 * nodep -
1639 * np -
1640 *
1641 * coupling :
1642 * getgravconst-
1643 * dpper
1644 * dpspace
1645 *
1646 * references :
1647 * hoots, roehrich, norad spacetrack report #3 1980
1648 * hoots, norad spacetrack report #6 1986
1649 * hoots, schumacher and glover 2004
1650 * vallado, crawford, hujsak, kelso 2006
1651 ----------------------------------------------------------------------------*/
1652
sgp4(gravconsttype whichconst,elsetrec & satrec,double tsince,double r[3],double v[3])1653 bool sgp4(gravconsttype whichconst, elsetrec& satrec, double tsince, double r[3], double v[3])
1654 {
1655 double am , axnl , aynl , betal , cosim , cnod ,
1656 cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu,
1657 delm , delomg, em , ecose , el2 , eo1 ,
1658 ep , esine , argpm, argpp , argpdf, pl , mrt ,
1659 mvt , rdotl , rl , rvdot , rvdotl, sinim ,
1660 sin2u, sineo1, sini , sinip , sinsu , sinu ,
1661 snod , su , t2 , t3 , t4 , tem5 , temp,
1662 temp1, temp2 , tempa, tempe , templ , u , ux ,
1663 uy , uz , vx , vy , vz , inclm , mm ,
1664 nm , nodem , xinc , xincp , xl , xlm , mp ,
1665 xmdf , xmx , xmy , nodedf, xnode , nodep , tc , dndt ,
1666 twopi, x2o3 , j2 , j3 , tumin , j4 , xke , j3oj2,
1667 radiusearthkm, mu, vkmpersec;
1668 int ktr;
1669 coseo1 = 0.0;
1670 mrt = 0.0;
1671 sineo1 = 0.0;
1672 j2 = 0;
1673 j4 = 0;
1674 xke = 0;
1675 j3oj2 = 0;
1676 radiusearthkm = 0;
1677
1678 /* ------------------ set mathematical constants --------------- */
1679 // sgp4fix divisor for divide by zero check on inclination
1680 // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to
1681 // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency
1682 const double temp4 = 1.5e-12;
1683 twopi = 2.0 * M_PI;
1684 x2o3 = 2.0 / 3.0;
1685 // sgp4fix identify constants and allow alternate values
1686 getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
1687 vkmpersec = radiusearthkm * xke/60.0;
1688
1689 /* --------------------- clear sgp4 error flag ----------------- */
1690 satrec.t = tsince;
1691 satrec.error = 0;
1692
1693 /* ------- update for secular gravity and atmospheric drag ----- */
1694 xmdf = satrec.mo + satrec.mdot * satrec.t;
1695 argpdf = satrec.argpo + satrec.argpdot * satrec.t;
1696 nodedf = satrec.nodeo + satrec.nodedot * satrec.t;
1697 argpm = argpdf;
1698 mm = xmdf;
1699 t2 = satrec.t * satrec.t;
1700 nodem = nodedf + satrec.nodecf * t2;
1701 tempa = 1.0 - satrec.cc1 * satrec.t;
1702 tempe = satrec.bstar * satrec.cc4 * satrec.t;
1703 templ = satrec.t2cof * t2;
1704
1705 if (satrec.isimp != 1)
1706 {
1707 delomg = satrec.omgcof * satrec.t;
1708 delm = satrec.xmcof * (std::pow((1.0 + satrec.eta * std::cos(xmdf)), 3) - satrec.delmo);
1709 temp = delomg + delm;
1710 mm = xmdf + temp;
1711 argpm = argpdf - temp;
1712 t3 = t2 * satrec.t;
1713 t4 = t3 * satrec.t;
1714 tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - satrec.d4 * t4;
1715 tempe = tempe + satrec.bstar * satrec.cc5 * (std::sin(mm) - satrec.sinmao);
1716 templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + satrec.t * satrec.t5cof);
1717 }
1718
1719 nm = satrec.no;
1720 em = satrec.ecco;
1721 inclm = satrec.inclo;
1722 if (satrec.method == 'd')
1723 {
1724 tc = satrec.t;
1725 dspace(satrec.irez,
1726 satrec.d2201, satrec.d2211, satrec.d3210,
1727 satrec.d3222, satrec.d4410, satrec.d4422,
1728 satrec.d5220, satrec.d5232, satrec.d5421,
1729 satrec.d5433, satrec.dedt, satrec.del1,
1730 satrec.del2, satrec.del3, satrec.didt,
1731 satrec.dmdt, satrec.dnodt, satrec.domdt,
1732 satrec.argpo, satrec.argpdot, satrec.t, tc,
1733 satrec.gsto, satrec.xfact, satrec.xlamo,
1734 satrec.no, satrec.atime,
1735 em, argpm, inclm, satrec.xli, mm, satrec.xni,
1736 nodem, dndt, nm);
1737 } // if method = d
1738
1739 if (nm <= 0.0)
1740 {
1741 // printf("# error nm %f\n", nm);
1742 satrec.error = 2;
1743 // sgp4fix add return
1744 return false;
1745 }
1746 am = std::pow((xke / nm),x2o3) * tempa * tempa;
1747 nm = xke / std::pow(am, 1.5);
1748 em = em - tempe;
1749 // fix tolerance for error recognition
1750 // sgp4fix am is fixed from the previous nm check
1751 if ((em >= 1.0) || (em < -0.001)/* || (am < 0.95)*/ )
1752 {
1753 // printf("# error em %f\n", em);
1754 satrec.error = 1;
1755 // sgp4fix to return if there is an error in eccentricity
1756 return false;
1757 }
1758 // sgp4fix fix tolerance to avoid a divide by zero
1759 if (em < 1.0e-6)
1760 em = 1.0e-6;
1761 mm = mm + satrec.no * templ;
1762 xlm = mm + argpm + nodem;
1763 // NOTE: Never read!
1764 // emsq = em * em;
1765 // temp = 1.0 - emsq;
1766
1767 nodem = std::fmod(nodem, twopi);
1768 argpm = std::fmod(argpm, twopi);
1769 xlm = std::fmod(xlm, twopi);
1770 mm = std::fmod(xlm - argpm - nodem, twopi);
1771
1772 /* ----------------- compute extra mean quantities ------------- */
1773 sinim = std::sin(inclm);
1774 cosim = std::cos(inclm);
1775
1776 /* -------------------- add lunar-solar periodics -------------- */
1777 ep = em;
1778 xincp = inclm;
1779 argpp = argpm;
1780 nodep = nodem;
1781 mp = mm;
1782 sinip = sinim;
1783 cosip = cosim;
1784 if (satrec.method == 'd')
1785 {
1786 dpper(satrec.e3, satrec.ee2, satrec.peo,
1787 satrec.pgho, satrec.pho, satrec.pinco,
1788 satrec.plo, satrec.se2, satrec.se3,
1789 satrec.sgh2, satrec.sgh3, satrec.sgh4,
1790 satrec.sh2, satrec.sh3, satrec.si2,
1791 satrec.si3, satrec.sl2, satrec.sl3,
1792 satrec.sl4, satrec.t, satrec.xgh2,
1793 satrec.xgh3, satrec.xgh4, satrec.xh2,
1794 satrec.xh3, satrec.xi2, satrec.xi3,
1795 satrec.xl2, satrec.xl3, satrec.xl4,
1796 satrec.zmol, satrec.zmos, satrec.inclo,
1797 'n', ep, xincp, nodep, argpp, mp, satrec.operationmode);
1798 if (xincp < 0.0)
1799 {
1800 xincp = -xincp;
1801 nodep = nodep + M_PI;
1802 argpp = argpp - M_PI;
1803 }
1804 if ((ep < 0.0 ) || ( ep > 1.0))
1805 {
1806 // printf("# error ep %f\n", ep);
1807 satrec.error = 3;
1808 // sgp4fix add return
1809 return false;
1810 }
1811 } // if method = d
1812
1813 /* -------------------- long period periodics ------------------ */
1814 if (satrec.method == 'd')
1815 {
1816 sinip = std::sin(xincp);
1817 cosip = std::cos(xincp);
1818 satrec.aycof = -0.5*j3oj2*sinip;
1819 // sgp4fix for divide by zero for xincp = 180 deg
1820 if (std::fabs(cosip+1.0) > 1.5e-12)
1821 satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip);
1822 else
1823 satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4;
1824 }
1825 axnl = ep * std::cos(argpp);
1826 temp = 1.0 / (am * (1.0 - ep * ep));
1827 aynl = ep* std::sin(argpp) + temp * satrec.aycof;
1828 xl = mp + argpp + nodep + temp * satrec.xlcof * axnl;
1829
1830 /* --------------------- solve kepler's equation --------------- */
1831 u = std::fmod(xl - nodep, twopi);
1832 eo1 = u;
1833 tem5 = 9999.9;
1834 ktr = 1;
1835 // sgp4fix for kepler iteration
1836 // the following iteration needs better limits on corrections
1837 while (( std::fabs(tem5) >= 1.0e-12) && (ktr <= 10) )
1838 {
1839 sineo1 = std::sin(eo1);
1840 coseo1 = std::cos(eo1);
1841 tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl;
1842 tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5;
1843 if(std::fabs(tem5) >= 0.95)
1844 tem5 = tem5 > 0.0 ? 0.95 : -0.95;
1845 eo1 = eo1 + tem5;
1846 ktr = ktr + 1;
1847 }
1848
1849 /* ------------- short period preliminary quantities ----------- */
1850 ecose = axnl*coseo1 + aynl*sineo1;
1851 esine = axnl*sineo1 - aynl*coseo1;
1852 el2 = axnl*axnl + aynl*aynl;
1853 pl = am*(1.0-el2);
1854 if (pl < 0.0)
1855 {
1856 // printf("# error pl %f\n", pl);
1857 satrec.error = 4;
1858 // sgp4fix add return
1859 return false;
1860 }
1861 else
1862 {
1863 rl = am * (1.0 - ecose);
1864 rdotl = std::sqrt(am) * esine/rl;
1865 rvdotl = std::sqrt(pl) / rl;
1866 betal = std::sqrt(1.0 - el2);
1867 temp = esine / (1.0 + betal);
1868 sinu = am / rl * (sineo1 - aynl - axnl * temp);
1869 cosu = am / rl * (coseo1 - axnl + aynl * temp);
1870 su = std::atan2(sinu, cosu);
1871 sin2u = (cosu + cosu) * sinu;
1872 cos2u = 1.0 - 2.0 * sinu * sinu;
1873 temp = 1.0 / pl;
1874 temp1 = 0.5 * j2 * temp;
1875 temp2 = temp1 * temp;
1876
1877 /* -------------- update for short period periodics ------------ */
1878 if (satrec.method == 'd')
1879 {
1880 cosisq = cosip * cosip;
1881 satrec.con41 = 3.0*cosisq - 1.0;
1882 satrec.x1mth2 = 1.0 - cosisq;
1883 satrec.x7thm1 = 7.0*cosisq - 1.0;
1884 }
1885 mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) +
1886 0.5 * temp1 * satrec.x1mth2 * cos2u;
1887 su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u;
1888 xnode = nodep + 1.5 * temp2 * cosip * sin2u;
1889 xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u;
1890 mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke;
1891 rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + 1.5 * satrec.con41) / xke;
1892
1893 /* --------------------- orientation vectors ------------------- */
1894 sinsu = std::sin(su);
1895 cossu = std::cos(su);
1896 snod = std::sin(xnode);
1897 cnod = std::cos(xnode);
1898 sini = std::sin(xinc);
1899 cosi = std::cos(xinc);
1900 xmx = -snod * cosi;
1901 xmy = cnod * cosi;
1902 ux = xmx * sinsu + cnod * cossu;
1903 uy = xmy * sinsu + snod * cossu;
1904 uz = sini * sinsu;
1905 vx = xmx * cossu - cnod * sinsu;
1906 vy = xmy * cossu - snod * sinsu;
1907 vz = sini * cossu;
1908
1909 /* --------- position and velocity (in km and km/sec) ---------- */
1910 r[0] = (mrt * ux)* radiusearthkm;
1911 r[1] = (mrt * uy)* radiusearthkm;
1912 r[2] = (mrt * uz)* radiusearthkm;
1913 v[0] = (mvt * ux + rvdot * vx) * vkmpersec;
1914 v[1] = (mvt * uy + rvdot * vy) * vkmpersec;
1915 v[2] = (mvt * uz + rvdot * vz) * vkmpersec;
1916 } // if pl > 0
1917
1918 // sgp4fix for decaying satellites
1919 if (mrt < 1.0)
1920 {
1921 // printf("# decay condition %11.6f \n",mrt);
1922 satrec.error = 6;
1923 return false;
1924 }
1925
1926 return true;
1927 } // end sgp4
1928
1929
1930 /* -----------------------------------------------------------------------------
1931 *
1932 * function gstime
1933 *
1934 * this function finds the greenwich sidereal time.
1935 *
1936 * author : david vallado 719-573-2600 1 mar 2001
1937 *
1938 * inputs description range / units
1939 * jdut1 - julian date in ut1 days from 4713 bc
1940 *
1941 * outputs :
1942 * gstime - greenwich sidereal time 0 to 2pi rad
1943 *
1944 * locals :
1945 * temp - temporary variable for doubles rad
1946 * tut1 - julian centuries from the
1947 * jan 1, 2000 12 h epoch (ut1)
1948 *
1949 * coupling :
1950 * none
1951 *
1952 * references :
1953 * vallado 2004, 191, eq 3-45
1954 * --------------------------------------------------------------------------- */
1955
gstime(double jdut1)1956 double gstime(double jdut1)
1957 {
1958 const double twopi = 2.0 * M_PI;
1959 const double deg2rad = M_PI / 180.0;
1960 double temp, tut1;
1961
1962 tut1 = (jdut1 - 2451545.0) / 36525.0;
1963 temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 +
1964 (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec
1965 temp = std::fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad
1966
1967 // ------------------------ check quadrants ---------------------
1968 if (temp < 0.0)
1969 temp += twopi;
1970
1971 return temp;
1972 } // end gstime
1973
1974 /* -----------------------------------------------------------------------------
1975 *
1976 * function getgravconst
1977 *
1978 * this function gets constants for the propagator. note that mu is identified to
1979 * facilitiate comparisons with newer models. the common useage is wgs72.
1980 *
1981 * author : david vallado 719-573-2600 21 jul 2006
1982 *
1983 * inputs :
1984 * whichconst - which set of constants to use wgs72old, wgs72, wgs84
1985 *
1986 * outputs :
1987 * tumin - minutes in one time unit
1988 * mu - earth gravitational parameter
1989 * radiusearthkm - radius of the earth in km
1990 * xke - reciprocal of tumin
1991 * j2, j3, j4 - un-normalized zonal harmonic values
1992 * j3oj2 - j3 divided by j2
1993 *
1994 * locals :
1995 *
1996 * coupling :
1997 * none
1998 *
1999 * references :
2000 * norad spacetrack report #3
2001 * vallado, crawford, hujsak, kelso 2006
2002 --------------------------------------------------------------------------- */
2003
getgravconst(gravconsttype whichconst,double & tumin,double & mu,double & radiusearthkm,double & xke,double & j2,double & j3,double & j4,double & j3oj2)2004 void getgravconst(gravconsttype whichconst, double& tumin, double& mu, double& radiusearthkm,
2005 double& xke, double& j2, double& j3, double& j4, double& j3oj2)
2006 {
2007 switch (whichconst)
2008 {
2009 case wgs72old:
2010 // -- wgs-72 low precision str#3 constants --
2011 mu = 398600.79964; // in km3 / s2
2012 radiusearthkm = 6378.135; // km
2013 xke = 0.0743669161;
2014 tumin = 1.0 / xke;
2015 j2 = 0.001082616;
2016 j3 = -0.00000253881;
2017 j4 = -0.00000165597;
2018 j3oj2 = j3 / j2;
2019 break;
2020 case wgs72:
2021 // ------------ wgs-72 constants ------------
2022 mu = 398600.8; // in km3 / s2
2023 radiusearthkm = 6378.135; // km
2024 xke = 60.0 / std::sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu);
2025 tumin = 1.0 / xke;
2026 j2 = 0.001082616;
2027 j3 = -0.00000253881;
2028 j4 = -0.00000165597;
2029 j3oj2 = j3 / j2;
2030 break;
2031 case wgs84:
2032 // ------------ wgs-84 constants ------------
2033 mu = 398600.5; // in km3 / s2
2034 radiusearthkm = 6378.137; // km
2035 xke = 60.0 / std::sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu);
2036 tumin = 1.0 / xke;
2037 j2 = 0.00108262998905;
2038 j3 = -0.00000253215306;
2039 j4 = -0.00000161098761;
2040 j3oj2 = j3 / j2;
2041 break;
2042 default:
2043 fprintf(stderr,"unknown gravity option (%d)\n",whichconst);
2044 break;
2045 }
2046 } // end getgravconst
2047
2048
2049
2050
2051
2052