File indexing completed on 2024-04-14 03:46:45
0001 /* ---------------------------------------------------------------- 0002 * 0003 * sgp4unit.cpp 0004 * 0005 * this file contains the sgp4 procedures for analytical propagation 0006 * of a satellite. the code was originally released in the 1980 and 1986 0007 * spacetrack papers. a detailed discussion of the theory and history 0008 * may be found in the 2006 aiaa paper by vallado, crawford, hujsak, 0009 * and kelso. 0010 * 0011 * companion code for 0012 * fundamentals of astrodynamics and applications 0013 * 2007 0014 * by david vallado 0015 * 0016 * (w) 719-573-2600, email dvallado@agi.com 0017 * 0018 * current : 0019 * 3 Nov 08 david vallado 0020 * put returns in for error codes 0021 * changes : 0022 * 29 sep 08 david vallado 0023 * fix atime for faster operation in dspace 0024 * add operationmode for afspc (a) or improved (i) 0025 * performance mode 0026 * 16 jun 08 david vallado 0027 * update small eccentricity check 0028 * 16 nov 07 david vallado 0029 * misc fixes for better compliance 0030 * 20 apr 07 david vallado 0031 * misc fixes for constants 0032 * 11 aug 06 david vallado 0033 * chg lyddane choice back to strn3, constants, misc doc 0034 * 15 dec 05 david vallado 0035 * misc fixes 0036 * 26 jul 05 david vallado 0037 * fixes for paper 0038 * note that each fix is preceded by a 0039 * comment with "sgp4fix" and an explanation of 0040 * what was changed 0041 * 10 aug 04 david vallado 0042 * 2nd printing baseline working 0043 * 14 may 01 david vallado 0044 * 2nd edition baseline 0045 * 80 norad 0046 * original baseline 0047 * ---------------------------------------------------------------- */ 0048 0049 #include "sgp4unit.h" 0050 0051 FILE *dbgfile; 0052 #define UNUSED(arg) (void)arg; 0053 0054 /* ----------- local functions - only ever used internally by sgp4 ---------- */ 0055 static void dpper 0056 ( 0057 double e3, double ee2, double peo, double pgho, double pho, 0058 double pinco, double plo, double se2, double se3, double sgh2, 0059 double sgh3, double sgh4, double sh2, double sh3, double si2, 0060 double si3, double sl2, double sl3, double sl4, double t, 0061 double xgh2, double xgh3, double xgh4, double xh2, double xh3, 0062 double xi2, double xi3, double xl2, double xl3, double xl4, 0063 double zmol, double zmos, double inclo, 0064 char init, 0065 double& ep, double& inclp, double& nodep, double& argpp, double& mp, 0066 char opsmode 0067 ); 0068 0069 static void dscom 0070 ( 0071 double epoch, double ep, double argpp, double tc, double inclp, 0072 double nodep, double np, 0073 double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, 0074 double& cosomm,double& day, double& e3, double& ee2, double& em, 0075 double& emsq, double& gam, double& peo, double& pgho, double& pho, 0076 double& pinco, double& plo, double& rtemsq, double& se2, double& se3, 0077 double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, 0078 double& si2, double& si3, double& sl2, double& sl3, double& sl4, 0079 double& s1, double& s2, double& s3, double& s4, double& s5, 0080 double& s6, double& s7, double& ss1, double& ss2, double& ss3, 0081 double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, 0082 double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, 0083 double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, 0084 double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, 0085 double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, 0086 double& xl4, double& nm, double& z1, double& z2, double& z3, 0087 double& z11, double& z12, double& z13, double& z21, double& z22, 0088 double& z23, double& z31, double& z32, double& z33, double& zmol, 0089 double& zmos 0090 ); 0091 0092 static void dsinit 0093 ( 0094 gravconsttype whichconst, 0095 double cosim, double emsq, double argpo, double s1, double s2, 0096 double s3, double s4, double s5, double sinim, double ss1, 0097 double ss2, double ss3, double ss4, double ss5, double sz1, 0098 double sz3, double sz11, double sz13, double sz21, double sz23, 0099 double sz31, double sz33, double t, double tc, double gsto, 0100 double mo, double mdot, double no, double nodeo, double nodedot, 0101 double xpidot, double z1, double z3, double z11, double z13, 0102 double z21, double z23, double z31, double z33, double ecco, 0103 double eccsq, double& em, double& argpm, double& inclm, double& mm, 0104 double& nm, double& nodem, 0105 int& irez, 0106 double& atime, double& d2201, double& d2211, double& d3210, double& d3222, 0107 double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, 0108 double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, 0109 double& dnodt, double& domdt, double& del1, double& del2, double& del3, 0110 double& xfact, double& xlamo, double& xli, double& xni 0111 ); 0112 0113 static void dspace 0114 ( 0115 int irez, 0116 double d2201, double d2211, double d3210, double d3222, double d4410, 0117 double d4422, double d5220, double d5232, double d5421, double d5433, 0118 double dedt, double del1, double del2, double del3, double didt, 0119 double dmdt, double dnodt, double domdt, double argpo, double argpdot, 0120 double t, double tc, double gsto, double xfact, double xlamo, 0121 double no, 0122 double& atime, double& em, double& argpm, double& inclm, double& xli, 0123 double& mm, double& xni, double& nodem, double& dndt, double& nm 0124 ); 0125 0126 static void initl 0127 ( 0128 int satn, gravconsttype whichconst, 0129 double ecco, double epoch, double inclo, double& no, 0130 char& method, 0131 double& ainv, double& ao, double& con41, double& con42, double& cosio, 0132 double& cosio2,double& eccsq, double& omeosq, double& posq, 0133 double& rp, double& rteosq,double& sinio , double& gsto, char opsmode 0134 ); 0135 0136 /* ----------------------------------------------------------------------------- 0137 * 0138 * procedure dpper 0139 * 0140 * this procedure provides deep space long period periodic contributions 0141 * to the mean elements. by design, these periodics are zero at epoch. 0142 * this used to be dscom which included initialization, but it's really a 0143 * recurring function. 0144 * 0145 * author : david vallado 719-573-2600 28 jun 2005 0146 * 0147 * inputs : 0148 * e3 - 0149 * ee2 - 0150 * peo - 0151 * pgho - 0152 * pho - 0153 * pinco - 0154 * plo - 0155 * se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - 0156 * t - 0157 * xh2, xh3, xi2, xi3, xl2, xl3, xl4 - 0158 * zmol - 0159 * zmos - 0160 * ep - eccentricity 0.0 - 1.0 0161 * inclo - inclination - needed for lyddane modification 0162 * nodep - right ascension of ascending node 0163 * argpp - argument of perigee 0164 * mp - mean anomaly 0165 * 0166 * outputs : 0167 * ep - eccentricity 0.0 - 1.0 0168 * inclp - inclination 0169 * nodep - right ascension of ascending node 0170 * argpp - argument of perigee 0171 * mp - mean anomaly 0172 * 0173 * locals : 0174 * alfdp - 0175 * betdp - 0176 * cosip , sinip , cosop , sinop , 0177 * dalf - 0178 * dbet - 0179 * dls - 0180 * f2, f3 - 0181 * pe - 0182 * pgh - 0183 * ph - 0184 * pinc - 0185 * pl - 0186 * sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , 0187 * sll , sls 0188 * xls - 0189 * xnoh - 0190 * zf - 0191 * zm - 0192 * 0193 * coupling : 0194 * none. 0195 * 0196 * references : 0197 * hoots, roehrich, norad spacetrack report #3 1980 0198 * hoots, norad spacetrack report #6 1986 0199 * hoots, schumacher and glover 2004 0200 * vallado, crawford, hujsak, kelso 2006 0201 ----------------------------------------------------------------------------*/ 0202 0203 static void dpper 0204 ( 0205 double e3, double ee2, double peo, double pgho, double pho, 0206 double pinco, double plo, double se2, double se3, double sgh2, 0207 double sgh3, double sgh4, double sh2, double sh3, double si2, 0208 double si3, double sl2, double sl3, double sl4, double t, 0209 double xgh2, double xgh3, double xgh4, double xh2, double xh3, 0210 double xi2, double xi3, double xl2, double xl3, double xl4, 0211 double zmol, double zmos, double inclo, 0212 char init, 0213 double& ep, double& inclp, double& nodep, double& argpp, double& mp, 0214 char opsmode 0215 ) 0216 { 0217 UNUSED( inclo ); 0218 /* --------------------- local variables ------------------------ */ 0219 const double twopi = 2.0 * M_PI; 0220 double alfdp, betdp, cosip, cosop, dalf, dbet, dls, 0221 f2, f3, pe, pgh, ph, pinc, pl , 0222 sel, ses, sghl, sghs, shll, shs, sil, 0223 sinip, sinop, sinzf, sis, sll, sls, xls, 0224 xnoh, zf, zm, zel, zes, znl, zns; 0225 0226 /* ---------------------- constants ----------------------------- */ 0227 zns = 1.19459e-5; 0228 zes = 0.01675; 0229 znl = 1.5835218e-4; 0230 zel = 0.05490; 0231 0232 /* --------------- calculate time varying periodics ----------- */ 0233 zm = zmos + zns * t; 0234 // be sure that the initial call has time set to zero 0235 if (init == 'y') 0236 zm = zmos; 0237 zf = zm + 2.0 * zes * sin(zm); 0238 sinzf = sin(zf); 0239 f2 = 0.5 * sinzf * sinzf - 0.25; 0240 f3 = -0.5 * sinzf * cos(zf); 0241 ses = se2* f2 + se3 * f3; 0242 sis = si2 * f2 + si3 * f3; 0243 sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; 0244 sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; 0245 shs = sh2 * f2 + sh3 * f3; 0246 zm = zmol + znl * t; 0247 if (init == 'y') 0248 zm = zmol; 0249 zf = zm + 2.0 * zel * sin(zm); 0250 sinzf = sin(zf); 0251 f2 = 0.5 * sinzf * sinzf - 0.25; 0252 f3 = -0.5 * sinzf * cos(zf); 0253 sel = ee2 * f2 + e3 * f3; 0254 sil = xi2 * f2 + xi3 * f3; 0255 sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; 0256 sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; 0257 shll = xh2 * f2 + xh3 * f3; 0258 pe = ses + sel; 0259 pinc = sis + sil; 0260 pl = sls + sll; 0261 pgh = sghs + sghl; 0262 ph = shs + shll; 0263 0264 if (init == 'n') 0265 { 0266 pe = pe - peo; 0267 pinc = pinc - pinco; 0268 pl = pl - plo; 0269 pgh = pgh - pgho; 0270 ph = ph - pho; 0271 inclp = inclp + pinc; 0272 ep = ep + pe; 0273 sinip = sin(inclp); 0274 cosip = cos(inclp); 0275 0276 /* ----------------- apply periodics directly ------------ */ 0277 // sgp4fix for lyddane choice 0278 // strn3 used original inclination - this is technically feasible 0279 // gsfc used perturbed inclination - also technically feasible 0280 // probably best to readjust the 0.2 limit value and limit discontinuity 0281 // 0.2 rad = 11.45916 deg 0282 // use next line for original strn3 approach and original inclination 0283 // if (inclo >= 0.2) 0284 // use next line for gsfc version and perturbed inclination 0285 if (inclp >= 0.2) 0286 { 0287 ph = ph / sinip; 0288 pgh = pgh - cosip * ph; 0289 argpp = argpp + pgh; 0290 nodep = nodep + ph; 0291 mp = mp + pl; 0292 } 0293 else 0294 { 0295 /* ---- apply periodics with lyddane modification ---- */ 0296 sinop = sin(nodep); 0297 cosop = cos(nodep); 0298 alfdp = sinip * sinop; 0299 betdp = sinip * cosop; 0300 dalf = ph * cosop + pinc * cosip * sinop; 0301 dbet = -ph * sinop + pinc * cosip * cosop; 0302 alfdp = alfdp + dalf; 0303 betdp = betdp + dbet; 0304 nodep = fmod(nodep, twopi); 0305 // sgp4fix for afspc written intrinsic functions 0306 // nodep used without a trigonometric function ahead 0307 if ((nodep < 0.0) && (opsmode == 'a')) { 0308 nodep = nodep + twopi; 0309 } 0310 xls = mp + argpp + cosip * nodep; 0311 dls = pl + pgh - pinc * nodep * sinip; 0312 xls = xls + dls; 0313 xnoh = nodep; 0314 nodep = atan2(alfdp, betdp); 0315 // sgp4fix for afspc written intrinsic functions 0316 // nodep used without a trigonometric function ahead 0317 if ((nodep < 0.0) && (opsmode == 'a')) { 0318 nodep = nodep + twopi; 0319 } 0320 if (fabs(xnoh - nodep) > M_PI) { 0321 if (nodep < xnoh) { 0322 nodep = nodep + twopi; 0323 } 0324 else { 0325 nodep = nodep - twopi; 0326 } 0327 } 0328 mp = mp + pl; 0329 argpp = xls - mp - cosip * nodep; 0330 } 0331 } // if init == 'n' 0332 0333 //#include "debug1.cpp" 0334 } // end dpper 0335 0336 /*----------------------------------------------------------------------------- 0337 * 0338 * procedure dscom 0339 * 0340 * this procedure provides deep space common items used by both the secular 0341 * and periodics subroutines. input is provided as shown. this routine 0342 * used to be called dpper, but the functions inside weren't well organized. 0343 * 0344 * author : david vallado 719-573-2600 28 jun 2005 0345 * 0346 * inputs : 0347 * epoch - 0348 * ep - eccentricity 0349 * argpp - argument of perigee 0350 * tc - 0351 * inclp - inclination 0352 * nodep - right ascension of ascending node 0353 * np - mean motion 0354 * 0355 * outputs : 0356 * sinim , cosim , sinomm , cosomm , snodm , cnodm 0357 * day - 0358 * e3 - 0359 * ee2 - 0360 * em - eccentricity 0361 * emsq - eccentricity squared 0362 * gam - 0363 * peo - 0364 * pgho - 0365 * pho - 0366 * pinco - 0367 * plo - 0368 * rtemsq - 0369 * se2, se3 - 0370 * sgh2, sgh3, sgh4 - 0371 * sh2, sh3, si2, si3, sl2, sl3, sl4 - 0372 * s1, s2, s3, s4, s5, s6, s7 - 0373 * ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - 0374 * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - 0375 * xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - 0376 * nm - mean motion 0377 * z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - 0378 * zmol - 0379 * zmos - 0380 * 0381 * locals : 0382 * a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - 0383 * betasq - 0384 * cc - 0385 * ctem, stem - 0386 * x1, x2, x3, x4, x5, x6, x7, x8 - 0387 * xnodce - 0388 * xnoi - 0389 * zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , 0390 * zcosi , zsini , zcosil , zsinil , 0391 * zx - 0392 * zy - 0393 * 0394 * coupling : 0395 * none. 0396 * 0397 * references : 0398 * hoots, roehrich, norad spacetrack report #3 1980 0399 * hoots, norad spacetrack report #6 1986 0400 * hoots, schumacher and glover 2004 0401 * vallado, crawford, hujsak, kelso 2006 0402 ----------------------------------------------------------------------------*/ 0403 0404 static void dscom 0405 ( 0406 double epoch, double ep, double argpp, double tc, double inclp, 0407 double nodep, double np, 0408 double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, 0409 double& cosomm,double& day, double& e3, double& ee2, double& em, 0410 double& emsq, double& gam, double& peo, double& pgho, double& pho, 0411 double& pinco, double& plo, double& rtemsq, double& se2, double& se3, 0412 double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, 0413 double& si2, double& si3, double& sl2, double& sl3, double& sl4, 0414 double& s1, double& s2, double& s3, double& s4, double& s5, 0415 double& s6, double& s7, double& ss1, double& ss2, double& ss3, 0416 double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, 0417 double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, 0418 double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, 0419 double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, 0420 double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, 0421 double& xl4, double& nm, double& z1, double& z2, double& z3, 0422 double& z11, double& z12, double& z13, double& z21, double& z22, 0423 double& z23, double& z31, double& z32, double& z33, double& zmol, 0424 double& zmos 0425 ) 0426 { 0427 /* -------------------------- constants ------------------------- */ 0428 const double zes = 0.01675; 0429 const double zel = 0.05490; 0430 const double c1ss = 2.9864797e-6; 0431 const double c1l = 4.7968065e-7; 0432 const double zsinis = 0.39785416; 0433 const double zcosis = 0.91744867; 0434 const double zcosgs = 0.1945905; 0435 const double zsings = -0.98088458; 0436 const double twopi = 2.0 * M_PI; 0437 0438 /* --------------------- local variables ------------------------ */ 0439 int lsflg; 0440 double a1 , a2 , a3 , a4 , a5 , a6 , a7 , 0441 a8 , a9 , a10 , betasq, cc , ctem , stem , 0442 x1 , x2 , x3 , x4 , x5 , x6 , x7 , 0443 x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, 0444 zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , 0445 zsinil, zx , zy; 0446 0447 nm = np; 0448 em = ep; 0449 snodm = sin(nodep); 0450 cnodm = cos(nodep); 0451 sinomm = sin(argpp); 0452 cosomm = cos(argpp); 0453 sinim = sin(inclp); 0454 cosim = cos(inclp); 0455 emsq = em * em; 0456 betasq = 1.0 - emsq; 0457 rtemsq = sqrt(betasq); 0458 0459 /* ----------------- initialize lunar solar terms --------------- */ 0460 peo = 0.0; 0461 pinco = 0.0; 0462 plo = 0.0; 0463 pgho = 0.0; 0464 pho = 0.0; 0465 day = epoch + 18261.5 + tc / 1440.0; 0466 xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); 0467 stem = sin(xnodce); 0468 ctem = cos(xnodce); 0469 zcosil = 0.91375164 - 0.03568096 * ctem; 0470 zsinil = sqrt(1.0 - zcosil * zcosil); 0471 zsinhl = 0.089683511 * stem / zsinil; 0472 zcoshl = sqrt(1.0 - zsinhl * zsinhl); 0473 gam = 5.8351514 + 0.0019443680 * day; 0474 zx = 0.39785416 * stem / zsinil; 0475 zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; 0476 zx = atan2(zx, zy); 0477 zx = gam + zx - xnodce; 0478 zcosgl = cos(zx); 0479 zsingl = sin(zx); 0480 0481 /* ------------------------- do solar terms --------------------- */ 0482 zcosg = zcosgs; 0483 zsing = zsings; 0484 zcosi = zcosis; 0485 zsini = zsinis; 0486 zcosh = cnodm; 0487 zsinh = snodm; 0488 cc = c1ss; 0489 xnoi = 1.0 / nm; 0490 0491 for (lsflg = 1; lsflg <= 2; lsflg++) 0492 { 0493 a1 = zcosg * zcosh + zsing * zcosi * zsinh; 0494 a3 = -zsing * zcosh + zcosg * zcosi * zsinh; 0495 a7 = -zcosg * zsinh + zsing * zcosi * zcosh; 0496 a8 = zsing * zsini; 0497 a9 = zsing * zsinh + zcosg * zcosi * zcosh; 0498 a10 = zcosg * zsini; 0499 a2 = cosim * a7 + sinim * a8; 0500 a4 = cosim * a9 + sinim * a10; 0501 a5 = -sinim * a7 + cosim * a8; 0502 a6 = -sinim * a9 + cosim * a10; 0503 0504 x1 = a1 * cosomm + a2 * sinomm; 0505 x2 = a3 * cosomm + a4 * sinomm; 0506 x3 = -a1 * sinomm + a2 * cosomm; 0507 x4 = -a3 * sinomm + a4 * cosomm; 0508 x5 = a5 * sinomm; 0509 x6 = a6 * sinomm; 0510 x7 = a5 * cosomm; 0511 x8 = a6 * cosomm; 0512 0513 z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; 0514 z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; 0515 z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; 0516 z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; 0517 z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; 0518 z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; 0519 z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); 0520 z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * 0521 (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); 0522 z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); 0523 z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); 0524 z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * 0525 (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); 0526 z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); 0527 z1 = z1 + z1 + betasq * z31; 0528 z2 = z2 + z2 + betasq * z32; 0529 z3 = z3 + z3 + betasq * z33; 0530 s3 = cc * xnoi; 0531 s2 = -0.5 * s3 / rtemsq; 0532 s4 = s3 * rtemsq; 0533 s1 = -15.0 * em * s4; 0534 s5 = x1 * x3 + x2 * x4; 0535 s6 = x2 * x3 + x1 * x4; 0536 s7 = x2 * x4 - x1 * x3; 0537 0538 /* ----------------------- do lunar terms ------------------- */ 0539 if (lsflg == 1) 0540 { 0541 ss1 = s1; 0542 ss2 = s2; 0543 ss3 = s3; 0544 ss4 = s4; 0545 ss5 = s5; 0546 ss6 = s6; 0547 ss7 = s7; 0548 sz1 = z1; 0549 sz2 = z2; 0550 sz3 = z3; 0551 sz11 = z11; 0552 sz12 = z12; 0553 sz13 = z13; 0554 sz21 = z21; 0555 sz22 = z22; 0556 sz23 = z23; 0557 sz31 = z31; 0558 sz32 = z32; 0559 sz33 = z33; 0560 zcosg = zcosgl; 0561 zsing = zsingl; 0562 zcosi = zcosil; 0563 zsini = zsinil; 0564 zcosh = zcoshl * cnodm + zsinhl * snodm; 0565 zsinh = snodm * zcoshl - cnodm * zsinhl; 0566 cc = c1l; 0567 } 0568 } 0569 0570 zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); 0571 zmos = fmod(6.2565837 + 0.017201977 * day, twopi); 0572 0573 /* ------------------------ do solar terms ---------------------- */ 0574 se2 = 2.0 * ss1 * ss6; 0575 se3 = 2.0 * ss1 * ss7; 0576 si2 = 2.0 * ss2 * sz12; 0577 si3 = 2.0 * ss2 * (sz13 - sz11); 0578 sl2 = -2.0 * ss3 * sz2; 0579 sl3 = -2.0 * ss3 * (sz3 - sz1); 0580 sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; 0581 sgh2 = 2.0 * ss4 * sz32; 0582 sgh3 = 2.0 * ss4 * (sz33 - sz31); 0583 sgh4 = -18.0 * ss4 * zes; 0584 sh2 = -2.0 * ss2 * sz22; 0585 sh3 = -2.0 * ss2 * (sz23 - sz21); 0586 0587 /* ------------------------ do lunar terms ---------------------- */ 0588 ee2 = 2.0 * s1 * s6; 0589 e3 = 2.0 * s1 * s7; 0590 xi2 = 2.0 * s2 * z12; 0591 xi3 = 2.0 * s2 * (z13 - z11); 0592 xl2 = -2.0 * s3 * z2; 0593 xl3 = -2.0 * s3 * (z3 - z1); 0594 xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; 0595 xgh2 = 2.0 * s4 * z32; 0596 xgh3 = 2.0 * s4 * (z33 - z31); 0597 xgh4 = -18.0 * s4 * zel; 0598 xh2 = -2.0 * s2 * z22; 0599 xh3 = -2.0 * s2 * (z23 - z21); 0600 0601 //#include "debug2.cpp" 0602 } // end dscom 0603 0604 /*----------------------------------------------------------------------------- 0605 * 0606 * procedure dsinit 0607 * 0608 * this procedure provides deep space contributions to mean motion dot due 0609 * to geopotential resonance with half day and one day orbits. 0610 * 0611 * author : david vallado 719-573-2600 28 jun 2005 0612 * 0613 * inputs : 0614 * cosim, sinim- 0615 * emsq - eccentricity squared 0616 * argpo - argument of perigee 0617 * s1, s2, s3, s4, s5 - 0618 * ss1, ss2, ss3, ss4, ss5 - 0619 * sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - 0620 * t - time 0621 * tc - 0622 * gsto - greenwich sidereal time rad 0623 * mo - mean anomaly 0624 * mdot - mean anomaly dot (rate) 0625 * no - mean motion 0626 * nodeo - right ascension of ascending node 0627 * nodedot - right ascension of ascending node dot (rate) 0628 * xpidot - 0629 * z1, z3, z11, z13, z21, z23, z31, z33 - 0630 * eccm - eccentricity 0631 * argpm - argument of perigee 0632 * inclm - inclination 0633 * mm - mean anomaly 0634 * xn - mean motion 0635 * nodem - right ascension of ascending node 0636 * 0637 * outputs : 0638 * em - eccentricity 0639 * argpm - argument of perigee 0640 * inclm - inclination 0641 * mm - mean anomaly 0642 * nm - mean motion 0643 * nodem - right ascension of ascending node 0644 * irez - flag for resonance 0-none, 1-one day, 2-half day 0645 * atime - 0646 * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - 0647 * dedt - 0648 * didt - 0649 * dmdt - 0650 * dndt - 0651 * dnodt - 0652 * domdt - 0653 * del1, del2, del3 - 0654 * ses , sghl , sghs , sgs , shl , shs , sis , sls 0655 * theta - 0656 * xfact - 0657 * xlamo - 0658 * xli - 0659 * xni 0660 * 0661 * locals : 0662 * ainv2 - 0663 * aonv - 0664 * cosisq - 0665 * eoc - 0666 * f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - 0667 * g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - 0668 * sini2 - 0669 * temp - 0670 * temp1 - 0671 * theta - 0672 * xno2 - 0673 * 0674 * coupling : 0675 * getgravconst 0676 * 0677 * references : 0678 * hoots, roehrich, norad spacetrack report #3 1980 0679 * hoots, norad spacetrack report #6 1986 0680 * hoots, schumacher and glover 2004 0681 * vallado, crawford, hujsak, kelso 2006 0682 ----------------------------------------------------------------------------*/ 0683 0684 static void dsinit 0685 ( 0686 gravconsttype whichconst, 0687 double cosim, double emsq, double argpo, double s1, double s2, 0688 double s3, double s4, double s5, double sinim, double ss1, 0689 double ss2, double ss3, double ss4, double ss5, double sz1, 0690 double sz3, double sz11, double sz13, double sz21, double sz23, 0691 double sz31, double sz33, double t, double tc, double gsto, 0692 double mo, double mdot, double no, double nodeo, double nodedot, 0693 double xpidot, double z1, double z3, double z11, double z13, 0694 double z21, double z23, double z31, double z33, double ecco, 0695 double eccsq, double& em, double& argpm, double& inclm, double& mm, 0696 double& nm, double& nodem, 0697 int& irez, 0698 double& atime, double& d2201, double& d2211, double& d3210, double& d3222, 0699 double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, 0700 double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, 0701 double& dnodt, double& domdt, double& del1, double& del2, double& del3, 0702 double& xfact, double& xlamo, double& xli, double& xni 0703 ) 0704 { 0705 /* --------------------- local variables ------------------------ */ 0706 const double twopi = 2.0 * M_PI; 0707 0708 double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , 0709 f321 , f322 , f330 , f441 , f442 , f522 , f523 , 0710 f542 , f543 , g200 , g201 , g211 , g300 , g310 , 0711 g322 , g410 , g422 , g520 , g521 , g532 , g533 , 0712 ses , sgs , sghl , sghs , shs , shll , sis , 0713 sini2 , sls , temp , temp1 , theta , xno2 , q22 , 0714 q31 , q33 , root22, root44, root54, rptim , root32, 0715 root52, x2o3 , xke , znl , emo , zns , emsqo, 0716 tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; 0717 0718 q22 = 1.7891679e-6; 0719 q31 = 2.1460748e-6; 0720 q33 = 2.2123015e-7; 0721 root22 = 1.7891679e-6; 0722 root44 = 7.3636953e-9; 0723 root54 = 2.1765803e-9; 0724 rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec 0725 root32 = 3.7393792e-7; 0726 root52 = 1.1428639e-7; 0727 x2o3 = 2.0 / 3.0; 0728 znl = 1.5835218e-4; 0729 zns = 1.19459e-5; 0730 0731 // sgp4fix identify constants and allow alternate values 0732 getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 0733 0734 /* -------------------- deep space initialization ------------ */ 0735 irez = 0; 0736 if ((nm < 0.0052359877) && (nm > 0.0034906585)) 0737 irez = 1; 0738 if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) 0739 irez = 2; 0740 0741 /* ------------------------ do solar terms ------------------- */ 0742 ses = ss1 * zns * ss5; 0743 sis = ss2 * zns * (sz11 + sz13); 0744 sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); 0745 sghs = ss4 * zns * (sz31 + sz33 - 6.0); 0746 shs = -zns * ss2 * (sz21 + sz23); 0747 // sgp4fix for 180 deg incl 0748 if ((inclm < 5.2359877e-2) || (inclm > M_PI - 5.2359877e-2)) 0749 shs = 0.0; 0750 if (sinim != 0.0) 0751 shs = shs / sinim; 0752 sgs = sghs - cosim * shs; 0753 0754 /* ------------------------- do lunar terms ------------------ */ 0755 dedt = ses + s1 * znl * s5; 0756 didt = sis + s2 * znl * (z11 + z13); 0757 dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); 0758 sghl = s4 * znl * (z31 + z33 - 6.0); 0759 shll = -znl * s2 * (z21 + z23); 0760 // sgp4fix for 180 deg incl 0761 if ((inclm < 5.2359877e-2) || (inclm > M_PI - 5.2359877e-2)) 0762 shll = 0.0; 0763 domdt = sgs + sghl; 0764 dnodt = shs; 0765 if (sinim != 0.0) 0766 { 0767 domdt = domdt - cosim / sinim * shll; 0768 dnodt = dnodt + shll / sinim; 0769 } 0770 0771 /* ----------- calculate deep space resonance effects -------- */ 0772 dndt = 0.0; 0773 theta = fmod(gsto + tc * rptim, twopi); 0774 em = em + dedt * t; 0775 inclm = inclm + didt * t; 0776 argpm = argpm + domdt * t; 0777 nodem = nodem + dnodt * t; 0778 mm = mm + dmdt * t; 0779 // sgp4fix for negative inclinations 0780 // the following if statement should be commented out 0781 //if (inclm < 0.0) 0782 // { 0783 // inclm = -inclm; 0784 // argpm = argpm - pi; 0785 // nodem = nodem + pi; 0786 // } 0787 0788 /* -------------- initialize the resonance terms ------------- */ 0789 if (irez != 0) 0790 { 0791 aonv = pow(nm / xke, x2o3); 0792 0793 /* ---------- geopotential resonance for 12 hour orbits ------ */ 0794 if (irez == 2) 0795 { 0796 cosisq = cosim * cosim; 0797 emo = em; 0798 em = ecco; 0799 emsqo = emsq; 0800 emsq = eccsq; 0801 eoc = em * emsq; 0802 g201 = -0.306 - (em - 0.64) * 0.440; 0803 0804 if (em <= 0.65) 0805 { 0806 g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; 0807 g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; 0808 g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; 0809 g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; 0810 g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; 0811 g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; 0812 } 0813 else 0814 { 0815 g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; 0816 g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; 0817 g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; 0818 g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; 0819 g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; 0820 if (em > 0.715) 0821 g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; 0822 else 0823 g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; 0824 } 0825 if (em < 0.7) 0826 { 0827 g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; 0828 g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; 0829 g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; 0830 } 0831 else 0832 { 0833 g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; 0834 g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; 0835 g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; 0836 } 0837 0838 sini2= sinim * sinim; 0839 f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); 0840 f221 = 1.5 * sini2; 0841 f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); 0842 f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); 0843 f441 = 35.0 * sini2 * f220; 0844 f442 = 39.3750 * sini2 * sini2; 0845 f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + 0846 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); 0847 f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + 0848 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); 0849 f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * 0850 (-12.0 + 8.0 * cosim + 10.0 * cosisq)); 0851 f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * 0852 (12.0 + 8.0 * cosim - 10.0 * cosisq)); 0853 xno2 = nm * nm; 0854 ainv2 = aonv * aonv; 0855 temp1 = 3.0 * xno2 * ainv2; 0856 temp = temp1 * root22; 0857 d2201 = temp * f220 * g201; 0858 d2211 = temp * f221 * g211; 0859 temp1 = temp1 * aonv; 0860 temp = temp1 * root32; 0861 d3210 = temp * f321 * g310; 0862 d3222 = temp * f322 * g322; 0863 temp1 = temp1 * aonv; 0864 temp = 2.0 * temp1 * root44; 0865 d4410 = temp * f441 * g410; 0866 d4422 = temp * f442 * g422; 0867 temp1 = temp1 * aonv; 0868 temp = temp1 * root52; 0869 d5220 = temp * f522 * g520; 0870 d5232 = temp * f523 * g532; 0871 temp = 2.0 * temp1 * root54; 0872 d5421 = temp * f542 * g521; 0873 d5433 = temp * f543 * g533; 0874 xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi); 0875 xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; 0876 em = emo; 0877 emsq = emsqo; 0878 } 0879 0880 /* ---------------- synchronous resonance terms -------------- */ 0881 if (irez == 1) 0882 { 0883 g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); 0884 g310 = 1.0 + 2.0 * emsq; 0885 g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); 0886 f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); 0887 f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); 0888 f330 = 1.0 + cosim; 0889 f330 = 1.875 * f330 * f330 * f330; 0890 del1 = 3.0 * nm * nm * aonv * aonv; 0891 del2 = 2.0 * del1 * f220 * g200 * q22; 0892 del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; 0893 del1 = del1 * f311 * g310 * q31 * aonv; 0894 xlamo = fmod(mo + nodeo + argpo - theta, twopi); 0895 xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; 0896 } 0897 0898 /* ------------ for sgp4, initialize the integrator ---------- */ 0899 xli = xlamo; 0900 xni = no; 0901 atime = 0.0; 0902 nm = no + dndt; 0903 } 0904 0905 //#include "debug3.cpp" 0906 } // end dsinit 0907 0908 /*----------------------------------------------------------------------------- 0909 * 0910 * procedure dspace 0911 * 0912 * this procedure provides deep space contributions to mean elements for 0913 * perturbing third body. these effects have been averaged over one 0914 * revolution of the sun and moon. for earth resonance effects, the 0915 * effects have been averaged over no revolutions of the satellite. 0916 * (mean motion) 0917 * 0918 * author : david vallado 719-573-2600 28 jun 2005 0919 * 0920 * inputs : 0921 * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - 0922 * dedt - 0923 * del1, del2, del3 - 0924 * didt - 0925 * dmdt - 0926 * dnodt - 0927 * domdt - 0928 * irez - flag for resonance 0-none, 1-one day, 2-half day 0929 * argpo - argument of perigee 0930 * argpdot - argument of perigee dot (rate) 0931 * t - time 0932 * tc - 0933 * gsto - gst 0934 * xfact - 0935 * xlamo - 0936 * no - mean motion 0937 * atime - 0938 * em - eccentricity 0939 * ft - 0940 * argpm - argument of perigee 0941 * inclm - inclination 0942 * xli - 0943 * mm - mean anomaly 0944 * xni - mean motion 0945 * nodem - right ascension of ascending node 0946 * 0947 * outputs : 0948 * atime - 0949 * em - eccentricity 0950 * argpm - argument of perigee 0951 * inclm - inclination 0952 * xli - 0953 * mm - mean anomaly 0954 * xni - 0955 * nodem - right ascension of ascending node 0956 * dndt - 0957 * nm - mean motion 0958 * 0959 * locals : 0960 * delt - 0961 * ft - 0962 * theta - 0963 * x2li - 0964 * x2omi - 0965 * xl - 0966 * xldot - 0967 * xnddt - 0968 * xndt - 0969 * xomi - 0970 * 0971 * coupling : 0972 * none - 0973 * 0974 * references : 0975 * hoots, roehrich, norad spacetrack report #3 1980 0976 * hoots, norad spacetrack report #6 1986 0977 * hoots, schumacher and glover 2004 0978 * vallado, crawford, hujsak, kelso 2006 0979 ----------------------------------------------------------------------------*/ 0980 0981 static void dspace 0982 ( 0983 int irez, 0984 double d2201, double d2211, double d3210, double d3222, double d4410, 0985 double d4422, double d5220, double d5232, double d5421, double d5433, 0986 double dedt, double del1, double del2, double del3, double didt, 0987 double dmdt, double dnodt, double domdt, double argpo, double argpdot, 0988 double t, double tc, double gsto, double xfact, double xlamo, 0989 double no, 0990 double& atime, double& em, double& argpm, double& inclm, double& xli, 0991 double& mm, double& xni, double& nodem, double& dndt, double& nm 0992 ) 0993 { 0994 const double twopi = 2.0 * M_PI; 0995 int iretn; 0996 double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, 0997 g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; 0998 0999 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 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 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 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 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 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