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