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 * 2013 14 * by david vallado 15 * 16 * (w) 719-573-2600, email dvallado@agi.com, davallado@gmail.com 17 * 18 * current : 19 * 12 mar 20 david vallado 20 * chg satnum to string for alpha 5 or 9-digit 21 * changes : 22 * 7 dec 15 david vallado 23 * fix jd, jdfrac 24 * 3 nov 14 david vallado 25 * update to msvs2013 c++ 26 * 30 aug 10 david vallado 27 * delete unused variables in initl 28 * replace pow integer 2, 3 with multiplies for speed 29 * 3 nov 08 david vallado 30 * put returns in for error codes 31 * 29 sep 08 david vallado 32 * fix atime for faster operation in dspace 33 * add operationmode for afspc (a) or improved (i) 34 * performance mode 35 * 16 jun 08 david vallado 36 * update small eccentricity check 37 * 16 nov 07 david vallado 38 * misc fixes for better compliance 39 * 20 apr 07 david vallado 40 * misc fixes for constants 41 * 11 aug 06 david vallado 42 * chg lyddane choice back to strn3, constants, misc doc 43 * 15 dec 05 david vallado 44 * misc fixes 45 * 26 jul 05 david vallado 46 * fixes for paper 47 * note that each fix is preceded by a 48 * comment with "sgp4fix" and an explanation of 49 * what was changed 50 * 10 aug 04 david vallado 51 * 2nd printing baseline working 52 * 14 may 01 david vallado 53 * 2nd edition baseline 54 * 80 norad 55 * original baseline 56 * ---------------------------------------------------------------- */ 57 58 #include "SGP4.h" 59 60 #define pi 3.14159265358979323846 61 62 // define global variables here, not in .h 63 // use extern in main 64 char help = 'n'; 65 FILE *dbgfile; 66 67 /* ----------- local functions - only ever used internally by sgp4 ---------- */ 68 static void dpper 69 ( 70 double e3, double ee2, double peo, double pgho, double pho, 71 double pinco, double plo, double se2, double se3, double sgh2, 72 double sgh3, double sgh4, double sh2, double sh3, double si2, 73 double si3, double sl2, double sl3, double sl4, double t, 74 double xgh2, double xgh3, double xgh4, double xh2, double xh3, 75 double xi2, double xi3, double xl2, double xl3, double xl4, 76 double zmol, double zmos, double inclo, 77 char init, 78 double& ep, double& inclp, double& nodep, double& argpp, double& mp, 79 char opsmode 80 ); 81 82 static void dscom 83 ( 84 double epoch, double ep, double argpp, double tc, double inclp, 85 double nodep, double np, 86 double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, 87 double& cosomm, double& day, double& e3, double& ee2, double& em, 88 double& emsq, double& gam, double& peo, double& pgho, double& pho, 89 double& pinco, double& plo, double& rtemsq, double& se2, double& se3, 90 double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, 91 double& si2, double& si3, double& sl2, double& sl3, double& sl4, 92 double& s1, double& s2, double& s3, double& s4, double& s5, 93 double& s6, double& s7, double& ss1, double& ss2, double& ss3, 94 double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, 95 double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, 96 double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, 97 double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, 98 double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, 99 double& xl4, double& nm, double& z1, double& z2, double& z3, 100 double& z11, double& z12, double& z13, double& z21, double& z22, 101 double& z23, double& z31, double& z32, double& z33, double& zmol, 102 double& zmos 103 ); 104 105 static void dsinit 106 ( 107 //sgp4fix no longer needed pass in xke 108 //gravconsttype whichconst, 109 double xke, 110 double cosim, double emsq, double argpo, double s1, double s2, 111 double s3, double s4, double s5, double sinim, double ss1, 112 double ss2, double ss3, double ss4, double ss5, double sz1, 113 double sz3, double sz11, double sz13, double sz21, double sz23, 114 double sz31, double sz33, double t, double tc, double gsto, 115 double mo, double mdot, double no, double nodeo, double nodedot, 116 double xpidot, double z1, double z3, double z11, double z13, 117 double z21, double z23, double z31, double z33, double ecco, 118 double eccsq, double& em, double& argpm, double& inclm, double& mm, 119 double& nm, double& nodem, 120 int& irez, 121 double& atime, double& d2201, double& d2211, double& d3210, double& d3222, 122 double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, 123 double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, 124 double& dnodt, double& domdt, double& del1, double& del2, double& del3, 125 double& xfact, double& xlamo, double& xli, double& xni 126 ); 127 128 static void dspace 129 ( 130 int irez, 131 double d2201, double d2211, double d3210, double d3222, double d4410, 132 double d4422, double d5220, double d5232, double d5421, double d5433, 133 double dedt, double del1, double del2, double del3, double didt, 134 double dmdt, double dnodt, double domdt, double argpo, double argpdot, 135 double t, double tc, double gsto, double xfact, double xlamo, 136 double no, 137 double& atime, double& em, double& argpm, double& inclm, double& xli, 138 double& mm, double& xni, double& nodem, double& dndt, double& nm 139 ); 140 141 static void initl 142 ( 143 // not needeed. included in satrec if needed later 144 // int satn, 145 // sgp4fix assin xke and j2 146 // gravconsttype whichconst, 147 double xke, double j2, 148 double ecco, double epoch, double inclo, double& no, 149 char& method, 150 double& ainv, double& ao, double& con41, double& con42, double& cosio, 151 double& cosio2, double& eccsq, double& omeosq, double& posq, 152 double& rp, double& rteosq, double& sinio, double& gsto, char opsmode 153 ); 154 155 namespace SGP4Funcs 156 { 157 158 /* ----------------------------------------------------------------------------- 159 * 160 * procedure dpper 161 * 162 * this procedure provides deep space long period periodic contributions 163 * to the mean elements. by design, these periodics are zero at epoch. 164 * this used to be dscom which included initialization, but it's really a 165 * recurring function. 166 * 167 * author : david vallado 719-573-2600 28 jun 2005 168 * 169 * inputs : 170 * e3 - 171 * ee2 - 172 * peo - 173 * pgho - 174 * pho - 175 * pinco - 176 * plo - 177 * se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - 178 * t - 179 * xh2, xh3, xi2, xi3, xl2, xl3, xl4 - 180 * zmol - 181 * zmos - 182 * ep - eccentricity 0.0 - 1.0 183 * inclo - inclination - needed for lyddane modification 184 * nodep - right ascension of ascending node 185 * argpp - argument of perigee 186 * mp - mean anomaly 187 * 188 * outputs : 189 * ep - eccentricity 0.0 - 1.0 190 * inclp - inclination 191 * nodep - right ascension of ascending node 192 * argpp - argument of perigee 193 * mp - mean anomaly 194 * 195 * locals : 196 * alfdp - 197 * betdp - 198 * cosip , sinip , cosop , sinop , 199 * dalf - 200 * dbet - 201 * dls - 202 * f2, f3 - 203 * pe - 204 * pgh - 205 * ph - 206 * pinc - 207 * pl - 208 * sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , 209 * sll , sls 210 * xls - 211 * xnoh - 212 * zf - 213 * zm - 214 * 215 * coupling : 216 * none. 217 * 218 * references : 219 * hoots, roehrich, norad spacetrack report #3 1980 220 * hoots, norad spacetrack report #6 1986 221 * hoots, schumacher and glover 2004 222 * vallado, crawford, hujsak, kelso 2006 223 ----------------------------------------------------------------------------*/ 224 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)225 static void dpper 226 ( 227 double e3, double ee2, double peo, double pgho, double pho, 228 double pinco, double plo, double se2, double se3, double sgh2, 229 double sgh3, double sgh4, double sh2, double sh3, double si2, 230 double si3, double sl2, double sl3, double sl4, double t, 231 double xgh2, double xgh3, double xgh4, double xh2, double xh3, 232 double xi2, double xi3, double xl2, double xl3, double xl4, 233 double zmol, double zmos, double inclo, 234 char init, 235 double& ep, double& inclp, double& nodep, double& argpp, double& mp, 236 char opsmode 237 ) 238 { 239 /* --------------------- local variables ------------------------ */ 240 const double twopi = 2.0 * pi; 241 double alfdp, betdp, cosip, cosop, dalf, dbet, dls, 242 f2, f3, pe, pgh, ph, pinc, pl, 243 sel, ses, sghl, sghs, shll, shs, sil, 244 sinip, sinop, sinzf, sis, sll, sls, xls, 245 xnoh, zf, zm, zel, zes, znl, zns; 246 247 /* ---------------------- constants ----------------------------- */ 248 zns = 1.19459e-5; 249 zes = 0.01675; 250 znl = 1.5835218e-4; 251 zel = 0.05490; 252 253 /* --------------- calculate time varying periodics ----------- */ 254 zm = zmos + zns * t; 255 // be sure that the initial call has time set to zero 256 if (init == 'y') 257 zm = zmos; 258 zf = zm + 2.0 * zes * sin(zm); 259 sinzf = sin(zf); 260 f2 = 0.5 * sinzf * sinzf - 0.25; 261 f3 = -0.5 * sinzf * cos(zf); 262 ses = se2* f2 + se3 * f3; 263 sis = si2 * f2 + si3 * f3; 264 sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; 265 sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; 266 shs = sh2 * f2 + sh3 * f3; 267 zm = zmol + znl * t; 268 if (init == 'y') 269 zm = zmol; 270 zf = zm + 2.0 * zel * sin(zm); 271 sinzf = sin(zf); 272 f2 = 0.5 * sinzf * sinzf - 0.25; 273 f3 = -0.5 * sinzf * cos(zf); 274 sel = ee2 * f2 + e3 * f3; 275 sil = xi2 * f2 + xi3 * f3; 276 sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; 277 sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; 278 shll = xh2 * f2 + xh3 * f3; 279 pe = ses + sel; 280 pinc = sis + sil; 281 pl = sls + sll; 282 pgh = sghs + sghl; 283 ph = shs + shll; 284 285 if (init == 'n') 286 { 287 pe = pe - peo; 288 pinc = pinc - pinco; 289 pl = pl - plo; 290 pgh = pgh - pgho; 291 ph = ph - pho; 292 inclp = inclp + pinc; 293 ep = ep + pe; 294 sinip = sin(inclp); 295 cosip = cos(inclp); 296 297 /* ----------------- apply periodics directly ------------ */ 298 // sgp4fix for lyddane choice 299 // strn3 used original inclination - this is technically feasible 300 // gsfc used perturbed inclination - also technically feasible 301 // probably best to readjust the 0.2 limit value and limit discontinuity 302 // 0.2 rad = 11.45916 deg 303 // use next line for original strn3 approach and original inclination 304 // if (inclo >= 0.2) 305 // use next line for gsfc version and perturbed inclination 306 if (inclp >= 0.2) 307 { 308 ph = ph / sinip; 309 pgh = pgh - cosip * ph; 310 argpp = argpp + pgh; 311 nodep = nodep + ph; 312 mp = mp + pl; 313 } 314 else 315 { 316 /* ---- apply periodics with lyddane modification ---- */ 317 sinop = sin(nodep); 318 cosop = cos(nodep); 319 alfdp = sinip * sinop; 320 betdp = sinip * cosop; 321 dalf = ph * cosop + pinc * cosip * sinop; 322 dbet = -ph * sinop + pinc * cosip * cosop; 323 alfdp = alfdp + dalf; 324 betdp = betdp + dbet; 325 nodep = fmod(nodep, twopi); 326 // sgp4fix for afspc written intrinsic functions 327 // nodep used without a trigonometric function ahead 328 if ((nodep < 0.0) && (opsmode == 'a')) 329 nodep = nodep + twopi; 330 xls = mp + argpp + cosip * nodep; 331 dls = pl + pgh - pinc * nodep * sinip; 332 xls = xls + dls; 333 xnoh = nodep; 334 nodep = atan2(alfdp, betdp); 335 // sgp4fix for afspc written intrinsic functions 336 // nodep used without a trigonometric function ahead 337 if ((nodep < 0.0) && (opsmode == 'a')) 338 nodep = nodep + twopi; 339 if (fabs(xnoh - nodep) > pi) 340 if (nodep < xnoh) 341 nodep = nodep + twopi; 342 else 343 nodep = nodep - twopi; 344 mp = mp + pl; 345 argpp = xls - mp - cosip * nodep; 346 } 347 } // if init == 'n' 348 349 //#include "debug1.cpp" 350 } // dpper 351 352 /*----------------------------------------------------------------------------- 353 * 354 * procedure dscom 355 * 356 * this procedure provides deep space common items used by both the secular 357 * and periodics subroutines. input is provided as shown. this routine 358 * used to be called dpper, but the functions inside weren't well organized. 359 * 360 * author : david vallado 719-573-2600 28 jun 2005 361 * 362 * inputs : 363 * epoch - 364 * ep - eccentricity 365 * argpp - argument of perigee 366 * tc - 367 * inclp - inclination 368 * nodep - right ascension of ascending node 369 * np - mean motion 370 * 371 * outputs : 372 * sinim , cosim , sinomm , cosomm , snodm , cnodm 373 * day - 374 * e3 - 375 * ee2 - 376 * em - eccentricity 377 * emsq - eccentricity squared 378 * gam - 379 * peo - 380 * pgho - 381 * pho - 382 * pinco - 383 * plo - 384 * rtemsq - 385 * se2, se3 - 386 * sgh2, sgh3, sgh4 - 387 * sh2, sh3, si2, si3, sl2, sl3, sl4 - 388 * s1, s2, s3, s4, s5, s6, s7 - 389 * ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - 390 * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - 391 * xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - 392 * nm - mean motion 393 * z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - 394 * zmol - 395 * zmos - 396 * 397 * locals : 398 * a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - 399 * betasq - 400 * cc - 401 * ctem, stem - 402 * x1, x2, x3, x4, x5, x6, x7, x8 - 403 * xnodce - 404 * xnoi - 405 * zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , 406 * zcosi , zsini , zcosil , zsinil , 407 * zx - 408 * zy - 409 * 410 * coupling : 411 * none. 412 * 413 * references : 414 * hoots, roehrich, norad spacetrack report #3 1980 415 * hoots, norad spacetrack report #6 1986 416 * hoots, schumacher and glover 2004 417 * vallado, crawford, hujsak, kelso 2006 418 ----------------------------------------------------------------------------*/ 419 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)420 static void dscom 421 ( 422 double epoch, double ep, double argpp, double tc, double inclp, 423 double nodep, double np, 424 double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, 425 double& cosomm, double& day, double& e3, double& ee2, double& em, 426 double& emsq, double& gam, double& peo, double& pgho, double& pho, 427 double& pinco, double& plo, double& rtemsq, double& se2, double& se3, 428 double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, 429 double& si2, double& si3, double& sl2, double& sl3, double& sl4, 430 double& s1, double& s2, double& s3, double& s4, double& s5, 431 double& s6, double& s7, double& ss1, double& ss2, double& ss3, 432 double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, 433 double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, 434 double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, 435 double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, 436 double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, 437 double& xl4, double& nm, double& z1, double& z2, double& z3, 438 double& z11, double& z12, double& z13, double& z21, double& z22, 439 double& z23, double& z31, double& z32, double& z33, double& zmol, 440 double& zmos 441 ) 442 { 443 /* -------------------------- constants ------------------------- */ 444 const double zes = 0.01675; 445 const double zel = 0.05490; 446 const double c1ss = 2.9864797e-6; 447 const double c1l = 4.7968065e-7; 448 const double zsinis = 0.39785416; 449 const double zcosis = 0.91744867; 450 const double zcosgs = 0.1945905; 451 const double zsings = -0.98088458; 452 const double twopi = 2.0 * pi; 453 454 /* --------------------- local variables ------------------------ */ 455 int lsflg; 456 double a1, a2, a3, a4, a5, a6, a7, 457 a8, a9, a10, betasq, cc, ctem, stem, 458 x1, x2, x3, x4, x5, x6, x7, 459 x8, xnodce, xnoi, zcosg, zcosgl, zcosh, zcoshl, 460 zcosi, zcosil, zsing, zsingl, zsinh, zsinhl, zsini, 461 zsinil, zx, zy; 462 463 nm = np; 464 em = ep; 465 snodm = sin(nodep); 466 cnodm = cos(nodep); 467 sinomm = sin(argpp); 468 cosomm = cos(argpp); 469 sinim = sin(inclp); 470 cosim = cos(inclp); 471 emsq = em * em; 472 betasq = 1.0 - emsq; 473 rtemsq = sqrt(betasq); 474 475 /* ----------------- initialize lunar solar terms --------------- */ 476 peo = 0.0; 477 pinco = 0.0; 478 plo = 0.0; 479 pgho = 0.0; 480 pho = 0.0; 481 day = epoch + 18261.5 + tc / 1440.0; 482 xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); 483 stem = sin(xnodce); 484 ctem = cos(xnodce); 485 zcosil = 0.91375164 - 0.03568096 * ctem; 486 zsinil = sqrt(1.0 - zcosil * zcosil); 487 zsinhl = 0.089683511 * stem / zsinil; 488 zcoshl = sqrt(1.0 - zsinhl * zsinhl); 489 gam = 5.8351514 + 0.0019443680 * day; 490 zx = 0.39785416 * stem / zsinil; 491 zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; 492 zx = atan2(zx, zy); 493 zx = gam + zx - xnodce; 494 zcosgl = cos(zx); 495 zsingl = sin(zx); 496 497 /* ------------------------- do solar terms --------------------- */ 498 zcosg = zcosgs; 499 zsing = zsings; 500 zcosi = zcosis; 501 zsini = zsinis; 502 zcosh = cnodm; 503 zsinh = snodm; 504 cc = c1ss; 505 xnoi = 1.0 / nm; 506 507 for (lsflg = 1; lsflg <= 2; lsflg++) 508 { 509 a1 = zcosg * zcosh + zsing * zcosi * zsinh; 510 a3 = -zsing * zcosh + zcosg * zcosi * zsinh; 511 a7 = -zcosg * zsinh + zsing * zcosi * zcosh; 512 a8 = zsing * zsini; 513 a9 = zsing * zsinh + zcosg * zcosi * zcosh; 514 a10 = zcosg * zsini; 515 a2 = cosim * a7 + sinim * a8; 516 a4 = cosim * a9 + sinim * a10; 517 a5 = -sinim * a7 + cosim * a8; 518 a6 = -sinim * a9 + cosim * a10; 519 520 x1 = a1 * cosomm + a2 * sinomm; 521 x2 = a3 * cosomm + a4 * sinomm; 522 x3 = -a1 * sinomm + a2 * cosomm; 523 x4 = -a3 * sinomm + a4 * cosomm; 524 x5 = a5 * sinomm; 525 x6 = a6 * sinomm; 526 x7 = a5 * cosomm; 527 x8 = a6 * cosomm; 528 529 z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; 530 z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; 531 z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; 532 z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; 533 z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; 534 z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; 535 z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7 - 6.0 * x3 * x5); 536 z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * 537 (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); 538 z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); 539 z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); 540 z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * 541 (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); 542 z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); 543 z1 = z1 + z1 + betasq * z31; 544 z2 = z2 + z2 + betasq * z32; 545 z3 = z3 + z3 + betasq * z33; 546 s3 = cc * xnoi; 547 s2 = -0.5 * s3 / rtemsq; 548 s4 = s3 * rtemsq; 549 s1 = -15.0 * em * s4; 550 s5 = x1 * x3 + x2 * x4; 551 s6 = x2 * x3 + x1 * x4; 552 s7 = x2 * x4 - x1 * x3; 553 554 /* ----------------------- do lunar terms ------------------- */ 555 if (lsflg == 1) 556 { 557 ss1 = s1; 558 ss2 = s2; 559 ss3 = s3; 560 ss4 = s4; 561 ss5 = s5; 562 ss6 = s6; 563 ss7 = s7; 564 sz1 = z1; 565 sz2 = z2; 566 sz3 = z3; 567 sz11 = z11; 568 sz12 = z12; 569 sz13 = z13; 570 sz21 = z21; 571 sz22 = z22; 572 sz23 = z23; 573 sz31 = z31; 574 sz32 = z32; 575 sz33 = z33; 576 zcosg = zcosgl; 577 zsing = zsingl; 578 zcosi = zcosil; 579 zsini = zsinil; 580 zcosh = zcoshl * cnodm + zsinhl * snodm; 581 zsinh = snodm * zcoshl - cnodm * zsinhl; 582 cc = c1l; 583 } 584 } 585 586 zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); 587 zmos = fmod(6.2565837 + 0.017201977 * day, twopi); 588 589 /* ------------------------ do solar terms ---------------------- */ 590 se2 = 2.0 * ss1 * ss6; 591 se3 = 2.0 * ss1 * ss7; 592 si2 = 2.0 * ss2 * sz12; 593 si3 = 2.0 * ss2 * (sz13 - sz11); 594 sl2 = -2.0 * ss3 * sz2; 595 sl3 = -2.0 * ss3 * (sz3 - sz1); 596 sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; 597 sgh2 = 2.0 * ss4 * sz32; 598 sgh3 = 2.0 * ss4 * (sz33 - sz31); 599 sgh4 = -18.0 * ss4 * zes; 600 sh2 = -2.0 * ss2 * sz22; 601 sh3 = -2.0 * ss2 * (sz23 - sz21); 602 603 /* ------------------------ do lunar terms ---------------------- */ 604 ee2 = 2.0 * s1 * s6; 605 e3 = 2.0 * s1 * s7; 606 xi2 = 2.0 * s2 * z12; 607 xi3 = 2.0 * s2 * (z13 - z11); 608 xl2 = -2.0 * s3 * z2; 609 xl3 = -2.0 * s3 * (z3 - z1); 610 xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; 611 xgh2 = 2.0 * s4 * z32; 612 xgh3 = 2.0 * s4 * (z33 - z31); 613 xgh4 = -18.0 * s4 * zel; 614 xh2 = -2.0 * s2 * z22; 615 xh3 = -2.0 * s2 * (z23 - z21); 616 617 //#include "debug2.cpp" 618 } // dscom 619 620 /*----------------------------------------------------------------------------- 621 * 622 * procedure dsinit 623 * 624 * this procedure provides deep space contributions to mean motion dot due 625 * to geopotential resonance with half day and one day orbits. 626 * 627 * author : david vallado 719-573-2600 28 jun 2005 628 * 629 * inputs : 630 * xke - reciprocal of tumin 631 * cosim, sinim- 632 * emsq - eccentricity squared 633 * argpo - argument of perigee 634 * s1, s2, s3, s4, s5 - 635 * ss1, ss2, ss3, ss4, ss5 - 636 * sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - 637 * t - time 638 * tc - 639 * gsto - greenwich sidereal time rad 640 * mo - mean anomaly 641 * mdot - mean anomaly dot (rate) 642 * no - mean motion 643 * nodeo - right ascension of ascending node 644 * nodedot - right ascension of ascending node dot (rate) 645 * xpidot - 646 * z1, z3, z11, z13, z21, z23, z31, z33 - 647 * eccm - eccentricity 648 * argpm - argument of perigee 649 * inclm - inclination 650 * mm - mean anomaly 651 * xn - mean motion 652 * nodem - right ascension of ascending node 653 * 654 * outputs : 655 * em - eccentricity 656 * argpm - argument of perigee 657 * inclm - inclination 658 * mm - mean anomaly 659 * nm - mean motion 660 * nodem - right ascension of ascending node 661 * irez - flag for resonance 0-none, 1-one day, 2-half day 662 * atime - 663 * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - 664 * dedt - 665 * didt - 666 * dmdt - 667 * dndt - 668 * dnodt - 669 * domdt - 670 * del1, del2, del3 - 671 * ses , sghl , sghs , sgs , shl , shs , sis , sls 672 * theta - 673 * xfact - 674 * xlamo - 675 * xli - 676 * xni 677 * 678 * locals : 679 * ainv2 - 680 * aonv - 681 * cosisq - 682 * eoc - 683 * f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - 684 * g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - 685 * sini2 - 686 * temp - 687 * temp1 - 688 * theta - 689 * xno2 - 690 * 691 * coupling : 692 * getgravconst- no longer used 693 * 694 * references : 695 * hoots, roehrich, norad spacetrack report #3 1980 696 * hoots, norad spacetrack report #6 1986 697 * hoots, schumacher and glover 2004 698 * vallado, crawford, hujsak, kelso 2006 699 ----------------------------------------------------------------------------*/ 700 dsinit(double xke,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)701 static void dsinit 702 ( 703 // sgp4fix just send in xke as a constant and eliminate getgravconst call 704 // gravconsttype whichconst, 705 double xke, 706 double cosim, double emsq, double argpo, double s1, double s2, 707 double s3, double s4, double s5, double sinim, double ss1, 708 double ss2, double ss3, double ss4, double ss5, double sz1, 709 double sz3, double sz11, double sz13, double sz21, double sz23, 710 double sz31, double sz33, double t, double tc, double gsto, 711 double mo, double mdot, double no, double nodeo, double nodedot, 712 double xpidot, double z1, double z3, double z11, double z13, 713 double z21, double z23, double z31, double z33, double ecco, 714 double eccsq, double& em, double& argpm, double& inclm, double& mm, 715 double& nm, double& nodem, 716 int& irez, 717 double& atime, double& d2201, double& d2211, double& d3210, double& d3222, 718 double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, 719 double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, 720 double& dnodt, double& domdt, double& del1, double& del2, double& del3, 721 double& xfact, double& xlamo, double& xli, double& xni 722 ) 723 { 724 /* --------------------- local variables ------------------------ */ 725 const double twopi = 2.0 * pi; 726 727 double ainv2, aonv = 0.0, cosisq, eoc, f220, f221, f311, 728 f321, f322, f330, f441, f442, f522, f523, 729 f542, f543, g200, g201, g211, g300, g310, 730 g322, g410, g422, g520, g521, g532, g533, 731 ses, sgs, sghl, sghs, shs, shll, sis, 732 sini2, sls, temp, temp1, theta, xno2, q22, 733 q31, q33, root22, root44, root54, rptim, root32, 734 root52, x2o3, znl, emo, zns, emsqo; 735 736 q22 = 1.7891679e-6; 737 q31 = 2.1460748e-6; 738 q33 = 2.2123015e-7; 739 root22 = 1.7891679e-6; 740 root44 = 7.3636953e-9; 741 root54 = 2.1765803e-9; 742 rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec 743 root32 = 3.7393792e-7; 744 root52 = 1.1428639e-7; 745 x2o3 = 2.0 / 3.0; 746 znl = 1.5835218e-4; 747 zns = 1.19459e-5; 748 749 // sgp4fix identify constants and allow alternate values 750 // just xke is used here so pass it in rather than have multiple calls 751 // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 752 753 /* -------------------- deep space initialization ------------ */ 754 irez = 0; 755 if ((nm < 0.0052359877) && (nm > 0.0034906585)) 756 irez = 1; 757 if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) 758 irez = 2; 759 760 /* ------------------------ do solar terms ------------------- */ 761 ses = ss1 * zns * ss5; 762 sis = ss2 * zns * (sz11 + sz13); 763 sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); 764 sghs = ss4 * zns * (sz31 + sz33 - 6.0); 765 shs = -zns * ss2 * (sz21 + sz23); 766 // sgp4fix for 180 deg incl 767 if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) 768 shs = 0.0; 769 if (sinim != 0.0) 770 shs = shs / sinim; 771 sgs = sghs - cosim * shs; 772 773 /* ------------------------- do lunar terms ------------------ */ 774 dedt = ses + s1 * znl * s5; 775 didt = sis + s2 * znl * (z11 + z13); 776 dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); 777 sghl = s4 * znl * (z31 + z33 - 6.0); 778 shll = -znl * s2 * (z21 + z23); 779 // sgp4fix for 180 deg incl 780 if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) 781 shll = 0.0; 782 domdt = sgs + sghl; 783 dnodt = shs; 784 if (sinim != 0.0) 785 { 786 domdt = domdt - cosim / sinim * shll; 787 dnodt = dnodt + shll / sinim; 788 } 789 790 /* ----------- calculate deep space resonance effects -------- */ 791 dndt = 0.0; 792 theta = fmod(gsto + tc * rptim, twopi); 793 em = em + dedt * t; 794 inclm = inclm + didt * t; 795 argpm = argpm + domdt * t; 796 nodem = nodem + dnodt * t; 797 mm = mm + dmdt * t; 798 // sgp4fix for negative inclinations 799 // the following if statement should be commented out 800 //if (inclm < 0.0) 801 // { 802 // inclm = -inclm; 803 // argpm = argpm - pi; 804 // nodem = nodem + pi; 805 // } 806 807 /* -------------- initialize the resonance terms ------------- */ 808 if (irez != 0) 809 { 810 aonv = pow(nm / xke, x2o3); 811 812 /* ---------- geopotential resonance for 12 hour orbits ------ */ 813 if (irez == 2) 814 { 815 cosisq = cosim * cosim; 816 emo = em; 817 em = ecco; 818 emsqo = emsq; 819 emsq = eccsq; 820 eoc = em * emsq; 821 g201 = -0.306 - (em - 0.64) * 0.440; 822 823 if (em <= 0.65) 824 { 825 g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; 826 g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; 827 g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; 828 g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; 829 g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; 830 g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; 831 } 832 else 833 { 834 g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; 835 g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; 836 g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; 837 g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; 838 g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; 839 if (em > 0.715) 840 g520 = -5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; 841 else 842 g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; 843 } 844 if (em < 0.7) 845 { 846 g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; 847 g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; 848 g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; 849 } 850 else 851 { 852 g533 = -37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; 853 g521 = -51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; 854 g532 = -40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; 855 } 856 857 sini2 = sinim * sinim; 858 f220 = 0.75 * (1.0 + 2.0 * cosim + cosisq); 859 f221 = 1.5 * sini2; 860 f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); 861 f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); 862 f441 = 35.0 * sini2 * f220; 863 f442 = 39.3750 * sini2 * sini2; 864 f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim - 5.0 * cosisq) + 865 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq)); 866 f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + 867 10.0 * cosisq) + 6.56250012 * (1.0 + 2.0 * cosim - 3.0 * cosisq)); 868 f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim + cosisq * 869 (-12.0 + 8.0 * cosim + 10.0 * cosisq)); 870 f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim + cosisq * 871 (12.0 + 8.0 * cosim - 10.0 * cosisq)); 872 xno2 = nm * nm; 873 ainv2 = aonv * aonv; 874 temp1 = 3.0 * xno2 * ainv2; 875 temp = temp1 * root22; 876 d2201 = temp * f220 * g201; 877 d2211 = temp * f221 * g211; 878 temp1 = temp1 * aonv; 879 temp = temp1 * root32; 880 d3210 = temp * f321 * g310; 881 d3222 = temp * f322 * g322; 882 temp1 = temp1 * aonv; 883 temp = 2.0 * temp1 * root44; 884 d4410 = temp * f441 * g410; 885 d4422 = temp * f442 * g422; 886 temp1 = temp1 * aonv; 887 temp = temp1 * root52; 888 d5220 = temp * f522 * g520; 889 d5232 = temp * f523 * g532; 890 temp = 2.0 * temp1 * root54; 891 d5421 = temp * f542 * g521; 892 d5433 = temp * f543 * g533; 893 xlamo = fmod(mo + nodeo + nodeo - theta - theta, twopi); 894 xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; 895 em = emo; 896 emsq = emsqo; 897 } 898 899 /* ---------------- synchronous resonance terms -------------- */ 900 if (irez == 1) 901 { 902 g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); 903 g310 = 1.0 + 2.0 * emsq; 904 g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); 905 f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); 906 f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); 907 f330 = 1.0 + cosim; 908 f330 = 1.875 * f330 * f330 * f330; 909 del1 = 3.0 * nm * nm * aonv * aonv; 910 del2 = 2.0 * del1 * f220 * g200 * q22; 911 del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; 912 del1 = del1 * f311 * g310 * q31 * aonv; 913 xlamo = fmod(mo + nodeo + argpo - theta, twopi); 914 xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; 915 } 916 917 /* ------------ for sgp4, initialize the integrator ---------- */ 918 xli = xlamo; 919 xni = no; 920 atime = 0.0; 921 nm = no + dndt; 922 } 923 924 //#include "debug3.cpp" 925 } // dsinit 926 927 /*----------------------------------------------------------------------------- 928 * 929 * procedure dspace 930 * 931 * this procedure provides deep space contributions to mean elements for 932 * perturbing third body. these effects have been averaged over one 933 * revolution of the sun and moon. for earth resonance effects, the 934 * effects have been averaged over no revolutions of the satellite. 935 * (mean motion) 936 * 937 * author : david vallado 719-573-2600 28 jun 2005 938 * 939 * inputs : 940 * d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - 941 * dedt - 942 * del1, del2, del3 - 943 * didt - 944 * dmdt - 945 * dnodt - 946 * domdt - 947 * irez - flag for resonance 0-none, 1-one day, 2-half day 948 * argpo - argument of perigee 949 * argpdot - argument of perigee dot (rate) 950 * t - time 951 * tc - 952 * gsto - gst 953 * xfact - 954 * xlamo - 955 * no - mean motion 956 * atime - 957 * em - eccentricity 958 * ft - 959 * argpm - argument of perigee 960 * inclm - inclination 961 * xli - 962 * mm - mean anomaly 963 * xni - mean motion 964 * nodem - right ascension of ascending node 965 * 966 * outputs : 967 * atime - 968 * em - eccentricity 969 * argpm - argument of perigee 970 * inclm - inclination 971 * xli - 972 * mm - mean anomaly 973 * xni - 974 * nodem - right ascension of ascending node 975 * dndt - 976 * nm - mean motion 977 * 978 * locals : 979 * delt - 980 * ft - 981 * theta - 982 * x2li - 983 * x2omi - 984 * xl - 985 * xldot - 986 * xnddt - 987 * xndt - 988 * xomi - 989 * 990 * coupling : 991 * none - 992 * 993 * references : 994 * hoots, roehrich, norad spacetrack report #3 1980 995 * hoots, norad spacetrack report #6 1986 996 * hoots, schumacher and glover 2004 997 * vallado, crawford, hujsak, kelso 2006 998 ----------------------------------------------------------------------------*/ 999 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)1000 static void dspace 1001 ( 1002 int irez, 1003 double d2201, double d2211, double d3210, double d3222, double d4410, 1004 double d4422, double d5220, double d5232, double d5421, double d5433, 1005 double dedt, double del1, double del2, double del3, double didt, 1006 double dmdt, double dnodt, double domdt, double argpo, double argpdot, 1007 double t, double tc, double gsto, double xfact, double xlamo, 1008 double no, 1009 double& atime, double& em, double& argpm, double& inclm, double& xli, 1010 double& mm, double& xni, double& nodem, double& dndt, double& nm 1011 ) 1012 { 1013 const double twopi = 2.0 * pi; 1014 int iretn, iret; 1015 double delt, ft, theta, x2li, x2omi, xl, xldot, xnddt, xndt, xomi, g22, g32, 1016 g44, g52, g54, fasx2, fasx4, fasx6, rptim, step2, stepn, stepp; 1017 1018 fasx2 = 0.13130908; 1019 fasx4 = 2.8843198; 1020 fasx6 = 0.37448087; 1021 g22 = 5.7686396; 1022 g32 = 0.95240898; 1023 g44 = 1.8014998; 1024 g52 = 1.0508330; 1025 g54 = 4.4108898; 1026 rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec 1027 stepp = 720.0; 1028 stepn = -720.0; 1029 step2 = 259200.0; 1030 1031 /* ----------- calculate deep space resonance effects ----------- */ 1032 dndt = 0.0; 1033 theta = fmod(gsto + tc * rptim, twopi); 1034 em = em + dedt * t; 1035 1036 inclm = inclm + didt * t; 1037 argpm = argpm + domdt * t; 1038 nodem = nodem + dnodt * t; 1039 mm = mm + dmdt * t; 1040 1041 // sgp4fix for negative inclinations 1042 // the following if statement should be commented out 1043 // if (inclm < 0.0) 1044 // { 1045 // inclm = -inclm; 1046 // argpm = argpm - pi; 1047 // nodem = nodem + pi; 1048 // } 1049 1050 /* - update resonances : numerical (euler-maclaurin) integration - */ 1051 /* ------------------------- epoch restart ---------------------- */ 1052 // sgp4fix for propagator problems 1053 // the following integration works for negative time steps and periods 1054 // the specific changes are unknown because the original code was so convoluted 1055 1056 // sgp4fix take out atime = 0.0 and fix for faster operation 1057 ft = 0.0; 1058 if (irez != 0) 1059 { 1060 // sgp4fix streamline check 1061 if ((atime == 0.0) || (t * atime <= 0.0) || (fabs(t) < fabs(atime))) 1062 { 1063 atime = 0.0; 1064 xni = no; 1065 xli = xlamo; 1066 } 1067 // sgp4fix move check outside loop 1068 if (t > 0.0) 1069 delt = stepp; 1070 else 1071 delt = stepn; 1072 1073 iretn = 381; // added for do loop 1074 iret = 0; // added for loop 1075 while (iretn == 381) 1076 { 1077 /* ------------------- dot terms calculated ------------- */ 1078 /* ----------- near - synchronous resonance terms ------- */ 1079 if (irez != 2) 1080 { 1081 xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + 1082 del3 * sin(3.0 * (xli - fasx6)); 1083 xldot = xni + xfact; 1084 xnddt = del1 * cos(xli - fasx2) + 1085 2.0 * del2 * cos(2.0 * (xli - fasx4)) + 1086 3.0 * del3 * cos(3.0 * (xli - fasx6)); 1087 xnddt = xnddt * xldot; 1088 } 1089 else 1090 { 1091 /* --------- near - half-day resonance terms -------- */ 1092 xomi = argpo + argpdot * atime; 1093 x2omi = xomi + xomi; 1094 x2li = xli + xli; 1095 xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + 1096 d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32) + 1097 d4410 * sin(x2omi + x2li - g44) + d4422 * sin(x2li - g44) + 1098 d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52) + 1099 d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); 1100 xldot = xni + xfact; 1101 xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + 1102 d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + 1103 d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + 1104 2.0 * (d4410 * cos(x2omi + x2li - g44) + 1105 d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + 1106 d5433 * cos(-xomi + x2li - g54)); 1107 xnddt = xnddt * xldot; 1108 } 1109 1110 /* ----------------------- integrator ------------------- */ 1111 // sgp4fix move end checks to end of routine 1112 if (fabs(t - atime) >= stepp) 1113 { 1114 iret = 0; 1115 iretn = 381; 1116 } 1117 else // exit here 1118 { 1119 ft = t - atime; 1120 iretn = 0; 1121 } 1122 1123 if (iretn == 381) 1124 { 1125 xli = xli + xldot * delt + xndt * step2; 1126 xni = xni + xndt * delt + xnddt * step2; 1127 atime = atime + delt; 1128 } 1129 } // while iretn = 381 1130 1131 nm = xni + xndt * ft + xnddt * ft * ft * 0.5; 1132 xl = xli + xldot * ft + xndt * ft * ft * 0.5; 1133 if (irez != 1) 1134 { 1135 mm = xl - 2.0 * nodem + 2.0 * theta; 1136 dndt = nm - no; 1137 } 1138 else 1139 { 1140 mm = xl - nodem - argpm + theta; 1141 dndt = nm - no; 1142 } 1143 nm = no + dndt; 1144 } 1145 1146 //#include "debug4.cpp" 1147 } // dsspace 1148 1149 /*----------------------------------------------------------------------------- 1150 * 1151 * procedure initl 1152 * 1153 * this procedure initializes the spg4 propagator. all the initialization is 1154 * consolidated here instead of having multiple loops inside other routines. 1155 * 1156 * author : david vallado 719-573-2600 28 jun 2005 1157 * 1158 * inputs : 1159 * satn - satellite number - not needed, placed in satrec 1160 * xke - reciprocal of tumin 1161 * j2 - j2 zonal harmonic 1162 * ecco - eccentricity 0.0 - 1.0 1163 * epoch - epoch time in days from jan 0, 1950. 0 hr 1164 * inclo - inclination of satellite 1165 * no - mean motion of satellite 1166 * 1167 * outputs : 1168 * ainv - 1.0 / a 1169 * ao - semi major axis 1170 * con41 - 1171 * con42 - 1.0 - 5.0 cos(i) 1172 * cosio - cosine of inclination 1173 * cosio2 - cosio squared 1174 * eccsq - eccentricity squared 1175 * method - flag for deep space 'd', 'n' 1176 * omeosq - 1.0 - ecco * ecco 1177 * posq - semi-parameter squared 1178 * rp - radius of perigee 1179 * rteosq - square root of (1.0 - ecco*ecco) 1180 * sinio - sine of inclination 1181 * gsto - gst at time of observation rad 1182 * no - mean motion of satellite 1183 * 1184 * locals : 1185 * ak - 1186 * d1 - 1187 * del - 1188 * adel - 1189 * po - 1190 * 1191 * coupling : 1192 * getgravconst- no longer used 1193 * gstime - find greenwich sidereal time from the julian date 1194 * 1195 * references : 1196 * hoots, roehrich, norad spacetrack report #3 1980 1197 * hoots, norad spacetrack report #6 1986 1198 * hoots, schumacher and glover 2004 1199 * vallado, crawford, hujsak, kelso 2006 1200 ----------------------------------------------------------------------------*/ 1201 initl(double xke,double j2,double ecco,double epoch,double inclo,double no_kozai,char opsmode,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,double & no_unkozai)1202 static void initl 1203 ( 1204 // sgp4fix satn not needed. include in satrec in case needed later 1205 // int satn, 1206 // sgp4fix just pass in xke and j2 1207 // gravconsttype whichconst, 1208 double xke, double j2, 1209 double ecco, double epoch, double inclo, double no_kozai, char opsmode, 1210 char& method, double& ainv, double& ao, double& con41, double& con42, double& cosio, 1211 double& cosio2, double& eccsq, double& omeosq, double& posq, 1212 double& rp, double& rteosq, double& sinio, double& gsto, double& no_unkozai 1213 ) 1214 { 1215 /* --------------------- local variables ------------------------ */ 1216 double ak, d1, del, adel, po, x2o3; 1217 1218 // sgp4fix use old way of finding gst 1219 double ds70; 1220 double ts70, tfrac, c1, thgr70, fk5r, c1p2p; 1221 const double twopi = 2.0 * pi; 1222 1223 /* ----------------------- earth constants ---------------------- */ 1224 // sgp4fix identify constants and allow alternate values 1225 // only xke and j2 are used here so pass them in directly 1226 // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 1227 x2o3 = 2.0 / 3.0; 1228 1229 /* ------------- calculate auxillary epoch quantities ---------- */ 1230 eccsq = ecco * ecco; 1231 omeosq = 1.0 - eccsq; 1232 rteosq = sqrt(omeosq); 1233 cosio = cos(inclo); 1234 cosio2 = cosio * cosio; 1235 1236 /* ------------------ un-kozai the mean motion ----------------- */ 1237 ak = pow(xke / no_kozai, x2o3); 1238 d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); 1239 del = d1 / (ak * ak); 1240 adel = ak * (1.0 - del * del - del * 1241 (1.0 / 3.0 + 134.0 * del * del / 81.0)); 1242 del = d1 / (adel * adel); 1243 no_unkozai = no_kozai / (1.0 + del); 1244 1245 ao = pow(xke / (no_unkozai), x2o3); 1246 sinio = sin(inclo); 1247 po = ao * omeosq; 1248 con42 = 1.0 - 5.0 * cosio2; 1249 con41 = -con42 - cosio2 - cosio2; 1250 ainv = 1.0 / ao; 1251 posq = po * po; 1252 rp = ao * (1.0 - ecco); 1253 method = 'n'; 1254 1255 // sgp4fix modern approach to finding sidereal time 1256 // if (opsmode == 'a') 1257 // { 1258 // sgp4fix use old way of finding gst 1259 // count integer number of days from 0 jan 1970 1260 ts70 = epoch - 7305.0; 1261 ds70 = floor(ts70 + 1.0e-8); 1262 tfrac = ts70 - ds70; 1263 // find greenwich location at epoch 1264 c1 = 1.72027916940703639e-2; 1265 thgr70 = 1.7321343856509374; 1266 fk5r = 5.07551419432269442e-15; 1267 c1p2p = c1 + twopi; 1268 double gsto1 = fmod(thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); 1269 if (gsto1 < 0.0) 1270 gsto1 = gsto1 + twopi; 1271 // } 1272 // else 1273 gsto = gstime_SGP4(epoch + 2433281.5); 1274 1275 //#include "debug5.cpp" 1276 } // initl 1277 1278 /*----------------------------------------------------------------------------- 1279 * 1280 * procedure sgp4init 1281 * 1282 * this procedure initializes variables for sgp4. 1283 * 1284 * author : david vallado 719-573-2600 28 jun 2005 1285 * 1286 * inputs : 1287 * opsmode - mode of operation afspc or improved 'a', 'i' 1288 * whichconst - which set of constants to use 72, 84 1289 * satn - satellite number 1290 * bstar - sgp4 type drag coefficient kg/m2er 1291 * ecco - eccentricity 1292 * epoch - epoch time in days from jan 0, 1950. 0 hr 1293 * argpo - argument of perigee (output if ds) 1294 * inclo - inclination 1295 * mo - mean anomaly (output if ds) 1296 * no - mean motion 1297 * nodeo - right ascension of ascending node 1298 * 1299 * outputs : 1300 * satrec - common values for subsequent calls 1301 * return code - non-zero on error. 1302 * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er 1303 * 2 - mean motion less than 0.0 1304 * 3 - pert elements, ecc < 0.0 or ecc > 1.0 1305 * 4 - semi-latus rectum < 0.0 1306 * 5 - epoch elements are sub-orbital 1307 * 6 - satellite has decayed 1308 * 1309 * locals : 1310 * cnodm , snodm , cosim , sinim , cosomm , sinomm 1311 * cc1sq , cc2 , cc3 1312 * coef , coef1 1313 * cosio4 - 1314 * day - 1315 * dndt - 1316 * em - eccentricity 1317 * emsq - eccentricity squared 1318 * eeta - 1319 * etasq - 1320 * gam - 1321 * argpm - argument of perigee 1322 * nodem - 1323 * inclm - inclination 1324 * mm - mean anomaly 1325 * nm - mean motion 1326 * perige - perigee 1327 * pinvsq - 1328 * psisq - 1329 * qzms24 - 1330 * rtemsq - 1331 * s1, s2, s3, s4, s5, s6, s7 - 1332 * sfour - 1333 * ss1, ss2, ss3, ss4, ss5, ss6, ss7 - 1334 * sz1, sz2, sz3 1335 * sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - 1336 * tc - 1337 * temp - 1338 * temp1, temp2, temp3 - 1339 * tsi - 1340 * xpidot - 1341 * xhdot1 - 1342 * z1, z2, z3 - 1343 * z11, z12, z13, z21, z22, z23, z31, z32, z33 - 1344 * 1345 * coupling : 1346 * getgravconst- 1347 * initl - 1348 * dscom - 1349 * dpper - 1350 * dsinit - 1351 * sgp4 - 1352 * 1353 * references : 1354 * hoots, roehrich, norad spacetrack report #3 1980 1355 * hoots, norad spacetrack report #6 1986 1356 * hoots, schumacher and glover 2004 1357 * vallado, crawford, hujsak, kelso 2006 1358 ----------------------------------------------------------------------------*/ 1359 sgp4init(gravconsttype whichconst,char opsmode,const char satn[5],const double epoch,const double xbstar,const double xndot,const double xnddot,const double xecco,const double xargpo,const double xinclo,const double xmo,const double xno_kozai,const double xnodeo,elsetrec & satrec)1360 bool sgp4init 1361 ( 1362 gravconsttype whichconst, char opsmode, const char satn[5], const double epoch, 1363 const double xbstar, const double xndot, const double xnddot, const double xecco, const double xargpo, 1364 const double xinclo, const double xmo, const double xno_kozai, 1365 const double xnodeo, elsetrec& satrec 1366 ) 1367 { 1368 /* --------------------- local variables ------------------------ */ 1369 double ao, ainv, con42, cosio, sinio, cosio2, eccsq, 1370 omeosq, posq, rp, rteosq, 1371 cnodm, snodm, cosim, sinim, cosomm, sinomm, cc1sq, 1372 cc2, cc3, coef, coef1, cosio4, day, dndt, 1373 em, emsq, eeta, etasq, gam, argpm, nodem, 1374 inclm, mm, nm, perige, pinvsq, psisq, qzms24, 1375 rtemsq, s1, s2, s3, s4, s5, s6, 1376 s7, sfour, ss1, ss2, ss3, ss4, ss5, 1377 ss6, ss7, sz1, sz2, sz3, sz11, sz12, 1378 sz13, sz21, sz22, sz23, sz31, sz32, sz33, 1379 tc, temp, temp1, temp2, temp3, tsi, xpidot, 1380 xhdot1, z1, z2, z3, z11, z12, z13, 1381 z21, z22, z23, z31, z32, z33, 1382 qzms2t, ss, x2o3, r[3], v[3], 1383 delmotemp, qzms2ttemp, qzms24temp; 1384 1385 /* ------------------------ initialization --------------------- */ 1386 // sgp4fix divisor for divide by zero check on inclination 1387 // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to 1388 // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency 1389 const double temp4 = 1.5e-12; 1390 1391 /* ----------- set all near earth variables to zero ------------ */ 1392 satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; 1393 satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; 1394 satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; 1395 satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; 1396 satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; 1397 satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; 1398 satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; 1399 satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; 1400 satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; 1401 1402 /* ----------- set all deep space variables to zero ------------ */ 1403 satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; 1404 satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; 1405 satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; 1406 satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; 1407 satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; 1408 satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; 1409 satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; 1410 satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; 1411 satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; 1412 satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; 1413 satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; 1414 satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; 1415 satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; 1416 satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; 1417 satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; 1418 satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; 1419 satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; 1420 satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; 1421 satrec.xli = 0.0; satrec.xni = 0.0; 1422 1423 /* ------------------------ earth constants ----------------------- */ 1424 // sgp4fix identify constants and allow alternate values 1425 // this is now the only call for the constants 1426 getgravconst(whichconst, satrec.tumin, satrec.mus, satrec.radiusearthkm, satrec.xke, 1427 satrec.j2, satrec.j3, satrec.j4, satrec.j3oj2); 1428 1429 //------------------------------------------------------------------------- 1430 1431 satrec.error = 0; 1432 satrec.operationmode = opsmode; 1433 // new alpha5 or 9-digit number 1434 #ifdef _MSC_VER 1435 strcpy_s(satrec.satnum, 6 * sizeof(char), satn); 1436 #else 1437 strcpy(satrec.satnum, satn); 1438 #endif 1439 1440 // sgp4fix - note the following variables are also passed directly via satrec. 1441 // it is possible to streamline the sgp4init call by deleting the "x" 1442 // variables, but the user would need to set the satrec.* values first. we 1443 // include the additional assignments in case twoline2rv is not used. 1444 satrec.bstar = xbstar; 1445 // sgp4fix allow additional parameters in the struct 1446 satrec.ndot = xndot; 1447 satrec.nddot = xnddot; 1448 satrec.ecco = xecco; 1449 satrec.argpo = xargpo; 1450 satrec.inclo = xinclo; 1451 satrec.mo = xmo; 1452 // sgp4fix rename variables to clarify which mean motion is intended 1453 satrec.no_kozai = xno_kozai; 1454 satrec.nodeo = xnodeo; 1455 1456 // single averaged mean elements 1457 satrec.am = satrec.em = satrec.im = satrec.Om = satrec.mm = satrec.nm = 0.0; 1458 1459 /* ------------------------ earth constants ----------------------- */ 1460 // sgp4fix identify constants and allow alternate values no longer needed 1461 // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 1462 ss = 78.0 / satrec.radiusearthkm + 1.0; 1463 // sgp4fix use multiply for speed instead of pow 1464 qzms2ttemp = (120.0 - 78.0) / satrec.radiusearthkm; 1465 qzms2t = qzms2ttemp * qzms2ttemp * qzms2ttemp * qzms2ttemp; 1466 x2o3 = 2.0 / 3.0; 1467 1468 satrec.init = 'y'; 1469 satrec.t = 0.0; 1470 1471 // sgp4fix remove satn as it is not needed in initl 1472 initl 1473 (satrec.xke, satrec.j2, satrec.ecco, epoch, satrec.inclo, satrec.no_kozai, satrec.operationmode, 1474 satrec.method, ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, 1475 posq, rp, rteosq, sinio, satrec.gsto, satrec.no_unkozai); 1476 satrec.a = pow(satrec.no_unkozai * satrec.tumin, (-2.0 / 3.0)); 1477 satrec.alta = satrec.a * (1.0 + satrec.ecco) - 1.0; 1478 satrec.altp = satrec.a * (1.0 - satrec.ecco) - 1.0; 1479 satrec.error = 0; 1480 1481 // sgp4fix remove this check as it is unnecessary 1482 // the mrt check in sgp4 handles decaying satellite cases even if the starting 1483 // condition is below the surface of te earth 1484 // if (rp < 1.0) 1485 // { 1486 // printf("# *** satn%d epoch elts sub-orbital ***\n", satn); 1487 // satrec.error = 5; 1488 // } 1489 1490 if ((omeosq >= 0.0) || (satrec.no_unkozai >= 0.0)) 1491 { 1492 satrec.isimp = 0; 1493 if (rp < (220.0 / satrec.radiusearthkm + 1.0)) 1494 satrec.isimp = 1; 1495 sfour = ss; 1496 qzms24 = qzms2t; 1497 perige = (rp - 1.0) * satrec.radiusearthkm; 1498 1499 /* - for perigees below 156 km, s and qoms2t are altered - */ 1500 if (perige < 156.0) 1501 { 1502 sfour = perige - 78.0; 1503 if (perige < 98.0) 1504 sfour = 20.0; 1505 // sgp4fix use multiply for speed instead of pow 1506 qzms24temp = (120.0 - sfour) / satrec.radiusearthkm; 1507 qzms24 = qzms24temp * qzms24temp * qzms24temp * qzms24temp; 1508 sfour = sfour / satrec.radiusearthkm + 1.0; 1509 } 1510 pinvsq = 1.0 / posq; 1511 1512 tsi = 1.0 / (ao - sfour); 1513 satrec.eta = ao * satrec.ecco * tsi; 1514 etasq = satrec.eta * satrec.eta; 1515 eeta = satrec.ecco * satrec.eta; 1516 psisq = fabs(1.0 - etasq); 1517 coef = qzms24 * pow(tsi, 4.0); 1518 coef1 = coef / pow(psisq, 3.5); 1519 cc2 = coef1 * satrec.no_unkozai * (ao * (1.0 + 1.5 * etasq + eeta * 1520 (4.0 + etasq)) + 0.375 * satrec.j2 * tsi / psisq * satrec.con41 * 1521 (8.0 + 3.0 * etasq * (8.0 + etasq))); 1522 satrec.cc1 = satrec.bstar * cc2; 1523 cc3 = 0.0; 1524 if (satrec.ecco > 1.0e-4) 1525 cc3 = -2.0 * coef * tsi * satrec.j3oj2 * satrec.no_unkozai * sinio / satrec.ecco; 1526 satrec.x1mth2 = 1.0 - cosio2; 1527 satrec.cc4 = 2.0* satrec.no_unkozai * coef1 * ao * omeosq * 1528 (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * 1529 (0.5 + 2.0 * etasq) - satrec.j2 * tsi / (ao * psisq) * 1530 (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * 1531 (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * 1532 (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); 1533 satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * 1534 (etasq + eeta) + eeta * etasq); 1535 cosio4 = cosio2 * cosio2; 1536 temp1 = 1.5 * satrec.j2 * pinvsq * satrec.no_unkozai; 1537 temp2 = 0.5 * temp1 * satrec.j2 * pinvsq; 1538 temp3 = -0.46875 * satrec.j4 * pinvsq * pinvsq * satrec.no_unkozai; 1539 satrec.mdot = satrec.no_unkozai + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * 1540 temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); 1541 satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * 1542 (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + 1543 temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); 1544 xhdot1 = -temp1 * cosio; 1545 satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + 1546 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; 1547 xpidot = satrec.argpdot + satrec.nodedot; 1548 satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); 1549 satrec.xmcof = 0.0; 1550 if (satrec.ecco > 1.0e-4) 1551 satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; 1552 satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; 1553 satrec.t2cof = 1.5 * satrec.cc1; 1554 // sgp4fix for divide by zero with xinco = 180 deg 1555 if (fabs(cosio + 1.0) > 1.5e-12) 1556 satrec.xlcof = -0.25 * satrec.j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); 1557 else 1558 satrec.xlcof = -0.25 * satrec.j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; 1559 satrec.aycof = -0.5 * satrec.j3oj2 * sinio; 1560 // sgp4fix use multiply for speed instead of pow 1561 delmotemp = 1.0 + satrec.eta * cos(satrec.mo); 1562 satrec.delmo = delmotemp * delmotemp * delmotemp; 1563 satrec.sinmao = sin(satrec.mo); 1564 satrec.x7thm1 = 7.0 * cosio2 - 1.0; 1565 1566 /* --------------- deep space initialization ------------- */ 1567 if ((2 * pi / satrec.no_unkozai) >= 225.0) 1568 { 1569 satrec.method = 'd'; 1570 satrec.isimp = 1; 1571 tc = 0.0; 1572 inclm = satrec.inclo; 1573 1574 dscom 1575 ( 1576 epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, 1577 satrec.no_unkozai, snodm, cnodm, sinim, cosim, sinomm, cosomm, 1578 day, satrec.e3, satrec.ee2, em, emsq, gam, 1579 satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, 1580 satrec.plo, rtemsq, satrec.se2, satrec.se3, 1581 satrec.sgh2, satrec.sgh3, satrec.sgh4, 1582 satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, 1583 satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, 1584 s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, 1585 sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, 1586 satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, 1587 satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, 1588 satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, 1589 z12, z13, z21, z22, z23, z31, z32, z33, 1590 satrec.zmol, satrec.zmos 1591 ); 1592 dpper 1593 ( 1594 satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, 1595 satrec.pho, satrec.pinco, satrec.plo, satrec.se2, 1596 satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, 1597 satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, 1598 satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, 1599 satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, 1600 satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, 1601 satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, 1602 satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo, 1603 satrec.operationmode 1604 ); 1605 1606 argpm = 0.0; 1607 nodem = 0.0; 1608 mm = 0.0; 1609 1610 dsinit 1611 ( 1612 satrec.xke, 1613 cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, 1614 ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, 1615 satrec.gsto, satrec.mo, satrec.mdot, satrec.no_unkozai, satrec.nodeo, 1616 satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, 1617 satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, 1618 satrec.irez, satrec.atime, 1619 satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222, 1620 satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, 1621 satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, 1622 satrec.dmdt, dndt, satrec.dnodt, satrec.domdt, 1623 satrec.del1, satrec.del2, satrec.del3, satrec.xfact, 1624 satrec.xlamo, satrec.xli, satrec.xni 1625 ); 1626 } 1627 1628 /* ----------- set variables if not deep space ----------- */ 1629 if (satrec.isimp != 1) 1630 { 1631 cc1sq = satrec.cc1 * satrec.cc1; 1632 satrec.d2 = 4.0 * ao * tsi * cc1sq; 1633 temp = satrec.d2 * tsi * satrec.cc1 / 3.0; 1634 satrec.d3 = (17.0 * ao + sfour) * temp; 1635 satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * 1636 satrec.cc1; 1637 satrec.t3cof = satrec.d2 + 2.0 * cc1sq; 1638 satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * 1639 (12.0 * satrec.d2 + 10.0 * cc1sq)); 1640 satrec.t5cof = 0.2 * (3.0 * satrec.d4 + 1641 12.0 * satrec.cc1 * satrec.d3 + 1642 6.0 * satrec.d2 * satrec.d2 + 1643 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); 1644 } 1645 } // if omeosq = 0 ... 1646 1647 /* finally propogate to zero epoch to initialize all others. */ 1648 // sgp4fix take out check to let satellites process until they are actually below earth surface 1649 // if(satrec.error == 0) 1650 sgp4(satrec, 0.0, r, v); 1651 1652 satrec.init = 'n'; 1653 1654 //#include "debug6.cpp" 1655 //sgp4fix return boolean. satrec.error contains any error codes 1656 return true; 1657 } // sgp4init 1658 1659 /*----------------------------------------------------------------------------- 1660 * 1661 * procedure sgp4 1662 * 1663 * this procedure is the sgp4 prediction model from space command. this is an 1664 * updated and combined version of sgp4 and sdp4, which were originally 1665 * published separately in spacetrack report #3. this version follows the 1666 * methodology from the aiaa paper (2006) describing the history and 1667 * development of the code. 1668 * 1669 * author : david vallado 719-573-2600 28 jun 2005 1670 * 1671 * inputs : 1672 * satrec - initialised structure from sgp4init() call. 1673 * tsince - time since epoch (minutes) 1674 * 1675 * outputs : 1676 * r - position vector km 1677 * v - velocity km/sec 1678 * return code - non-zero on error. 1679 * 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er 1680 * 2 - mean motion less than 0.0 1681 * 3 - pert elements, ecc < 0.0 or ecc > 1.0 1682 * 4 - semi-latus rectum < 0.0 1683 * 5 - epoch elements are sub-orbital 1684 * 6 - satellite has decayed 1685 * 1686 * locals : 1687 * am - 1688 * axnl, aynl - 1689 * betal - 1690 * cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , 1691 * sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , 1692 * cosisq , cossu , sinsu , cosu , sinu 1693 * delm - 1694 * delomg - 1695 * dndt - 1696 * eccm - 1697 * emsq - 1698 * ecose - 1699 * el2 - 1700 * eo1 - 1701 * eccp - 1702 * esine - 1703 * argpm - 1704 * argpp - 1705 * omgadf -c 1706 * pl - 1707 * r - 1708 * rtemsq - 1709 * rdotl - 1710 * rl - 1711 * rvdot - 1712 * rvdotl - 1713 * su - 1714 * t2 , t3 , t4 , tc 1715 * tem5, temp , temp1 , temp2 , tempa , tempe , templ 1716 * u , ux , uy , uz , vx , vy , vz 1717 * inclm - inclination 1718 * mm - mean anomaly 1719 * nm - mean motion 1720 * nodem - right asc of ascending node 1721 * xinc - 1722 * xincp - 1723 * xl - 1724 * xlm - 1725 * mp - 1726 * xmdf - 1727 * xmx - 1728 * xmy - 1729 * nodedf - 1730 * xnode - 1731 * nodep - 1732 * np - 1733 * 1734 * coupling : 1735 * getgravconst- no longer used. Variables are conatined within satrec 1736 * dpper 1737 * dpspace 1738 * 1739 * references : 1740 * hoots, roehrich, norad spacetrack report #3 1980 1741 * hoots, norad spacetrack report #6 1986 1742 * hoots, schumacher and glover 2004 1743 * vallado, crawford, hujsak, kelso 2006 1744 ----------------------------------------------------------------------------*/ 1745 sgp4(elsetrec & satrec,double tsince,double r[3],double v[3])1746 bool sgp4 1747 ( 1748 elsetrec& satrec, double tsince, 1749 double r[3], double v[3] 1750 ) 1751 { 1752 double am, axnl, aynl, betal, cosim, cnod, 1753 cos2u, coseo1, cosi, cosip, cosisq, cossu, cosu, 1754 delm, delomg, em, emsq, ecose, el2, eo1, 1755 ep, esine, argpm, argpp, argpdf, pl, mrt = 0.0, 1756 mvt, rdotl, rl, rvdot, rvdotl, sinim, 1757 sin2u, sineo1, sini, sinip, sinsu, sinu, 1758 snod, su, t2, t3, t4, tem5, temp, 1759 temp1, temp2, tempa, tempe, templ, u, ux, 1760 uy, uz, vx, vy, vz, inclm, mm, 1761 nm, nodem, xinc, xincp, xl, xlm, mp, 1762 xmdf, xmx, xmy, nodedf, xnode, nodep, tc, dndt, 1763 twopi, x2o3, vkmpersec, delmtemp; 1764 int ktr; 1765 1766 /* ------------------ set mathematical constants --------------- */ 1767 // sgp4fix divisor for divide by zero check on inclination 1768 // the old check used 1.0 + cos(pi-1.0e-9), but then compared it to 1769 // 1.5 e-12, so the threshold was changed to 1.5e-12 for consistency 1770 const double temp4 = 1.5e-12; 1771 twopi = 2.0 * pi; 1772 x2o3 = 2.0 / 3.0; 1773 // sgp4fix identify constants and allow alternate values 1774 // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 1775 vkmpersec = satrec.radiusearthkm * satrec.xke / 60.0; 1776 1777 /* --------------------- clear sgp4 error flag ----------------- */ 1778 satrec.t = tsince; 1779 satrec.error = 0; 1780 1781 /* ------- update for secular gravity and atmospheric drag ----- */ 1782 xmdf = satrec.mo + satrec.mdot * satrec.t; 1783 argpdf = satrec.argpo + satrec.argpdot * satrec.t; 1784 nodedf = satrec.nodeo + satrec.nodedot * satrec.t; 1785 argpm = argpdf; 1786 mm = xmdf; 1787 t2 = satrec.t * satrec.t; 1788 nodem = nodedf + satrec.nodecf * t2; 1789 tempa = 1.0 - satrec.cc1 * satrec.t; 1790 tempe = satrec.bstar * satrec.cc4 * satrec.t; 1791 templ = satrec.t2cof * t2; 1792 1793 if (satrec.isimp != 1) 1794 { 1795 delomg = satrec.omgcof * satrec.t; 1796 // sgp4fix use mutliply for speed instead of pow 1797 delmtemp = 1.0 + satrec.eta * cos(xmdf); 1798 delm = satrec.xmcof * 1799 (delmtemp * delmtemp * delmtemp - 1800 satrec.delmo); 1801 temp = delomg + delm; 1802 mm = xmdf + temp; 1803 argpm = argpdf - temp; 1804 t3 = t2 * satrec.t; 1805 t4 = t3 * satrec.t; 1806 tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - 1807 satrec.d4 * t4; 1808 tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - 1809 satrec.sinmao); 1810 templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + 1811 satrec.t * satrec.t5cof); 1812 } 1813 1814 nm = satrec.no_unkozai; 1815 em = satrec.ecco; 1816 inclm = satrec.inclo; 1817 if (satrec.method == 'd') 1818 { 1819 tc = satrec.t; 1820 dspace 1821 ( 1822 satrec.irez, 1823 satrec.d2201, satrec.d2211, satrec.d3210, 1824 satrec.d3222, satrec.d4410, satrec.d4422, 1825 satrec.d5220, satrec.d5232, satrec.d5421, 1826 satrec.d5433, satrec.dedt, satrec.del1, 1827 satrec.del2, satrec.del3, satrec.didt, 1828 satrec.dmdt, satrec.dnodt, satrec.domdt, 1829 satrec.argpo, satrec.argpdot, satrec.t, tc, 1830 satrec.gsto, satrec.xfact, satrec.xlamo, 1831 satrec.no_unkozai, satrec.atime, 1832 em, argpm, inclm, satrec.xli, mm, satrec.xni, 1833 nodem, dndt, nm 1834 ); 1835 } // if method = d 1836 1837 if (nm <= 0.0) 1838 { 1839 // printf("# error nm %f\n", nm); 1840 satrec.error = 2; 1841 // sgp4fix add return 1842 return false; 1843 } 1844 am = pow((satrec.xke / nm), x2o3) * tempa * tempa; 1845 nm = satrec.xke / pow(am, 1.5); 1846 em = em - tempe; 1847 1848 // fix tolerance for error recognition 1849 // sgp4fix am is fixed from the previous nm check 1850 if ((em >= 1.0) || (em < -0.001)/* || (am < 0.95)*/) 1851 { 1852 // printf("# error em %f\n", em); 1853 satrec.error = 1; 1854 // sgp4fix to return if there is an error in eccentricity 1855 return false; 1856 } 1857 // sgp4fix fix tolerance to avoid a divide by zero 1858 if (em < 1.0e-6) 1859 em = 1.0e-6; 1860 mm = mm + satrec.no_unkozai * templ; 1861 xlm = mm + argpm + nodem; 1862 emsq = em * em; 1863 temp = 1.0 - emsq; 1864 1865 nodem = fmod(nodem, twopi); 1866 argpm = fmod(argpm, twopi); 1867 xlm = fmod(xlm, twopi); 1868 mm = fmod(xlm - argpm - nodem, twopi); 1869 1870 // sgp4fix recover singly averaged mean elements 1871 satrec.am = am; 1872 satrec.em = em; 1873 satrec.im = inclm; 1874 satrec.Om = nodem; 1875 satrec.om = argpm; 1876 satrec.mm = mm; 1877 satrec.nm = nm; 1878 1879 /* ----------------- compute extra mean quantities ------------- */ 1880 sinim = sin(inclm); 1881 cosim = cos(inclm); 1882 1883 /* -------------------- add lunar-solar periodics -------------- */ 1884 ep = em; 1885 xincp = inclm; 1886 argpp = argpm; 1887 nodep = nodem; 1888 mp = mm; 1889 sinip = sinim; 1890 cosip = cosim; 1891 if (satrec.method == 'd') 1892 { 1893 dpper 1894 ( 1895 satrec.e3, satrec.ee2, satrec.peo, 1896 satrec.pgho, satrec.pho, satrec.pinco, 1897 satrec.plo, satrec.se2, satrec.se3, 1898 satrec.sgh2, satrec.sgh3, satrec.sgh4, 1899 satrec.sh2, satrec.sh3, satrec.si2, 1900 satrec.si3, satrec.sl2, satrec.sl3, 1901 satrec.sl4, satrec.t, satrec.xgh2, 1902 satrec.xgh3, satrec.xgh4, satrec.xh2, 1903 satrec.xh3, satrec.xi2, satrec.xi3, 1904 satrec.xl2, satrec.xl3, satrec.xl4, 1905 satrec.zmol, satrec.zmos, satrec.inclo, 1906 'n', ep, xincp, nodep, argpp, mp, satrec.operationmode 1907 ); 1908 if (xincp < 0.0) 1909 { 1910 xincp = -xincp; 1911 nodep = nodep + pi; 1912 argpp = argpp - pi; 1913 } 1914 if ((ep < 0.0) || (ep > 1.0)) 1915 { 1916 // printf("# error ep %f\n", ep); 1917 satrec.error = 3; 1918 // sgp4fix add return 1919 return false; 1920 } 1921 } // if method = d 1922 1923 /* -------------------- long period periodics ------------------ */ 1924 if (satrec.method == 'd') 1925 { 1926 sinip = sin(xincp); 1927 cosip = cos(xincp); 1928 satrec.aycof = -0.5*satrec.j3oj2*sinip; 1929 // sgp4fix for divide by zero for xincp = 180 deg 1930 if (fabs(cosip + 1.0) > 1.5e-12) 1931 satrec.xlcof = -0.25 * satrec.j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); 1932 else 1933 satrec.xlcof = -0.25 * satrec.j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; 1934 } 1935 axnl = ep * cos(argpp); 1936 temp = 1.0 / (am * (1.0 - ep * ep)); 1937 aynl = ep* sin(argpp) + temp * satrec.aycof; 1938 xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; 1939 1940 /* --------------------- solve kepler's equation --------------- */ 1941 u = fmod(xl - nodep, twopi); 1942 eo1 = u; 1943 tem5 = 9999.9; 1944 ktr = 1; 1945 // sgp4fix for kepler iteration 1946 // the following iteration needs better limits on corrections 1947 while ((fabs(tem5) >= 1.0e-12) && (ktr <= 10)) 1948 { 1949 sineo1 = sin(eo1); 1950 coseo1 = cos(eo1); 1951 tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; 1952 tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; 1953 if (fabs(tem5) >= 0.95) 1954 tem5 = tem5 > 0.0 ? 0.95 : -0.95; 1955 eo1 = eo1 + tem5; 1956 ktr = ktr + 1; 1957 } 1958 1959 /* ------------- short period preliminary quantities ----------- */ 1960 ecose = axnl*coseo1 + aynl*sineo1; 1961 esine = axnl*sineo1 - aynl*coseo1; 1962 el2 = axnl*axnl + aynl*aynl; 1963 pl = am*(1.0 - el2); 1964 if (pl < 0.0) 1965 { 1966 // printf("# error pl %f\n", pl); 1967 satrec.error = 4; 1968 // sgp4fix add return 1969 return false; 1970 } 1971 else 1972 { 1973 rl = am * (1.0 - ecose); 1974 rdotl = sqrt(am) * esine / rl; 1975 rvdotl = sqrt(pl) / rl; 1976 betal = sqrt(1.0 - el2); 1977 temp = esine / (1.0 + betal); 1978 sinu = am / rl * (sineo1 - aynl - axnl * temp); 1979 cosu = am / rl * (coseo1 - axnl + aynl * temp); 1980 su = atan2(sinu, cosu); 1981 sin2u = (cosu + cosu) * sinu; 1982 cos2u = 1.0 - 2.0 * sinu * sinu; 1983 temp = 1.0 / pl; 1984 temp1 = 0.5 * satrec.j2 * temp; 1985 temp2 = temp1 * temp; 1986 1987 /* -------------- update for short period periodics ------------ */ 1988 if (satrec.method == 'd') 1989 { 1990 cosisq = cosip * cosip; 1991 satrec.con41 = 3.0*cosisq - 1.0; 1992 satrec.x1mth2 = 1.0 - cosisq; 1993 satrec.x7thm1 = 7.0*cosisq - 1.0; 1994 } 1995 mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + 1996 0.5 * temp1 * satrec.x1mth2 * cos2u; 1997 su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; 1998 xnode = nodep + 1.5 * temp2 * cosip * sin2u; 1999 xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; 2000 mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / satrec.xke; 2001 rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + 2002 1.5 * satrec.con41) / satrec.xke; 2003 2004 /* --------------------- orientation vectors ------------------- */ 2005 sinsu = sin(su); 2006 cossu = cos(su); 2007 snod = sin(xnode); 2008 cnod = cos(xnode); 2009 sini = sin(xinc); 2010 cosi = cos(xinc); 2011 xmx = -snod * cosi; 2012 xmy = cnod * cosi; 2013 ux = xmx * sinsu + cnod * cossu; 2014 uy = xmy * sinsu + snod * cossu; 2015 uz = sini * sinsu; 2016 vx = xmx * cossu - cnod * sinsu; 2017 vy = xmy * cossu - snod * sinsu; 2018 vz = sini * cossu; 2019 2020 /* --------- position and velocity (in km and km/sec) ---------- */ 2021 r[0] = (mrt * ux)* satrec.radiusearthkm; 2022 r[1] = (mrt * uy)* satrec.radiusearthkm; 2023 r[2] = (mrt * uz)* satrec.radiusearthkm; 2024 v[0] = (mvt * ux + rvdot * vx) * vkmpersec; 2025 v[1] = (mvt * uy + rvdot * vy) * vkmpersec; 2026 v[2] = (mvt * uz + rvdot * vz) * vkmpersec; 2027 } // if pl > 0 2028 2029 // sgp4fix for decaying satellites 2030 if (mrt < 1.0) 2031 { 2032 // printf("# decay condition %11.6f \n",mrt); 2033 satrec.error = 6; 2034 return false; 2035 } 2036 2037 //#include "debug7.cpp" 2038 return true; 2039 } // sgp4 2040 2041 2042 2043 2044 2045 /* ----------------------------------------------------------------------------- 2046 * 2047 * function getgravconst 2048 * 2049 * this function gets constants for the propagator. note that mu is identified to 2050 * facilitiate comparisons with newer models. the common useage is wgs72. 2051 * 2052 * author : david vallado 719-573-2600 21 jul 2006 2053 * 2054 * inputs : 2055 * whichconst - which set of constants to use wgs72old, wgs72, wgs84 2056 * 2057 * outputs : 2058 * tumin - minutes in one time unit 2059 * mu - earth gravitational parameter 2060 * radiusearthkm - radius of the earth in km 2061 * xke - reciprocal of tumin 2062 * j2, j3, j4 - un-normalized zonal harmonic values 2063 * j3oj2 - j3 divided by j2 2064 * 2065 * locals : 2066 * 2067 * coupling : 2068 * none 2069 * 2070 * references : 2071 * norad spacetrack report #3 2072 * vallado, crawford, hujsak, kelso 2006 2073 --------------------------------------------------------------------------- */ 2074 getgravconst(gravconsttype whichconst,double & tumin,double & mus,double & radiusearthkm,double & xke,double & j2,double & j3,double & j4,double & j3oj2)2075 void getgravconst 2076 ( 2077 gravconsttype whichconst, 2078 double& tumin, 2079 double& mus, 2080 double& radiusearthkm, 2081 double& xke, 2082 double& j2, 2083 double& j3, 2084 double& j4, 2085 double& j3oj2 2086 ) 2087 { 2088 2089 switch (whichconst) 2090 { 2091 // -- wgs-72 low precision str#3 constants -- 2092 case wgs72old: 2093 mus = 398600.79964; // in km3 / s2 2094 radiusearthkm = 6378.135; // km 2095 xke = 0.0743669161; // reciprocal of tumin 2096 tumin = 1.0 / xke; 2097 j2 = 0.001082616; 2098 j3 = -0.00000253881; 2099 j4 = -0.00000165597; 2100 j3oj2 = j3 / j2; 2101 break; 2102 // ------------ wgs-72 constants ------------ 2103 case wgs72: 2104 mus = 398600.8; // in km3 / s2 2105 radiusearthkm = 6378.135; // km 2106 xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm / mus); 2107 tumin = 1.0 / xke; 2108 j2 = 0.001082616; 2109 j3 = -0.00000253881; 2110 j4 = -0.00000165597; 2111 j3oj2 = j3 / j2; 2112 break; 2113 case wgs84: 2114 // ------------ wgs-84 constants ------------ 2115 mus = 398600.5; // in km3 / s2 2116 radiusearthkm = 6378.137; // km 2117 xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm / mus); 2118 tumin = 1.0 / xke; 2119 j2 = 0.00108262998905; 2120 j3 = -0.00000253215306; 2121 j4 = -0.00000161098761; 2122 j3oj2 = j3 / j2; 2123 break; 2124 default: 2125 fprintf(stderr, "unknown gravity option (%d)\n", whichconst); 2126 break; 2127 } 2128 2129 } // getgravconst 2130 2131 // older sgp4io methods 2132 /* ----------------------------------------------------------------------------- 2133 * 2134 * function twoline2rv 2135 * 2136 * this function converts the two line element set character string data to 2137 * variables and initializes the sgp4 variables. several intermediate varaibles 2138 * and quantities are determined. note that the result is a structure so multiple 2139 * satellites can be processed simaltaneously without having to reinitialize. the 2140 * verification mode is an important option that permits quick checks of any 2141 * changes to the underlying technical theory. this option works using a 2142 * modified tle file in which the start, stop, and delta time values are 2143 * included at the end of the second line of data. this only works with the 2144 * verification mode. the catalog mode simply propagates from -1440 to 1440 min 2145 * from epoch and is useful when performing entire catalog runs. 2146 * 2147 * author : david vallado 719-573-2600 1 mar 2001 2148 * 2149 * inputs : 2150 * longstr1 - first line of the tle 2151 * longstr2 - second line of the tle 2152 * typerun - type of run verification 'v', catalog 'c', 2153 * manual 'm' 2154 * typeinput - type of manual input mfe 'm', epoch 'e', dayofyr 'd' 2155 * opsmode - mode of operation afspc or improved 'a', 'i' 2156 * whichconst - which set of constants to use 72, 84 2157 * 2158 * outputs : 2159 * satrec - structure containing all the sgp4 satellite information 2160 * 2161 * coupling : 2162 * getgravconst- 2163 * days2mdhms - conversion of days to month, day, hour, minute, second 2164 * jday - convert day month year hour minute second into julian date 2165 * sgp4init - initialize the sgp4 variables 2166 * 2167 * references : 2168 * norad spacetrack report #3 2169 * vallado, crawford, hujsak, kelso 2006 2170 --------------------------------------------------------------------------- */ 2171 twoline2rv(char longstr1[130],char longstr2[130],char typerun,char typeinput,char opsmode,gravconsttype whichconst,double & startmfe,double & stopmfe,double & deltamin,elsetrec & satrec)2172 void twoline2rv 2173 ( 2174 char longstr1[130], char longstr2[130], 2175 char typerun, char typeinput, char opsmode, 2176 gravconsttype whichconst, 2177 double& startmfe, double& stopmfe, double& deltamin, 2178 elsetrec& satrec 2179 ) 2180 { 2181 const double deg2rad = pi / 180.0; // 0.0174532925199433 2182 const double xpdotp = 1440.0 / (2.0 *pi); // 229.1831180523293 2183 2184 double sec; 2185 double startsec, stopsec, startdayofyr, stopdayofyr, jdstart, jdstop, jdstartF, jdstopF; 2186 int startyear, stopyear, startmon, stopmon, startday, stopday, 2187 starthr, stophr, startmin, stopmin; 2188 int cardnumb, j; 2189 // sgp4fix include in satrec 2190 // long revnum = 0, elnum = 0; 2191 // char classification, intldesg[11]; 2192 int year = 0; 2193 int mon, day, hr, minute, nexp, ibexp; 2194 2195 // sgp4fix no longer needed 2196 // getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); 2197 2198 satrec.error = 0; 2199 2200 // set the implied decimal points since doing a formated read 2201 // fixes for bad input data values (missing, ...) 2202 for (j = 10; j <= 15; j++) 2203 if (longstr1[j] == ' ') 2204 longstr1[j] = '_'; 2205 2206 if (longstr1[44] != ' ') 2207 longstr1[43] = longstr1[44]; 2208 longstr1[44] = '.'; 2209 if (longstr1[7] == ' ') 2210 longstr1[7] = 'U'; 2211 if (longstr1[9] == ' ') 2212 longstr1[9] = '.'; 2213 for (j = 45; j <= 49; j++) 2214 if (longstr1[j] == ' ') 2215 longstr1[j] = '0'; 2216 if (longstr1[51] == ' ') 2217 longstr1[51] = '0'; 2218 if (longstr1[53] != ' ') 2219 longstr1[52] = longstr1[53]; 2220 longstr1[53] = '.'; 2221 longstr2[25] = '.'; 2222 for (j = 26; j <= 32; j++) 2223 if (longstr2[j] == ' ') 2224 longstr2[j] = '0'; 2225 if (longstr1[62] == ' ') 2226 longstr1[62] = '0'; 2227 if (longstr1[68] == ' ') 2228 longstr1[68] = '0'; 2229 #ifdef _MSC_VER // chk if compiling in MSVS c++ 2230 sscanf_s(longstr1, "%2d %5s %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ", 2231 &cardnumb, &satrec.satnum, 6 * sizeof(char), &satrec.classification, sizeof(char), &satrec.intldesg, 11 * sizeof(char), &satrec.epochyr, 2232 &satrec.epochdays, &satrec.ndot, &satrec.nddot, &nexp, &satrec.bstar, &ibexp, &satrec.ephtype, &satrec.elnum); 2233 #else 2234 sscanf(longstr1, "%2d %5s %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ", 2235 &cardnumb, &satrec.satnum, &satrec.classification, &satrec.intldesg, &satrec.epochyr, 2236 &satrec.epochdays, &satrec.ndot, &satrec.nddot, &nexp, &satrec.bstar, 2237 &ibexp, &satrec.ephtype, &satrec.elnum); 2238 #endif 2239 if (typerun == 'v') // run for specified times from the file 2240 { 2241 if (longstr2[52] == ' ') 2242 { 2243 #ifdef _MSC_VER 2244 sscanf_s(longstr2, "%2d %5s %9lf %9lf %8lf %9lf %9lf %10lf %6ld %lf %lf %lf \n", 2245 &cardnumb, &satrec.satnum, 6 * sizeof(char), &satrec.inclo, 2246 &satrec.nodeo, &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no_kozai, 2247 &satrec.revnum, &startmfe, &stopmfe, &deltamin); 2248 #else 2249 sscanf(longstr2, "%2d %5s %9lf %9lf %8lf %9lf %9lf %10lf %6ld %lf %lf %lf \n", 2250 &cardnumb, &satrec.satnum, &satrec.inclo, 2251 &satrec.nodeo, &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no_kozai, 2252 &satrec.revnum, &startmfe, &stopmfe, &deltamin); 2253 #endif 2254 } 2255 else 2256 { 2257 #ifdef _MSC_VER 2258 sscanf_s(longstr2, "%2d %5s %9lf %9lf %8lf %9lf %9lf %11lf %6ld %lf %lf %lf \n", 2259 &cardnumb, &satrec.satnum, 6 * sizeof(char), &satrec.inclo, 2260 &satrec.nodeo, &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no_kozai, 2261 &satrec.revnum, &startmfe, &stopmfe, &deltamin); 2262 #else 2263 sscanf(longstr2, "%2d %5s %9lf %9lf %8lf %9lf %9lf %11lf %6ld %lf %lf %lf \n", 2264 &cardnumb, &satrec.satnum, &satrec.inclo, 2265 &satrec.nodeo, &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no_kozai, 2266 &satrec.revnum, &startmfe, &stopmfe, &deltamin); 2267 #endif 2268 } 2269 } 2270 else // simply run -1 day to +1 day or user input times 2271 { 2272 if (longstr2[52] == ' ') 2273 { 2274 #ifdef _MSC_VER 2275 sscanf_s(longstr2, "%2d %5s %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n", 2276 &cardnumb, &satrec.satnum, 6 * sizeof(char), &satrec.inclo, 2277 &satrec.nodeo, &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no_kozai, 2278 &satrec.revnum); 2279 #else 2280 sscanf(longstr2, "%2d %5s %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n", 2281 &cardnumb, &satrec.satnum, &satrec.inclo, 2282 &satrec.nodeo, &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no_kozai, 2283 &satrec.revnum); 2284 #endif 2285 } 2286 else 2287 { 2288 #ifdef _MSC_VER 2289 sscanf_s(longstr2, "%2d %5s %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n", 2290 &cardnumb, &satrec.satnum, 6 * sizeof(char), &satrec.inclo, 2291 &satrec.nodeo, &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no_kozai, 2292 &satrec.revnum); 2293 #else 2294 sscanf(longstr2, "%2d %5s %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n", 2295 &cardnumb, &satrec.satnum, &satrec.inclo, 2296 &satrec.nodeo, &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no_kozai, 2297 &satrec.revnum); 2298 #endif 2299 } 2300 } 2301 2302 // ---- find no, ndot, nddot ---- 2303 satrec.no_kozai = satrec.no_kozai / xpdotp; //* rad/min 2304 satrec.nddot = satrec.nddot * pow(10.0, nexp); 2305 satrec.bstar = satrec.bstar * pow(10.0, ibexp); 2306 2307 // ---- convert to sgp4 units ---- 2308 // satrec.a = pow( satrec.no_kozai*tumin , (-2.0/3.0) ); 2309 satrec.ndot = satrec.ndot / (xpdotp*1440.0); //* ? * minperday 2310 satrec.nddot = satrec.nddot / (xpdotp*1440.0 * 1440); 2311 2312 // ---- find standard orbital elements ---- 2313 satrec.inclo = satrec.inclo * deg2rad; 2314 satrec.nodeo = satrec.nodeo * deg2rad; 2315 satrec.argpo = satrec.argpo * deg2rad; 2316 satrec.mo = satrec.mo * deg2rad; 2317 2318 // sgp4fix not needed here 2319 // satrec.alta = satrec.a*(1.0 + satrec.ecco) - 1.0; 2320 // satrec.altp = satrec.a*(1.0 - satrec.ecco) - 1.0; 2321 2322 // ---------------------------------------------------------------- 2323 // find sgp4epoch time of element set 2324 // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch) 2325 // and minutes from the epoch (time) 2326 // ---------------------------------------------------------------- 2327 2328 // ---------------- temp fix for years from 1957-2056 ------------------- 2329 // --------- correct fix will occur when year is 4-digit in tle --------- 2330 if (satrec.epochyr < 57) 2331 year = satrec.epochyr + 2000; 2332 else 2333 year = satrec.epochyr + 1900; 2334 2335 days2mdhms_SGP4(year, satrec.epochdays, mon, day, hr, minute, sec); 2336 jday_SGP4(year, mon, day, hr, minute, sec, satrec.jdsatepoch, satrec.jdsatepochF); 2337 2338 // ---- input start stop times manually 2339 if ((typerun != 'v') && (typerun != 'c')) 2340 { 2341 // ------------- enter start/stop ymd hms values -------------------- 2342 if (typeinput == 'e') 2343 { 2344 printf("input start prop year mon day hr min sec \n"); 2345 // make sure there is no space at the end of the format specifiers in scanf! 2346 #ifdef _MSC_VER 2347 scanf_s("%i %i %i %i %i %lf", &startyear, &startmon, &startday, &starthr, &startmin, &startsec); 2348 #else 2349 scanf("%i %i %i %i %i %lf", &startyear, &startmon, &startday, &starthr, &startmin, &startsec); 2350 #endif 2351 fflush(stdin); 2352 jday_SGP4(startyear, startmon, startday, starthr, startmin, startsec, jdstart, jdstartF); 2353 2354 printf("input stop prop year mon day hr min sec \n"); 2355 #ifdef _MSC_VER 2356 scanf_s("%i %i %i %i %i %lf", &stopyear, &stopmon, &stopday, &stophr, &stopmin, &stopsec); 2357 #else 2358 scanf("%i %i %i %i %i %lf", &stopyear, &stopmon, &stopday, &stophr, &stopmin, &stopsec); 2359 #endif 2360 fflush(stdin); 2361 jday_SGP4(stopyear, stopmon, stopday, stophr, stopmin, stopsec, jdstop, jdstopF); 2362 2363 startmfe = (jdstart - satrec.jdsatepoch) * 1440.0 + (jdstartF - satrec.jdsatepochF) * 1440.0; 2364 stopmfe = (jdstop - satrec.jdsatepoch) * 1440.0 + (jdstopF - satrec.jdsatepochF) * 1440.0; 2365 2366 printf("input time step in minutes \n"); 2367 #ifdef _MSC_VER 2368 scanf_s("%lf", &deltamin); 2369 #else 2370 scanf("%lf", &deltamin); 2371 #endif 2372 } 2373 // -------- enter start/stop year and days of year values ----------- 2374 if (typeinput == 'd') 2375 { 2376 printf("input start year dayofyr \n"); 2377 #ifdef _MSC_VER 2378 scanf_s("%i %lf", &startyear, &startdayofyr); 2379 #else 2380 scanf("%i %lf", &startyear, &startdayofyr); 2381 #endif 2382 printf("input stop year dayofyr \n"); 2383 #ifdef _MSC_VER 2384 scanf_s("%i %lf", &stopyear, &stopdayofyr); 2385 #else 2386 scanf("%i %lf", &stopyear, &stopdayofyr); 2387 #endif 2388 2389 days2mdhms_SGP4(startyear, startdayofyr, mon, day, hr, minute, sec); 2390 jday_SGP4(startyear, mon, day, hr, minute, sec, jdstart, jdstartF); 2391 days2mdhms_SGP4(stopyear, stopdayofyr, mon, day, hr, minute, sec); 2392 jday_SGP4(stopyear, mon, day, hr, minute, sec, jdstop, jdstopF); 2393 2394 startmfe = (jdstart - satrec.jdsatepoch) * 1440.0 + (jdstartF - satrec.jdsatepochF) * 1440.0; 2395 stopmfe = (jdstop - satrec.jdsatepoch) * 1440.0 + (jdstopF - satrec.jdsatepochF) * 1440.0; 2396 2397 printf("input time step in minutes \n"); 2398 #ifdef _MSC_VER 2399 scanf_s("%lf", &deltamin); 2400 #else 2401 2402 scanf("%lf", &deltamin); 2403 #endif 2404 } 2405 // ------------------ enter start/stop mfe values ------------------- 2406 if (typeinput == 'm') 2407 { 2408 #ifdef _MSC_VER 2409 printf("input start min from epoch \n"); 2410 scanf_s("%lf", &startmfe); 2411 printf("input stop min from epoch \n"); 2412 scanf_s("%lf", &stopmfe); 2413 printf("input time step in minutes \n"); 2414 scanf_s("%lf", &deltamin); 2415 #else 2416 printf("input start min from epoch \n"); 2417 scanf("%lf", &startmfe); 2418 printf("input stop min from epoch \n"); 2419 scanf("%lf", &stopmfe); 2420 printf("input time step in minutes \n"); 2421 scanf("%lf", &deltamin); 2422 #endif 2423 } 2424 } 2425 2426 // ------------ perform complete catalog evaluation, -+ 1 day ----------- 2427 if (typerun == 'c') 2428 { 2429 startmfe = -1440.0; 2430 stopmfe = 1440.0; 2431 deltamin = 10.0; 2432 } 2433 2434 // ---------------- initialize the orbit at sgp4epoch ------------------- 2435 sgp4init(whichconst, opsmode, satrec.satnum, (satrec.jdsatepoch + satrec.jdsatepochF) - 2433281.5, satrec.bstar, 2436 satrec.ndot, satrec.nddot, satrec.ecco, satrec.argpo, satrec.inclo, satrec.mo, satrec.no_kozai, 2437 satrec.nodeo, satrec); 2438 } // twoline2rv 2439 2440 2441 // older sgp4ext methods 2442 /* ----------------------------------------------------------------------------- 2443 * 2444 * function gstime_SGP4 2445 * 2446 * this function finds the greenwich sidereal time. 2447 * 2448 * author : david vallado 719-573-2600 1 mar 2001 2449 * 2450 * inputs description range / units 2451 * jdut1 - julian date in ut1 days from 4713 bc 2452 * 2453 * outputs : 2454 * gstime - greenwich sidereal time 0 to 2pi rad 2455 * 2456 * locals : 2457 * temp - temporary variable for doubles rad 2458 * tut1 - julian centuries from the 2459 * jan 1, 2000 12 h epoch (ut1) 2460 * 2461 * coupling : 2462 * none 2463 * 2464 * references : 2465 * vallado 2013, 187, eq 3-45 2466 * --------------------------------------------------------------------------- */ 2467 gstime_SGP4(double jdut1)2468 double gstime_SGP4 2469 ( 2470 double jdut1 2471 ) 2472 { 2473 const double twopi = 2.0 * pi; 2474 const double deg2rad = pi / 180.0; 2475 double temp, tut1; 2476 2477 tut1 = (jdut1 - 2451545.0) / 36525.0; 2478 temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + 2479 (876600.0 * 3600 + 8640184.812866) * tut1 + 67310.54841; // sec 2480 temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad 2481 2482 // ------------------------ check quadrants --------------------- 2483 if (temp < 0.0) 2484 temp += twopi; 2485 2486 return temp; 2487 } // gstime 2488 sgn_SGP4(double x)2489 double sgn_SGP4 2490 ( 2491 double x 2492 ) 2493 { 2494 if (x < 0.0) 2495 { 2496 return -1.0; 2497 } 2498 else 2499 { 2500 return 1.0; 2501 } 2502 2503 } // sgn 2504 2505 /* ----------------------------------------------------------------------------- 2506 * 2507 * function mag_SGP4 2508 * 2509 * this procedure finds the magnitude of a vector. 2510 * 2511 * author : david vallado 719-573-2600 1 mar 2001 2512 * 2513 * inputs description range / units 2514 * vec - vector 2515 * 2516 * outputs : 2517 * mag - answer 2518 * 2519 * locals : 2520 * none. 2521 * 2522 * coupling : 2523 * none. 2524 * --------------------------------------------------------------------------- */ 2525 mag_SGP4(double x[3])2526 double mag_SGP4 2527 ( 2528 double x[3] 2529 ) 2530 { 2531 return sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); 2532 } // mag 2533 2534 /* ----------------------------------------------------------------------------- 2535 * 2536 * procedure cross_SGP4 2537 * 2538 * this procedure crosses two vectors. 2539 * 2540 * author : david vallado 719-573-2600 1 mar 2001 2541 * 2542 * inputs description range / units 2543 * vec1 - vector number 1 2544 * vec2 - vector number 2 2545 * 2546 * outputs : 2547 * outvec - vector result of a x b 2548 * 2549 * locals : 2550 * none. 2551 * 2552 * coupling : 2553 * mag magnitude of a vector 2554 ---------------------------------------------------------------------------- */ 2555 cross_SGP4(double vec1[3],double vec2[3],double outvec[3])2556 void cross_SGP4 2557 ( 2558 double vec1[3], double vec2[3], double outvec[3] 2559 ) 2560 { 2561 outvec[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1]; 2562 outvec[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2]; 2563 outvec[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0]; 2564 } // end cross 2565 2566 2567 /* ----------------------------------------------------------------------------- 2568 * 2569 * function dot_SGP4 2570 * 2571 * this function finds the dot product of two vectors. 2572 * 2573 * author : david vallado 719-573-2600 1 mar 2001 2574 * 2575 * inputs description range / units 2576 * vec1 - vector number 1 2577 * vec2 - vector number 2 2578 * 2579 * outputs : 2580 * dot - result 2581 * 2582 * locals : 2583 * none. 2584 * 2585 * coupling : 2586 * none. 2587 * --------------------------------------------------------------------------- */ 2588 dot_SGP4(double x[3],double y[3])2589 double dot_SGP4 2590 ( 2591 double x[3], double y[3] 2592 ) 2593 { 2594 return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); 2595 } // dot 2596 2597 /* ----------------------------------------------------------------------------- 2598 * 2599 * procedure angle_SGP4 2600 * 2601 * this procedure calculates the angle between two vectors. the output is 2602 * set to 999999.1 to indicate an undefined value. be sure to check for 2603 * this at the output phase. 2604 * 2605 * author : david vallado 719-573-2600 1 mar 2001 2606 * 2607 * inputs description range / units 2608 * vec1 - vector number 1 2609 * vec2 - vector number 2 2610 * 2611 * outputs : 2612 * theta - angle between the two vectors -pi to pi 2613 * 2614 * locals : 2615 * temp - temporary real variable 2616 * 2617 * coupling : 2618 * dot dot product of two vectors 2619 * --------------------------------------------------------------------------- */ 2620 angle_SGP4(double vec1[3],double vec2[3])2621 double angle_SGP4 2622 ( 2623 double vec1[3], 2624 double vec2[3] 2625 ) 2626 { 2627 double small, undefined, magv1, magv2, temp; 2628 small = 0.00000001; 2629 undefined = 999999.1; 2630 2631 magv1 = mag_SGP4(vec1); 2632 magv2 = mag_SGP4(vec2); 2633 2634 if (magv1*magv2 > small*small) 2635 { 2636 temp = dot_SGP4(vec1, vec2) / (magv1*magv2); 2637 if (fabs(temp) > 1.0) 2638 temp = sgn_SGP4(temp) * 1.0; 2639 return acos(temp); 2640 } 2641 else 2642 return undefined; 2643 } // angle 2644 2645 2646 /* ----------------------------------------------------------------------------- 2647 * 2648 * function asinh_SGP4 2649 * 2650 * this function evaluates the inverse hyperbolic sine function. 2651 * 2652 * author : david vallado 719-573-2600 1 mar 2001 2653 * 2654 * inputs description range / units 2655 * xval - angle value any real 2656 * 2657 * outputs : 2658 * arcsinh - result any real 2659 * 2660 * locals : 2661 * none. 2662 * 2663 * coupling : 2664 * none. 2665 * --------------------------------------------------------------------------- */ 2666 asinh_SGP4(double xval)2667 double asinh_SGP4 2668 ( 2669 double xval 2670 ) 2671 { 2672 return log(xval + sqrt(xval*xval + 1.0)); 2673 } // asinh 2674 2675 2676 /* ----------------------------------------------------------------------------- 2677 * 2678 * function newtonnu_SGP4 2679 * 2680 * this function solves keplers equation when the true anomaly is known. 2681 * the mean and eccentric, parabolic, or hyperbolic anomaly is also found. 2682 * the parabolic limit at 168� is arbitrary. the hyperbolic anomaly is also 2683 * limited. the hyperbolic sine is used because it's not double valued. 2684 * 2685 * author : david vallado 719-573-2600 27 may 2002 2686 * 2687 * revisions 2688 * vallado - fix small 24 sep 2002 2689 * 2690 * inputs description range / units 2691 * ecc - eccentricity 0.0 to 2692 * nu - true anomaly -2pi to 2pi rad 2693 * 2694 * outputs : 2695 * e0 - eccentric anomaly 0.0 to 2pi rad 153.02 � 2696 * m - mean anomaly 0.0 to 2pi rad 151.7425 � 2697 * 2698 * locals : 2699 * e1 - eccentric anomaly, next value rad 2700 * sine - sine of e 2701 * cose - cosine of e 2702 * ktr - index 2703 * 2704 * coupling : 2705 * asinh - arc hyperbolic sine 2706 * 2707 * references : 2708 * vallado 2013, 77, alg 5 2709 * --------------------------------------------------------------------------- */ 2710 newtonnu_SGP4(double ecc,double nu,double & e0,double & m)2711 void newtonnu_SGP4 2712 ( 2713 double ecc, double nu, 2714 double& e0, double& m 2715 ) 2716 { 2717 double small, sine, cose; 2718 2719 // --------------------- implementation --------------------- 2720 e0 = 999999.9; 2721 m = 999999.9; 2722 small = 0.00000001; 2723 2724 // --------------------------- circular ------------------------ 2725 if (fabs(ecc) < small) 2726 { 2727 m = nu; 2728 e0 = nu; 2729 } 2730 else 2731 // ---------------------- elliptical ----------------------- 2732 if (ecc < 1.0 - small) 2733 { 2734 sine = (sqrt(1.0 - ecc*ecc) * sin(nu)) / (1.0 + ecc*cos(nu)); 2735 cose = (ecc + cos(nu)) / (1.0 + ecc*cos(nu)); 2736 e0 = atan2(sine, cose); 2737 m = e0 - ecc*sin(e0); 2738 } 2739 else 2740 // -------------------- hyperbolic -------------------- 2741 if (ecc > 1.0 + small) 2742 { 2743 if ((ecc > 1.0) && (fabs(nu) + 0.00001 < pi - acos(1.0 / ecc))) 2744 { 2745 sine = (sqrt(ecc*ecc - 1.0) * sin(nu)) / (1.0 + ecc*cos(nu)); 2746 e0 = asinh_SGP4(sine); 2747 m = ecc*sinh(e0) - e0; 2748 } 2749 } 2750 else 2751 // ----------------- parabolic --------------------- 2752 if (fabs(nu) < 168.0*pi / 180.0) 2753 { 2754 e0 = tan(nu*0.5); 2755 m = e0 + (e0*e0*e0) / 3.0; 2756 } 2757 2758 if (ecc < 1.0) 2759 { 2760 m = fmod(m, 2.0 *pi); 2761 if (m < 0.0) 2762 m = m + 2.0 *pi; 2763 e0 = fmod(e0, 2.0 *pi); 2764 } 2765 } // newtonnu 2766 2767 2768 /* ----------------------------------------------------------------------------- 2769 * 2770 * function rv2coe_SGP4 2771 * 2772 * this function finds the classical orbital elements given the geocentric 2773 * equatorial position and velocity vectors. 2774 * 2775 * author : david vallado 719-573-2600 21 jun 2002 2776 * 2777 * revisions 2778 * vallado - fix special cases 5 sep 2002 2779 * vallado - delete extra check in inclination code 16 oct 2002 2780 * vallado - add constant file use 29 jun 2003 2781 * vallado - add mu 2 apr 2007 2782 * 2783 * inputs description range / units 2784 * r - ijk position vector km 2785 * v - ijk velocity vector km / s 2786 * mu - gravitational parameter km3 / s2 2787 * 2788 * outputs : 2789 * p - semilatus rectum km 2790 * a - semimajor axis km 2791 * ecc - eccentricity 2792 * incl - inclination 0.0 to pi rad 2793 * omega - right ascension of ascending node 0.0 to 2pi rad 2794 * argp - argument of perigee 0.0 to 2pi rad 2795 * nu - true anomaly 0.0 to 2pi rad 2796 * m - mean anomaly 0.0 to 2pi rad 2797 * arglat - argument of latitude (ci) 0.0 to 2pi rad 2798 * truelon - true longitude (ce) 0.0 to 2pi rad 2799 * lonper - longitude of periapsis (ee) 0.0 to 2pi rad 2800 * 2801 * locals : 2802 * hbar - angular momentum h vector km2 / s 2803 * ebar - eccentricity e vector 2804 * nbar - line of nodes n vector 2805 * c1 - v**2 - u/r 2806 * rdotv - r dot v 2807 * hk - hk unit vector 2808 * sme - specfic mechanical energy km2 / s2 2809 * i - index 2810 * e - eccentric, parabolic, 2811 * hyperbolic anomaly rad 2812 * temp - temporary variable 2813 * typeorbit - type of orbit ee, ei, ce, ci 2814 * 2815 * coupling : 2816 * mag - magnitude of a vector 2817 * cross - cross product of two vectors 2818 * angle - find the angle between two vectors 2819 * newtonnu - find the mean anomaly 2820 * 2821 * references : 2822 * vallado 2013, 113, alg 9, ex 2-5 2823 * --------------------------------------------------------------------------- */ 2824 rv2coe_SGP4(double r[3],double v[3],double mus,double & p,double & a,double & ecc,double & incl,double & omega,double & argp,double & nu,double & m,double & arglat,double & truelon,double & lonper)2825 void rv2coe_SGP4 2826 ( 2827 double r[3], double v[3], double mus, 2828 double& p, double& a, double& ecc, double& incl, double& omega, double& argp, 2829 double& nu, double& m, double& arglat, double& truelon, double& lonper 2830 ) 2831 { 2832 double undefined, small, hbar[3], nbar[3], magr, magv, magn, ebar[3], sme, 2833 rdotv, infinite, temp, c1, hk, twopi, magh, halfpi, e; 2834 2835 int i; 2836 // switch this to an integer msvs seems to have probelms with this and strncpy_s 2837 //char typeorbit[2]; 2838 int typeorbit; 2839 // here 2840 // typeorbit = 1 = 'ei' 2841 // typeorbit = 2 = 'ce' 2842 // typeorbit = 3 = 'ci' 2843 // typeorbit = 4 = 'ee' 2844 2845 twopi = 2.0 * pi; 2846 halfpi = 0.5 * pi; 2847 small = 0.00000001; 2848 undefined = 999999.1; 2849 infinite = 999999.9; 2850 2851 // ------------------------- implementation ----------------- 2852 magr = mag_SGP4(r); 2853 magv = mag_SGP4(v); 2854 2855 // ------------------ find h n and e vectors ---------------- 2856 cross_SGP4(r, v, hbar); 2857 magh = mag_SGP4(hbar); 2858 if (magh > small) 2859 { 2860 nbar[0] = -hbar[1]; 2861 nbar[1] = hbar[0]; 2862 nbar[2] = 0.0; 2863 magn = mag_SGP4(nbar); 2864 c1 = magv*magv - mus / magr; 2865 rdotv = dot_SGP4(r, v); 2866 for (i = 0; i <= 2; i++) 2867 ebar[i] = (c1*r[i] - rdotv*v[i]) / mus; 2868 ecc = mag_SGP4(ebar); 2869 2870 // ------------ find a e and semi-latus rectum ---------- 2871 sme = (magv*magv*0.5) - (mus / magr); 2872 if (fabs(sme) > small) 2873 a = -mus / (2.0 *sme); 2874 else 2875 a = infinite; 2876 p = magh*magh / mus; 2877 2878 // ----------------- find inclination ------------------- 2879 hk = hbar[2] / magh; 2880 incl = acos(hk); 2881 2882 // -------- determine type of orbit for later use -------- 2883 // ------ elliptical, parabolic, hyperbolic inclined ------- 2884 //#ifdef _MSC_VER // chk if compiling under MSVS 2885 // strcpy_s(typeorbit, 2 * sizeof(char), "ei"); 2886 //#else 2887 // strcpy(typeorbit, "ei"); 2888 //#endif 2889 typeorbit = 1; 2890 2891 if (ecc < small) 2892 { 2893 // ---------------- circular equatorial --------------- 2894 if ((incl < small) | (fabs(incl - pi) < small)) 2895 { 2896 //#ifdef _MSC_VER 2897 // strcpy_s(typeorbit, sizeof(typeorbit), "ce"); 2898 //#else 2899 // strcpy(typeorbit, "ce"); 2900 //#endif 2901 typeorbit = 2; 2902 } 2903 else 2904 { 2905 // -------------- circular inclined --------------- 2906 //#ifdef _MSC_VER 2907 // strcpy_s(typeorbit, sizeof(typeorbit), "ci"); 2908 //#else 2909 // strcpy(typeorbit, "ci"); 2910 //#endif 2911 typeorbit = 3; 2912 } 2913 } 2914 else 2915 { 2916 // - elliptical, parabolic, hyperbolic equatorial -- 2917 if ((incl < small) | (fabs(incl - pi) < small)){ 2918 //#ifdef _MSC_VER 2919 // strcpy_s(typeorbit, sizeof(typeorbit), "ee"); 2920 //#else 2921 // strcpy(typeorbit, "ee"); 2922 //#endif 2923 typeorbit = 4; 2924 } 2925 } 2926 2927 // ---------- find right ascension of the ascending node ------------ 2928 if (magn > small) 2929 { 2930 temp = nbar[0] / magn; 2931 if (fabs(temp) > 1.0) 2932 temp = sgn_SGP4(temp); 2933 omega = acos(temp); 2934 if (nbar[1] < 0.0) 2935 omega = twopi - omega; 2936 } 2937 else 2938 omega = undefined; 2939 2940 // ---------------- find argument of perigee --------------- 2941 //if (strcmp(typeorbit, "ei") == 0) 2942 if (typeorbit == 1) 2943 { 2944 argp = angle_SGP4(nbar, ebar); 2945 if (ebar[2] < 0.0) 2946 argp = twopi - argp; 2947 } 2948 else 2949 argp = undefined; 2950 2951 // ------------ find true anomaly at epoch ------------- 2952 //if (typeorbit[0] == 'e') 2953 if ((typeorbit == 1) || (typeorbit == 4)) 2954 { 2955 nu = angle_SGP4(ebar, r); 2956 if (rdotv < 0.0) 2957 nu = twopi - nu; 2958 } 2959 else 2960 nu = undefined; 2961 2962 // ---- find argument of latitude - circular inclined ----- 2963 //if (strcmp(typeorbit, "ci") == 0) 2964 if (typeorbit == 3) 2965 { 2966 arglat = angle_SGP4(nbar, r); 2967 if (r[2] < 0.0) 2968 arglat = twopi - arglat; 2969 m = arglat; 2970 } 2971 else 2972 arglat = undefined; 2973 2974 // -- find longitude of perigee - elliptical equatorial ---- 2975 //if ((ecc>small) && (strcmp(typeorbit, "ee") == 0)) 2976 if ((ecc>small) && (typeorbit == 4)) 2977 { 2978 temp = ebar[0] / ecc; 2979 if (fabs(temp) > 1.0) 2980 temp = sgn_SGP4(temp); 2981 lonper = acos(temp); 2982 if (ebar[1] < 0.0) 2983 lonper = twopi - lonper; 2984 if (incl > halfpi) 2985 lonper = twopi - lonper; 2986 } 2987 else 2988 lonper = undefined; 2989 2990 // -------- find true longitude - circular equatorial ------ 2991 //if ((magr>small) && (strcmp(typeorbit, "ce") == 0)) 2992 if ((magr > small) && (typeorbit == 2)) 2993 { 2994 temp = r[0] / magr; 2995 if (fabs(temp) > 1.0) 2996 temp = sgn_SGP4(temp); 2997 truelon = acos(temp); 2998 if (r[1] < 0.0) 2999 truelon = twopi - truelon; 3000 if (incl > halfpi) 3001 truelon = twopi - truelon; 3002 m = truelon; 3003 } 3004 else 3005 truelon = undefined; 3006 3007 // ------------ find mean anomaly for all orbits ----------- 3008 //if (typeorbit[0] == 'e') 3009 if ((typeorbit == 1) || (typeorbit == 4)) 3010 newtonnu_SGP4(ecc, nu, e, m); 3011 } 3012 else 3013 { 3014 p = undefined; 3015 a = undefined; 3016 ecc = undefined; 3017 incl = undefined; 3018 omega = undefined; 3019 argp = undefined; 3020 nu = undefined; 3021 m = undefined; 3022 arglat = undefined; 3023 truelon = undefined; 3024 lonper = undefined; 3025 } 3026 } // rv2coe 3027 3028 3029 /* ----------------------------------------------------------------------------- 3030 * 3031 * procedure jday_SGP4 3032 * 3033 * this procedure finds the julian date given the year, month, day, and time. 3034 * the julian date is defined by each elapsed day since noon, jan 1, 4713 bc. 3035 * 3036 * algorithm : calculate the answer in one step for efficiency 3037 * 3038 * author : david vallado 719-573-2600 1 mar 2001 3039 * 3040 * inputs description range / units 3041 * year - year 1900 .. 2100 3042 * mon - month 1 .. 12 3043 * day - day 1 .. 28,29,30,31 3044 * hr - universal time hour 0 .. 23 3045 * min - universal time min 0 .. 59 3046 * sec - universal time sec 0.0 .. 59.999 3047 * 3048 * outputs : 3049 * jd - julian date days from 4713 bc 3050 * jdfrac - julian date fraction into day days from 4713 bc 3051 * 3052 * locals : 3053 * none. 3054 * 3055 * coupling : 3056 * none. 3057 * 3058 * references : 3059 * vallado 2013, 183, alg 14, ex 3-4 3060 * --------------------------------------------------------------------------- */ 3061 jday_SGP4(int year,int mon,int day,int hr,int minute,double sec,double & jd,double & jdFrac)3062 void jday_SGP4 3063 ( 3064 int year, int mon, int day, int hr, int minute, double sec, 3065 double& jd, double& jdFrac 3066 ) 3067 { 3068 jd = 367.0 * year - 3069 floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25) + 3070 floor(275 * mon / 9.0) + 3071 day + 1721013.5; // use - 678987.0 to go to mjd directly 3072 jdFrac = (sec + minute * 60.0 + hr * 3600.0) / 86400.0; 3073 3074 // check that the day and fractional day are correct 3075 if (fabs(jdFrac) > 1.0) 3076 { 3077 double dtt = floor(jdFrac); 3078 jd = jd + dtt; 3079 jdFrac = jdFrac - dtt; 3080 } 3081 3082 // - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5; 3083 } // jday 3084 3085 3086 /* ----------------------------------------------------------------------------- 3087 * 3088 * procedure days2mdhms_SGP4 3089 * 3090 * this procedure converts the day of the year, days, to the equivalent month 3091 * day, hour, minute and second. 3092 * 3093 * algorithm : set up array for the number of days per month 3094 * find leap year - use 1900 because 2000 is a leap year 3095 * loop through a temp value while the value is < the days 3096 * perform int conversions to the correct day and month 3097 * convert remainder into h m s using type conversions 3098 * 3099 * author : david vallado 719-573-2600 1 mar 2001 3100 * 3101 * inputs description range / units 3102 * year - year 1900 .. 2100 3103 * days - julian day of the year 1.0 .. 366.0 3104 * 3105 * outputs : 3106 * mon - month 1 .. 12 3107 * day - day 1 .. 28,29,30,31 3108 * hr - hour 0 .. 23 3109 * min - minute 0 .. 59 3110 * sec - second 0.0 .. 59.999 3111 * 3112 * locals : 3113 * dayofyr - day of year 3114 * temp - temporary extended values 3115 * inttemp - temporary int value 3116 * i - index 3117 * lmonth[13] - int array containing the number of days per month 3118 * 3119 * coupling : 3120 * none. 3121 * --------------------------------------------------------------------------- */ 3122 days2mdhms_SGP4(int year,double days,int & mon,int & day,int & hr,int & minute,double & sec)3123 void days2mdhms_SGP4 3124 ( 3125 int year, double days, 3126 int& mon, int& day, int& hr, int& minute, double& sec 3127 ) 3128 { 3129 int i, inttemp, dayofyr; 3130 double temp; 3131 int lmonth[] = { 0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; 3132 3133 dayofyr = (int)floor(days); 3134 /* ----------------- find month and day of month ---------------- */ 3135 if ((year % 4) == 0) 3136 lmonth[2] = 29; 3137 3138 i = 1; 3139 inttemp = 0; 3140 while ((dayofyr > inttemp + lmonth[i]) && (i < 12)) 3141 { 3142 inttemp = inttemp + lmonth[i]; 3143 i++; 3144 } 3145 mon = i; 3146 day = dayofyr - inttemp; 3147 3148 /* ----------------- find hours minutes and seconds ------------- */ 3149 temp = (days - dayofyr) * 24.0; 3150 hr = (int)floor(temp); 3151 temp = (temp - hr) * 60.0; 3152 minute = (int)floor(temp); 3153 sec = (temp - minute) * 60.0; 3154 } // days2mdhms 3155 3156 /* ----------------------------------------------------------------------------- 3157 * 3158 * procedure invjday_SGP4 3159 * 3160 * this procedure finds the year, month, day, hour, minute and second 3161 * given the julian date. tu can be ut1, tdt, tdb, etc. 3162 * 3163 * algorithm : set up starting values 3164 * find leap year - use 1900 because 2000 is a leap year 3165 * find the elapsed days through the year in a loop 3166 * call routine to find each individual value 3167 * 3168 * author : david vallado 719-573-2600 1 mar 2001 3169 * 3170 * inputs description range / units 3171 * jd - julian date days from 4713 bc 3172 * jdfrac - julian date fraction into day days from 4713 bc 3173 * 3174 * outputs : 3175 * year - year 1900 .. 2100 3176 * mon - month 1 .. 12 3177 * day - day 1 .. 28,29,30,31 3178 * hr - hour 0 .. 23 3179 * min - minute 0 .. 59 3180 * sec - second 0.0 .. 59.999 3181 * 3182 * locals : 3183 * days - day of year plus fractional 3184 * portion of a day days 3185 * tu - julian centuries from 0 h 3186 * jan 0, 1900 3187 * temp - temporary double values 3188 * leapyrs - number of leap years from 1900 3189 * 3190 * coupling : 3191 * days2mdhms - finds month, day, hour, minute and second given days and year 3192 * 3193 * references : 3194 * vallado 2013, 203, alg 22, ex 3-13 3195 * --------------------------------------------------------------------------- */ 3196 invjday_SGP4(double jd,double jdfrac,int & year,int & mon,int & day,int & hr,int & minute,double & sec)3197 void invjday_SGP4 3198 ( 3199 double jd, double jdfrac, 3200 int& year, int& mon, int& day, 3201 int& hr, int& minute, double& sec 3202 ) 3203 { 3204 int leapyrs; 3205 double dt, days, tu, temp; 3206 3207 // check jdfrac for multiple days 3208 if (fabs(jdfrac) >= 1.0) 3209 { 3210 jd = jd + floor(jdfrac); 3211 jdfrac = jdfrac - floor(jdfrac); 3212 } 3213 3214 // check for fraction of a day included in the jd 3215 dt = jd - floor(jd) - 0.5; 3216 if (fabs(dt) > 0.00000001) 3217 { 3218 jd = jd - dt; 3219 jdfrac = jdfrac + dt; 3220 } 3221 3222 /* --------------- find year and days of the year --------------- */ 3223 temp = jd - 2415019.5; 3224 tu = temp / 365.25; 3225 year = 1900 + (int)floor(tu); 3226 leapyrs = (int)floor((year - 1901) * 0.25); 3227 3228 days = floor(temp - ((year - 1900) * 365.0 + leapyrs)); 3229 3230 /* ------------ check for case of beginning of a year ----------- */ 3231 if (days + jdfrac < 1.0) 3232 { 3233 year = year - 1; 3234 leapyrs = (int)floor((year - 1901) * 0.25); 3235 days = floor(temp - ((year - 1900) * 365.0 + leapyrs)); 3236 } 3237 3238 /* ----------------- find remaining data ------------------------- */ 3239 days2mdhms_SGP4(year, days + jdfrac, mon, day, hr, minute, sec); 3240 } // invjday 3241 3242 3243 } // namespace SGP4Funcs 3244 3245 3246