2091 lines
78 KiB
C++
2091 lines
78 KiB
C++
/* ----------------------------------------------------------------
|
|
*
|
|
* sgp4unit.cpp
|
|
*
|
|
* this file contains the sgp4 procedures for analytical propagation
|
|
* of a satellite. the code was originally released in the 1980 and 1986
|
|
* spacetrack papers. a detailed discussion of the theory and history
|
|
* may be found in the 2006 aiaa paper by vallado, crawford, hujsak,
|
|
* and kelso.
|
|
*
|
|
* companion code for
|
|
* fundamentals of astrodynamics and applications
|
|
* 2007
|
|
* by david vallado
|
|
*
|
|
* (w) 719-573-2600, email dvallado@agi.com
|
|
*
|
|
* current :
|
|
* 16 nov 07 david vallado
|
|
* misc fixes for better compliance
|
|
* changes :
|
|
* 20 apr 07 david vallado
|
|
* misc fixes for constants
|
|
* 11 aug 06 david vallado
|
|
* chg lyddane choice back to strn3, constants, misc doc
|
|
* 15 dec 05 david vallado
|
|
* misc fixes
|
|
* 26 jul 05 david vallado
|
|
* fixes for paper
|
|
* note that each fix is preceded by a
|
|
* comment with "sgp4fix" and an explanation of
|
|
* what was changed
|
|
* 10 aug 04 david vallado
|
|
* 2nd printing baseline working
|
|
* 14 may 01 david vallado
|
|
* 2nd edition baseline
|
|
* 80 norad
|
|
* original baseline
|
|
* ---------------------------------------------------------------- */
|
|
|
|
#include "sgp4unit.h"
|
|
|
|
const char help = 'n';
|
|
FILE *dbgfile;
|
|
|
|
#define pi 3.14159265358979323846
|
|
|
|
|
|
/* ----------- local functions - only ever used internally by sgp4 ---------- */
|
|
static void 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
|
|
);
|
|
|
|
static void 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
|
|
);
|
|
|
|
static void dsinit
|
|
(
|
|
gravconsttype whichconst,
|
|
double cosim, double emsq, double argpo, double s1, double s2,
|
|
double s3, double s4, double s5, double sinim, double ss1,
|
|
double ss2, double ss3, double ss4, double ss5, double sz1,
|
|
double sz3, double sz11, double sz13, double sz21, double sz23,
|
|
double sz31, double sz33, double t, double tc, double gsto,
|
|
double mo, double mdot, double no, double nodeo, double nodedot,
|
|
double xpidot, double z1, double z3, double z11, double z13,
|
|
double z21, double z23, double z31, double z33, double ecco,
|
|
double eccsq, double& em, double& argpm, double& inclm, double& mm,
|
|
double& nm, double& nodem,
|
|
int& irez,
|
|
double& atime, double& d2201, double& d2211, double& d3210, double& d3222,
|
|
double& d4410, double& d4422, double& d5220, double& d5232, double& d5421,
|
|
double& d5433, double& dedt, double& didt, double& dmdt, double& dndt,
|
|
double& dnodt, double& domdt, double& del1, double& del2, double& del3,
|
|
double& xfact, double& xlamo, double& xli, double& xni
|
|
);
|
|
|
|
static void 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
|
|
);
|
|
|
|
static void initl
|
|
(
|
|
int satn, gravconsttype whichconst,
|
|
double ecco, double epoch, double inclo, double& no,
|
|
char& method,
|
|
double& ainv, double& ao, double& con41, double& con42, double& cosio,
|
|
double& cosio2,double& eccsq, double& omeosq, double& posq,
|
|
double& rp, double& rteosq,double& sinio , double& gsto
|
|
);
|
|
|
|
/* -----------------------------------------------------------------------------
|
|
*
|
|
* procedure dpper
|
|
*
|
|
* this procedure provides deep space long period periodic contributions
|
|
* to the mean elements. by design, these periodics are zero at epoch.
|
|
* this used to be dscom which included initialization, but it's really a
|
|
* recurring function.
|
|
*
|
|
* author : david vallado 719-573-2600 28 jun 2005
|
|
*
|
|
* inputs :
|
|
* e3 -
|
|
* ee2 -
|
|
* peo -
|
|
* pgho -
|
|
* pho -
|
|
* pinco -
|
|
* plo -
|
|
* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 -
|
|
* t -
|
|
* xh2, xh3, xi2, xi3, xl2, xl3, xl4 -
|
|
* zmol -
|
|
* zmos -
|
|
* ep - eccentricity 0.0 - 1.0
|
|
* inclo - inclination - needed for lyddane modification
|
|
* nodep - right ascension of ascending node
|
|
* argpp - argument of perigee
|
|
* mp - mean anomaly
|
|
*
|
|
* outputs :
|
|
* ep - eccentricity 0.0 - 1.0
|
|
* inclp - inclination
|
|
* nodep - right ascension of ascending node
|
|
* argpp - argument of perigee
|
|
* mp - mean anomaly
|
|
*
|
|
* locals :
|
|
* alfdp -
|
|
* betdp -
|
|
* cosip , sinip , cosop , sinop ,
|
|
* dalf -
|
|
* dbet -
|
|
* dls -
|
|
* f2, f3 -
|
|
* pe -
|
|
* pgh -
|
|
* ph -
|
|
* pinc -
|
|
* pl -
|
|
* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis ,
|
|
* sll , sls
|
|
* xls -
|
|
* xnoh -
|
|
* zf -
|
|
* zm -
|
|
*
|
|
* coupling :
|
|
* none.
|
|
*
|
|
* references :
|
|
* hoots, roehrich, norad spacetrack report #3 1980
|
|
* hoots, norad spacetrack report #6 1986
|
|
* hoots, schumacher and glover 2004
|
|
* vallado, crawford, hujsak, kelso 2006
|
|
----------------------------------------------------------------------------*/
|
|
|
|
static void 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
|
|
)
|
|
{
|
|
/* --------------------- local variables ------------------------ */
|
|
const double twopi = 2.0 * pi;
|
|
double alfdp, betdp, cosip, cosop, dalf, dbet, dls,
|
|
f2, f3, pe, pgh, ph, pinc, pl ,
|
|
sel, ses, sghl, sghs, shll, shs, sil,
|
|
sinip, sinop, sinzf, sis, sll, sls, xls,
|
|
xnoh, zf, zm, zel, zes, znl, zns;
|
|
|
|
/* ---------------------- constants ----------------------------- */
|
|
zns = 1.19459e-5;
|
|
zes = 0.01675;
|
|
znl = 1.5835218e-4;
|
|
zel = 0.05490;
|
|
|
|
/* --------------- calculate time varying periodics ----------- */
|
|
zm = zmos + zns * t;
|
|
// be sure that the initial call has time set to zero
|
|
if (init == 'y')
|
|
zm = zmos;
|
|
zf = zm + 2.0 * zes * sin(zm);
|
|
sinzf = sin(zf);
|
|
f2 = 0.5 * sinzf * sinzf - 0.25;
|
|
f3 = -0.5 * sinzf * cos(zf);
|
|
ses = se2* f2 + se3 * f3;
|
|
sis = si2 * f2 + si3 * f3;
|
|
sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf;
|
|
sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf;
|
|
shs = sh2 * f2 + sh3 * f3;
|
|
zm = zmol + znl * t;
|
|
if (init == 'y')
|
|
zm = zmol;
|
|
zf = zm + 2.0 * zel * sin(zm);
|
|
sinzf = sin(zf);
|
|
f2 = 0.5 * sinzf * sinzf - 0.25;
|
|
f3 = -0.5 * sinzf * cos(zf);
|
|
sel = ee2 * f2 + e3 * f3;
|
|
sil = xi2 * f2 + xi3 * f3;
|
|
sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf;
|
|
sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf;
|
|
shll = xh2 * f2 + xh3 * f3;
|
|
pe = ses + sel;
|
|
pinc = sis + sil;
|
|
pl = sls + sll;
|
|
pgh = sghs + sghl;
|
|
ph = shs + shll;
|
|
|
|
if (init == 'n')
|
|
{
|
|
pe = pe - peo;
|
|
pinc = pinc - pinco;
|
|
pl = pl - plo;
|
|
pgh = pgh - pgho;
|
|
ph = ph - pho;
|
|
inclp = inclp + pinc;
|
|
ep = ep + pe;
|
|
sinip = sin(inclp);
|
|
cosip = cos(inclp);
|
|
|
|
/* ----------------- apply periodics directly ------------ */
|
|
// sgp4fix for lyddane choice
|
|
// strn3 used original inclination - this is technically feasible
|
|
// gsfc used perturbed inclination - also technically feasible
|
|
// probably best to readjust the 0.2 limit value and limit discontinuity
|
|
// 0.2 rad = 11.45916 deg
|
|
// use next line for original strn3 approach and original inclination
|
|
// if (inclo >= 0.2)
|
|
// use next line for gsfc version and perturbed inclination
|
|
if (inclp >= 0.2)
|
|
{
|
|
ph = ph / sinip;
|
|
pgh = pgh - cosip * ph;
|
|
argpp = argpp + pgh;
|
|
nodep = nodep + ph;
|
|
mp = mp + pl;
|
|
}
|
|
else
|
|
{
|
|
/* ---- apply periodics with lyddane modification ---- */
|
|
sinop = sin(nodep);
|
|
cosop = cos(nodep);
|
|
alfdp = sinip * sinop;
|
|
betdp = sinip * cosop;
|
|
dalf = ph * cosop + pinc * cosip * sinop;
|
|
dbet = -ph * sinop + pinc * cosip * cosop;
|
|
alfdp = alfdp + dalf;
|
|
betdp = betdp + dbet;
|
|
nodep = fmod(nodep, twopi);
|
|
// sgp4fix for afspc written intrinsic functions
|
|
// nodep used without a trigonometric function ahead
|
|
if (nodep < 0.0)
|
|
nodep = nodep + twopi;
|
|
xls = mp + argpp + cosip * nodep;
|
|
dls = pl + pgh - pinc * nodep * sinip;
|
|
xls = xls + dls;
|
|
xnoh = nodep;
|
|
nodep = atan2(alfdp, betdp);
|
|
// sgp4fix for afspc written intrinsic functions
|
|
// nodep used without a trigonometric function ahead
|
|
if (nodep < 0.0)
|
|
nodep = nodep + twopi;
|
|
if (fabs(xnoh - nodep) > pi){
|
|
if (nodep < xnoh)
|
|
nodep = nodep + twopi;
|
|
else
|
|
nodep = nodep - twopi;
|
|
}
|
|
mp = mp + pl;
|
|
argpp = xls - mp - cosip * nodep;
|
|
}
|
|
} // if init == 'n'
|
|
|
|
//#include "debug1.cpp"
|
|
} // end dpper
|
|
|
|
/*-----------------------------------------------------------------------------
|
|
*
|
|
* procedure dscom
|
|
*
|
|
* this procedure provides deep space common items used by both the secular
|
|
* and periodics subroutines. input is provided as shown. this routine
|
|
* used to be called dpper, but the functions inside weren't well organized.
|
|
*
|
|
* author : david vallado 719-573-2600 28 jun 2005
|
|
*
|
|
* inputs :
|
|
* epoch -
|
|
* ep - eccentricity
|
|
* argpp - argument of perigee
|
|
* tc -
|
|
* inclp - inclination
|
|
* nodep - right ascension of ascending node
|
|
* np - mean motion
|
|
*
|
|
* outputs :
|
|
* sinim , cosim , sinomm , cosomm , snodm , cnodm
|
|
* day -
|
|
* e3 -
|
|
* ee2 -
|
|
* em - eccentricity
|
|
* emsq - eccentricity squared
|
|
* gam -
|
|
* peo -
|
|
* pgho -
|
|
* pho -
|
|
* pinco -
|
|
* plo -
|
|
* rtemsq -
|
|
* se2, se3 -
|
|
* sgh2, sgh3, sgh4 -
|
|
* sh2, sh3, si2, si3, sl2, sl3, sl4 -
|
|
* s1, s2, s3, s4, s5, s6, s7 -
|
|
* ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 -
|
|
* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 -
|
|
* xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 -
|
|
* nm - mean motion
|
|
* z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 -
|
|
* zmol -
|
|
* zmos -
|
|
*
|
|
* locals :
|
|
* a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 -
|
|
* betasq -
|
|
* cc -
|
|
* ctem, stem -
|
|
* x1, x2, x3, x4, x5, x6, x7, x8 -
|
|
* xnodce -
|
|
* xnoi -
|
|
* zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl ,
|
|
* zcosi , zsini , zcosil , zsinil ,
|
|
* zx -
|
|
* zy -
|
|
*
|
|
* coupling :
|
|
* none.
|
|
*
|
|
* references :
|
|
* hoots, roehrich, norad spacetrack report #3 1980
|
|
* hoots, norad spacetrack report #6 1986
|
|
* hoots, schumacher and glover 2004
|
|
* vallado, crawford, hujsak, kelso 2006
|
|
----------------------------------------------------------------------------*/
|
|
|
|
static void 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
|
|
)
|
|
{
|
|
/* -------------------------- constants ------------------------- */
|
|
const double zes = 0.01675;
|
|
const double zel = 0.05490;
|
|
const double c1ss = 2.9864797e-6;
|
|
const double c1l = 4.7968065e-7;
|
|
const double zsinis = 0.39785416;
|
|
const double zcosis = 0.91744867;
|
|
const double zcosgs = 0.1945905;
|
|
const double zsings = -0.98088458;
|
|
const double twopi = 2.0 * pi;
|
|
|
|
/* --------------------- local variables ------------------------ */
|
|
int lsflg;
|
|
double a1 , a2 , a3 , a4 , a5 , a6 , a7 ,
|
|
a8 , a9 , a10 , betasq, cc , ctem , stem ,
|
|
x1 , x2 , x3 , x4 , x5 , x6 , x7 ,
|
|
x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl,
|
|
zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini ,
|
|
zsinil, zx , zy;
|
|
|
|
nm = np;
|
|
em = ep;
|
|
snodm = sin(nodep);
|
|
cnodm = cos(nodep);
|
|
sinomm = sin(argpp);
|
|
cosomm = cos(argpp);
|
|
sinim = sin(inclp);
|
|
cosim = cos(inclp);
|
|
emsq = em * em;
|
|
betasq = 1.0 - emsq;
|
|
rtemsq = sqrt(betasq);
|
|
|
|
/* ----------------- initialize lunar solar terms --------------- */
|
|
peo = 0.0;
|
|
pinco = 0.0;
|
|
plo = 0.0;
|
|
pgho = 0.0;
|
|
pho = 0.0;
|
|
day = epoch + 18261.5 + tc / 1440.0;
|
|
xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi);
|
|
stem = sin(xnodce);
|
|
ctem = cos(xnodce);
|
|
zcosil = 0.91375164 - 0.03568096 * ctem;
|
|
zsinil = sqrt(1.0 - zcosil * zcosil);
|
|
zsinhl = 0.089683511 * stem / zsinil;
|
|
zcoshl = sqrt(1.0 - zsinhl * zsinhl);
|
|
gam = 5.8351514 + 0.0019443680 * day;
|
|
zx = 0.39785416 * stem / zsinil;
|
|
zy = zcoshl * ctem + 0.91744867 * zsinhl * stem;
|
|
zx = atan2(zx, zy);
|
|
zx = gam + zx - xnodce;
|
|
zcosgl = cos(zx);
|
|
zsingl = sin(zx);
|
|
|
|
/* ------------------------- do solar terms --------------------- */
|
|
zcosg = zcosgs;
|
|
zsing = zsings;
|
|
zcosi = zcosis;
|
|
zsini = zsinis;
|
|
zcosh = cnodm;
|
|
zsinh = snodm;
|
|
cc = c1ss;
|
|
xnoi = 1.0 / nm;
|
|
|
|
for (lsflg = 1; lsflg <= 2; lsflg++)
|
|
{
|
|
a1 = zcosg * zcosh + zsing * zcosi * zsinh;
|
|
a3 = -zsing * zcosh + zcosg * zcosi * zsinh;
|
|
a7 = -zcosg * zsinh + zsing * zcosi * zcosh;
|
|
a8 = zsing * zsini;
|
|
a9 = zsing * zsinh + zcosg * zcosi * zcosh;
|
|
a10 = zcosg * zsini;
|
|
a2 = cosim * a7 + sinim * a8;
|
|
a4 = cosim * a9 + sinim * a10;
|
|
a5 = -sinim * a7 + cosim * a8;
|
|
a6 = -sinim * a9 + cosim * a10;
|
|
|
|
x1 = a1 * cosomm + a2 * sinomm;
|
|
x2 = a3 * cosomm + a4 * sinomm;
|
|
x3 = -a1 * sinomm + a2 * cosomm;
|
|
x4 = -a3 * sinomm + a4 * cosomm;
|
|
x5 = a5 * sinomm;
|
|
x6 = a6 * sinomm;
|
|
x7 = a5 * cosomm;
|
|
x8 = a6 * cosomm;
|
|
|
|
z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3;
|
|
z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4;
|
|
z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4;
|
|
z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq;
|
|
z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq;
|
|
z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq;
|
|
z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5);
|
|
z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq *
|
|
(-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5));
|
|
z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6);
|
|
z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7);
|
|
z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq *
|
|
(24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8));
|
|
z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8);
|
|
z1 = z1 + z1 + betasq * z31;
|
|
z2 = z2 + z2 + betasq * z32;
|
|
z3 = z3 + z3 + betasq * z33;
|
|
s3 = cc * xnoi;
|
|
s2 = -0.5 * s3 / rtemsq;
|
|
s4 = s3 * rtemsq;
|
|
s1 = -15.0 * em * s4;
|
|
s5 = x1 * x3 + x2 * x4;
|
|
s6 = x2 * x3 + x1 * x4;
|
|
s7 = x2 * x4 - x1 * x3;
|
|
|
|
/* ----------------------- do lunar terms ------------------- */
|
|
if (lsflg == 1)
|
|
{
|
|
ss1 = s1;
|
|
ss2 = s2;
|
|
ss3 = s3;
|
|
ss4 = s4;
|
|
ss5 = s5;
|
|
ss6 = s6;
|
|
ss7 = s7;
|
|
sz1 = z1;
|
|
sz2 = z2;
|
|
sz3 = z3;
|
|
sz11 = z11;
|
|
sz12 = z12;
|
|
sz13 = z13;
|
|
sz21 = z21;
|
|
sz22 = z22;
|
|
sz23 = z23;
|
|
sz31 = z31;
|
|
sz32 = z32;
|
|
sz33 = z33;
|
|
zcosg = zcosgl;
|
|
zsing = zsingl;
|
|
zcosi = zcosil;
|
|
zsini = zsinil;
|
|
zcosh = zcoshl * cnodm + zsinhl * snodm;
|
|
zsinh = snodm * zcoshl - cnodm * zsinhl;
|
|
cc = c1l;
|
|
}
|
|
}
|
|
|
|
zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi);
|
|
zmos = fmod(6.2565837 + 0.017201977 * day, twopi);
|
|
|
|
/* ------------------------ do solar terms ---------------------- */
|
|
se2 = 2.0 * ss1 * ss6;
|
|
se3 = 2.0 * ss1 * ss7;
|
|
si2 = 2.0 * ss2 * sz12;
|
|
si3 = 2.0 * ss2 * (sz13 - sz11);
|
|
sl2 = -2.0 * ss3 * sz2;
|
|
sl3 = -2.0 * ss3 * (sz3 - sz1);
|
|
sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes;
|
|
sgh2 = 2.0 * ss4 * sz32;
|
|
sgh3 = 2.0 * ss4 * (sz33 - sz31);
|
|
sgh4 = -18.0 * ss4 * zes;
|
|
sh2 = -2.0 * ss2 * sz22;
|
|
sh3 = -2.0 * ss2 * (sz23 - sz21);
|
|
|
|
/* ------------------------ do lunar terms ---------------------- */
|
|
ee2 = 2.0 * s1 * s6;
|
|
e3 = 2.0 * s1 * s7;
|
|
xi2 = 2.0 * s2 * z12;
|
|
xi3 = 2.0 * s2 * (z13 - z11);
|
|
xl2 = -2.0 * s3 * z2;
|
|
xl3 = -2.0 * s3 * (z3 - z1);
|
|
xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel;
|
|
xgh2 = 2.0 * s4 * z32;
|
|
xgh3 = 2.0 * s4 * (z33 - z31);
|
|
xgh4 = -18.0 * s4 * zel;
|
|
xh2 = -2.0 * s2 * z22;
|
|
xh3 = -2.0 * s2 * (z23 - z21);
|
|
|
|
//#include "debug2.cpp"
|
|
} // end dscom
|
|
|
|
/*-----------------------------------------------------------------------------
|
|
*
|
|
* procedure dsinit
|
|
*
|
|
* this procedure provides deep space contributions to mean motion dot due
|
|
* to geopotential resonance with half day and one day orbits.
|
|
*
|
|
* author : david vallado 719-573-2600 28 jun 2005
|
|
*
|
|
* inputs :
|
|
* cosim, sinim-
|
|
* emsq - eccentricity squared
|
|
* argpo - argument of perigee
|
|
* s1, s2, s3, s4, s5 -
|
|
* ss1, ss2, ss3, ss4, ss5 -
|
|
* sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 -
|
|
* t - time
|
|
* tc -
|
|
* gsto - greenwich sidereal time rad
|
|
* mo - mean anomaly
|
|
* mdot - mean anomaly dot (rate)
|
|
* no - mean motion
|
|
* nodeo - right ascension of ascending node
|
|
* nodedot - right ascension of ascending node dot (rate)
|
|
* xpidot -
|
|
* z1, z3, z11, z13, z21, z23, z31, z33 -
|
|
* eccm - eccentricity
|
|
* argpm - argument of perigee
|
|
* inclm - inclination
|
|
* mm - mean anomaly
|
|
* xn - mean motion
|
|
* nodem - right ascension of ascending node
|
|
*
|
|
* outputs :
|
|
* em - eccentricity
|
|
* argpm - argument of perigee
|
|
* inclm - inclination
|
|
* mm - mean anomaly
|
|
* nm - mean motion
|
|
* nodem - right ascension of ascending node
|
|
* irez - flag for resonance 0-none, 1-one day, 2-half day
|
|
* atime -
|
|
* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 -
|
|
* dedt -
|
|
* didt -
|
|
* dmdt -
|
|
* dndt -
|
|
* dnodt -
|
|
* domdt -
|
|
* del1, del2, del3 -
|
|
* ses , sghl , sghs , sgs , shl , shs , sis , sls
|
|
* theta -
|
|
* xfact -
|
|
* xlamo -
|
|
* xli -
|
|
* xni
|
|
*
|
|
* locals :
|
|
* ainv2 -
|
|
* aonv -
|
|
* cosisq -
|
|
* eoc -
|
|
* f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 -
|
|
* g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 -
|
|
* sini2 -
|
|
* temp -
|
|
* temp1 -
|
|
* theta -
|
|
* xno2 -
|
|
*
|
|
* coupling :
|
|
* getgravconst
|
|
*
|
|
* references :
|
|
* hoots, roehrich, norad spacetrack report #3 1980
|
|
* hoots, norad spacetrack report #6 1986
|
|
* hoots, schumacher and glover 2004
|
|
* vallado, crawford, hujsak, kelso 2006
|
|
----------------------------------------------------------------------------*/
|
|
|
|
static void dsinit
|
|
(
|
|
gravconsttype whichconst,
|
|
double cosim, double emsq, double argpo, double s1, double s2,
|
|
double s3, double s4, double s5, double sinim, double ss1,
|
|
double ss2, double ss3, double ss4, double ss5, double sz1,
|
|
double sz3, double sz11, double sz13, double sz21, double sz23,
|
|
double sz31, double sz33, double t, double tc, double gsto,
|
|
double mo, double mdot, double no, double nodeo, double nodedot,
|
|
double xpidot, double z1, double z3, double z11, double z13,
|
|
double z21, double z23, double z31, double z33, double ecco,
|
|
double eccsq, double& em, double& argpm, double& inclm, double& mm,
|
|
double& nm, double& nodem,
|
|
int& irez,
|
|
double& atime, double& d2201, double& d2211, double& d3210, double& d3222,
|
|
double& d4410, double& d4422, double& d5220, double& d5232, double& d5421,
|
|
double& d5433, double& dedt, double& didt, double& dmdt, double& dndt,
|
|
double& dnodt, double& domdt, double& del1, double& del2, double& del3,
|
|
double& xfact, double& xlamo, double& xli, double& xni
|
|
)
|
|
{
|
|
/* --------------------- local variables ------------------------ */
|
|
const double twopi = 2.0 * pi;
|
|
|
|
double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 ,
|
|
f321 , f322 , f330 , f441 , f442 , f522 , f523 ,
|
|
f542 , f543 , g200 , g201 , g211 , g300 , g310 ,
|
|
g322 , g410 , g422 , g520 , g521 , g532 , g533 ,
|
|
ses , sgs , sghl , sghs , shs , shll , sis ,
|
|
sini2 , sls , temp , temp1 , theta , xno2 , q22 ,
|
|
q31 , q33 , root22, root44, root54, rptim , root32,
|
|
root52, x2o3 , xke , znl , emo , zns , emsqo,
|
|
tumin, mu, radiusearthkm, j2, j3, j4, j3oj2;
|
|
|
|
q22 = 1.7891679e-6;
|
|
q31 = 2.1460748e-6;
|
|
q33 = 2.2123015e-7;
|
|
root22 = 1.7891679e-6;
|
|
root44 = 7.3636953e-9;
|
|
root54 = 2.1765803e-9;
|
|
rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
|
|
root32 = 3.7393792e-7;
|
|
root52 = 1.1428639e-7;
|
|
x2o3 = 2.0 / 3.0;
|
|
znl = 1.5835218e-4;
|
|
zns = 1.19459e-5;
|
|
|
|
// sgp4fix identify constants and allow alternate values
|
|
getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
|
|
|
|
/* -------------------- deep space initialization ------------ */
|
|
irez = 0;
|
|
if ((nm < 0.0052359877) && (nm > 0.0034906585))
|
|
irez = 1;
|
|
if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5))
|
|
irez = 2;
|
|
|
|
/* ------------------------ do solar terms ------------------- */
|
|
ses = ss1 * zns * ss5;
|
|
sis = ss2 * zns * (sz11 + sz13);
|
|
sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq);
|
|
sghs = ss4 * zns * (sz31 + sz33 - 6.0);
|
|
shs = -zns * ss2 * (sz21 + sz23);
|
|
// sgp4fix for 180 deg incl
|
|
if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2))
|
|
shs = 0.0;
|
|
if (sinim != 0.0)
|
|
shs = shs / sinim;
|
|
sgs = sghs - cosim * shs;
|
|
|
|
/* ------------------------- do lunar terms ------------------ */
|
|
dedt = ses + s1 * znl * s5;
|
|
didt = sis + s2 * znl * (z11 + z13);
|
|
dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq);
|
|
sghl = s4 * znl * (z31 + z33 - 6.0);
|
|
shll = -znl * s2 * (z21 + z23);
|
|
// sgp4fix for 180 deg incl
|
|
if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2))
|
|
shll = 0.0;
|
|
domdt = sgs + sghl;
|
|
dnodt = shs;
|
|
if (sinim != 0.0)
|
|
{
|
|
domdt = domdt - cosim / sinim * shll;
|
|
dnodt = dnodt + shll / sinim;
|
|
}
|
|
|
|
/* ----------- calculate deep space resonance effects -------- */
|
|
dndt = 0.0;
|
|
theta = fmod(gsto + tc * rptim, twopi);
|
|
em = em + dedt * t;
|
|
inclm = inclm + didt * t;
|
|
argpm = argpm + domdt * t;
|
|
nodem = nodem + dnodt * t;
|
|
mm = mm + dmdt * t;
|
|
// sgp4fix for negative inclinations
|
|
// the following if statement should be commented out
|
|
//if (inclm < 0.0)
|
|
// {
|
|
// inclm = -inclm;
|
|
// argpm = argpm - pi;
|
|
// nodem = nodem + pi;
|
|
// }
|
|
|
|
/* -------------- initialize the resonance terms ------------- */
|
|
if (irez != 0)
|
|
{
|
|
aonv = pow(nm / xke, x2o3);
|
|
|
|
/* ---------- geopotential resonance for 12 hour orbits ------ */
|
|
if (irez == 2)
|
|
{
|
|
cosisq = cosim * cosim;
|
|
emo = em;
|
|
em = ecco;
|
|
emsqo = emsq;
|
|
emsq = eccsq;
|
|
eoc = em * emsq;
|
|
g201 = -0.306 - (em - 0.64) * 0.440;
|
|
|
|
if (em <= 0.65)
|
|
{
|
|
g211 = 3.616 - 13.2470 * em + 16.2900 * emsq;
|
|
g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc;
|
|
g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc;
|
|
g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc;
|
|
g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc;
|
|
g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc;
|
|
}
|
|
else
|
|
{
|
|
g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc;
|
|
g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc;
|
|
g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc;
|
|
g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc;
|
|
g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc;
|
|
if (em > 0.715)
|
|
g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc;
|
|
else
|
|
g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq;
|
|
}
|
|
if (em < 0.7)
|
|
{
|
|
g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc;
|
|
g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc;
|
|
g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc;
|
|
}
|
|
else
|
|
{
|
|
g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc;
|
|
g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc;
|
|
g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc;
|
|
}
|
|
|
|
sini2= sinim * sinim;
|
|
f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq);
|
|
f221 = 1.5 * sini2;
|
|
f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq);
|
|
f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq);
|
|
f441 = 35.0 * sini2 * f220;
|
|
f442 = 39.3750 * sini2 * sini2;
|
|
f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) +
|
|
0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) );
|
|
f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim +
|
|
10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq));
|
|
f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq *
|
|
(-12.0 + 8.0 * cosim + 10.0 * cosisq));
|
|
f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq *
|
|
(12.0 + 8.0 * cosim - 10.0 * cosisq));
|
|
xno2 = nm * nm;
|
|
ainv2 = aonv * aonv;
|
|
temp1 = 3.0 * xno2 * ainv2;
|
|
temp = temp1 * root22;
|
|
d2201 = temp * f220 * g201;
|
|
d2211 = temp * f221 * g211;
|
|
temp1 = temp1 * aonv;
|
|
temp = temp1 * root32;
|
|
d3210 = temp * f321 * g310;
|
|
d3222 = temp * f322 * g322;
|
|
temp1 = temp1 * aonv;
|
|
temp = 2.0 * temp1 * root44;
|
|
d4410 = temp * f441 * g410;
|
|
d4422 = temp * f442 * g422;
|
|
temp1 = temp1 * aonv;
|
|
temp = temp1 * root52;
|
|
d5220 = temp * f522 * g520;
|
|
d5232 = temp * f523 * g532;
|
|
temp = 2.0 * temp1 * root54;
|
|
d5421 = temp * f542 * g521;
|
|
d5433 = temp * f543 * g533;
|
|
xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi);
|
|
xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no;
|
|
em = emo;
|
|
emsq = emsqo;
|
|
}
|
|
|
|
/* ---------------- synchronous resonance terms -------------- */
|
|
if (irez == 1)
|
|
{
|
|
g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq);
|
|
g310 = 1.0 + 2.0 * emsq;
|
|
g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq);
|
|
f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim);
|
|
f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim);
|
|
f330 = 1.0 + cosim;
|
|
f330 = 1.875 * f330 * f330 * f330;
|
|
del1 = 3.0 * nm * nm * aonv * aonv;
|
|
del2 = 2.0 * del1 * f220 * g200 * q22;
|
|
del3 = 3.0 * del1 * f330 * g300 * q33 * aonv;
|
|
del1 = del1 * f311 * g310 * q31 * aonv;
|
|
xlamo = fmod(mo + nodeo + argpo - theta, twopi);
|
|
xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no;
|
|
}
|
|
|
|
/* ------------ for sgp4, initialize the integrator ---------- */
|
|
xli = xlamo;
|
|
xni = no;
|
|
atime = 0.0;
|
|
nm = no + dndt;
|
|
}
|
|
|
|
//#include "debug3.cpp"
|
|
} // end dsinit
|
|
|
|
/*-----------------------------------------------------------------------------
|
|
*
|
|
* procedure dspace
|
|
*
|
|
* this procedure provides deep space contributions to mean elements for
|
|
* perturbing third body. these effects have been averaged over one
|
|
* revolution of the sun and moon. for earth resonance effects, the
|
|
* effects have been averaged over no revolutions of the satellite.
|
|
* (mean motion)
|
|
*
|
|
* author : david vallado 719-573-2600 28 jun 2005
|
|
*
|
|
* inputs :
|
|
* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 -
|
|
* dedt -
|
|
* del1, del2, del3 -
|
|
* didt -
|
|
* dmdt -
|
|
* dnodt -
|
|
* domdt -
|
|
* irez - flag for resonance 0-none, 1-one day, 2-half day
|
|
* argpo - argument of perigee
|
|
* argpdot - argument of perigee dot (rate)
|
|
* t - time
|
|
* tc -
|
|
* gsto - gst
|
|
* xfact -
|
|
* xlamo -
|
|
* no - mean motion
|
|
* atime -
|
|
* em - eccentricity
|
|
* ft -
|
|
* argpm - argument of perigee
|
|
* inclm - inclination
|
|
* xli -
|
|
* mm - mean anomaly
|
|
* xni - mean motion
|
|
* nodem - right ascension of ascending node
|
|
*
|
|
* outputs :
|
|
* atime -
|
|
* em - eccentricity
|
|
* argpm - argument of perigee
|
|
* inclm - inclination
|
|
* xli -
|
|
* mm - mean anomaly
|
|
* xni -
|
|
* nodem - right ascension of ascending node
|
|
* dndt -
|
|
* nm - mean motion
|
|
*
|
|
* locals :
|
|
* delt -
|
|
* ft -
|
|
* theta -
|
|
* x2li -
|
|
* x2omi -
|
|
* xl -
|
|
* xldot -
|
|
* xnddt -
|
|
* xndt -
|
|
* xomi -
|
|
*
|
|
* coupling :
|
|
* none -
|
|
*
|
|
* references :
|
|
* hoots, roehrich, norad spacetrack report #3 1980
|
|
* hoots, norad spacetrack report #6 1986
|
|
* hoots, schumacher and glover 2004
|
|
* vallado, crawford, hujsak, kelso 2006
|
|
----------------------------------------------------------------------------*/
|
|
|
|
static void 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
|
|
)
|
|
{
|
|
const double twopi = 2.0 * pi;
|
|
int iretn , iret;
|
|
double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32,
|
|
g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp;
|
|
|
|
ft = 0.0;
|
|
fasx2 = 0.13130908;
|
|
fasx4 = 2.8843198;
|
|
fasx6 = 0.37448087;
|
|
g22 = 5.7686396;
|
|
g32 = 0.95240898;
|
|
g44 = 1.8014998;
|
|
g52 = 1.0508330;
|
|
g54 = 4.4108898;
|
|
rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec
|
|
stepp = 720.0;
|
|
stepn = -720.0;
|
|
step2 = 259200.0;
|
|
|
|
/* ----------- calculate deep space resonance effects ----------- */
|
|
dndt = 0.0;
|
|
theta = fmod(gsto + tc * rptim, twopi);
|
|
em = em + dedt * t;
|
|
|
|
inclm = inclm + didt * t;
|
|
argpm = argpm + domdt * t;
|
|
nodem = nodem + dnodt * t;
|
|
mm = mm + dmdt * t;
|
|
|
|
// sgp4fix for negative inclinations
|
|
// the following if statement should be commented out
|
|
// if (inclm < 0.0)
|
|
// {
|
|
// inclm = -inclm;
|
|
// argpm = argpm - pi;
|
|
// nodem = nodem + pi;
|
|
// }
|
|
|
|
/* - update resonances : numerical (euler-maclaurin) integration - */
|
|
/* ------------------------- epoch restart ---------------------- */
|
|
// sgp4fix for propagator problems
|
|
// the following integration works for negative time steps and periods
|
|
// the specific changes are unknown because the original code was so convoluted
|
|
|
|
ft = 0.0;
|
|
atime = 0.0;
|
|
if (irez != 0)
|
|
{
|
|
if ((atime == 0.0) || ((t >= 0.0) && (atime < 0.0)) ||
|
|
((t < 0.0) && (atime >= 0.0)))
|
|
{
|
|
if (t >= 0.0)
|
|
delt = stepp;
|
|
else
|
|
delt = stepn;
|
|
atime = 0.0;
|
|
xni = no;
|
|
xli = xlamo;
|
|
}
|
|
iretn = 381; // added for do loop
|
|
iret = 0; // added for loop
|
|
while (iretn == 381)
|
|
{
|
|
if ((fabs(t) < fabs(atime)) || (iret == 351))
|
|
{
|
|
if (t >= 0.0)
|
|
delt = stepn;
|
|
else
|
|
delt = stepp;
|
|
iret = 351;
|
|
iretn = 381;
|
|
}
|
|
else
|
|
{
|
|
if (t > 0.0) // error if prev if has atime:=0.0 and t:=0.0 (ge)
|
|
delt = stepp;
|
|
else
|
|
delt = stepn;
|
|
if (fabs(t - atime) >= stepp)
|
|
{
|
|
iret = 0;
|
|
iretn = 381;
|
|
}
|
|
else
|
|
{
|
|
ft = t - atime;
|
|
iretn = 0;
|
|
}
|
|
}
|
|
|
|
/* ------------------- dot terms calculated ------------- */
|
|
/* ----------- near - synchronous resonance terms ------- */
|
|
if (irez != 2)
|
|
{
|
|
xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) +
|
|
del3 * sin(3.0 * (xli - fasx6));
|
|
xldot = xni + xfact;
|
|
xnddt = del1 * cos(xli - fasx2) +
|
|
2.0 * del2 * cos(2.0 * (xli - fasx4)) +
|
|
3.0 * del3 * cos(3.0 * (xli - fasx6));
|
|
xnddt = xnddt * xldot;
|
|
}
|
|
else
|
|
{
|
|
/* --------- near - half-day resonance terms -------- */
|
|
xomi = argpo + argpdot * atime;
|
|
x2omi = xomi + xomi;
|
|
x2li = xli + xli;
|
|
xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) +
|
|
d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+
|
|
d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) +
|
|
d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+
|
|
d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54);
|
|
xldot = xni + xfact;
|
|
xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) +
|
|
d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) +
|
|
d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) +
|
|
2.0 * (d4410 * cos(x2omi + x2li - g44) +
|
|
d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) +
|
|
d5433 * cos(-xomi + x2li - g54));
|
|
xnddt = xnddt * xldot;
|
|
}
|
|
|
|
/* ----------------------- integrator ------------------- */
|
|
if (iretn == 381)
|
|
{
|
|
xli = xli + xldot * delt + xndt * step2;
|
|
xni = xni + xndt * delt + xnddt * step2;
|
|
atime = atime + delt;
|
|
}
|
|
} // while iretn = 381
|
|
|
|
nm = xni + xndt * ft + xnddt * ft * ft * 0.5;
|
|
xl = xli + xldot * ft + xndt * ft * ft * 0.5;
|
|
if (irez != 1)
|
|
{
|
|
mm = xl - 2.0 * nodem + 2.0 * theta;
|
|
dndt = nm - no;
|
|
}
|
|
else
|
|
{
|
|
mm = xl - nodem - argpm + theta;
|
|
dndt = nm - no;
|
|
}
|
|
nm = no + dndt;
|
|
}
|
|
|
|
//#include "debug4.cpp"
|
|
} // end dsspace
|
|
|
|
/*-----------------------------------------------------------------------------
|
|
*
|
|
* procedure initl
|
|
*
|
|
* this procedure initializes the spg4 propagator. all the initialization is
|
|
* consolidated here instead of having multiple loops inside other routines.
|
|
*
|
|
* author : david vallado 719-573-2600 28 jun 2005
|
|
*
|
|
* inputs :
|
|
* ecco - eccentricity 0.0 - 1.0
|
|
* epoch - epoch time in days from jan 0, 1950. 0 hr
|
|
* inclo - inclination of satellite
|
|
* no - mean motion of satellite
|
|
* satn - satellite number
|
|
*
|
|
* outputs :
|
|
* ainv - 1.0 / a
|
|
* ao - semi major axis
|
|
* con41 -
|
|
* con42 - 1.0 - 5.0 cos(i)
|
|
* cosio - cosine of inclination
|
|
* cosio2 - cosio squared
|
|
* eccsq - eccentricity squared
|
|
* method - flag for deep space 'd', 'n'
|
|
* omeosq - 1.0 - ecco * ecco
|
|
* posq - semi-parameter squared
|
|
* rp - radius of perigee
|
|
* rteosq - square root of (1.0 - ecco*ecco)
|
|
* sinio - sine of inclination
|
|
* gsto - gst at time of observation rad
|
|
* no - mean motion of satellite
|
|
*
|
|
* locals :
|
|
* ak -
|
|
* d1 -
|
|
* del -
|
|
* adel -
|
|
* po -
|
|
*
|
|
* coupling :
|
|
* getgravconst
|
|
* gstime - find greenwich sidereal time from the julian date
|
|
*
|
|
* references :
|
|
* hoots, roehrich, norad spacetrack report #3 1980
|
|
* hoots, norad spacetrack report #6 1986
|
|
* hoots, schumacher and glover 2004
|
|
* vallado, crawford, hujsak, kelso 2006
|
|
----------------------------------------------------------------------------*/
|
|
|
|
static void initl
|
|
(
|
|
int satn, gravconsttype whichconst,
|
|
double ecco, double epoch, double inclo, double& no,
|
|
char& method,
|
|
double& ainv, double& ao, double& con41, double& con42, double& cosio,
|
|
double& cosio2,double& eccsq, double& omeosq, double& posq,
|
|
double& rp, double& rteosq,double& sinio , double& gsto
|
|
)
|
|
{
|
|
/* --------------------- local variables ------------------------ */
|
|
double ak, d1, del, adel, po, x2o3, j2, xke,
|
|
tumin, mu, radiusearthkm, j3, j4, j3oj2;
|
|
|
|
// sgp4fix use old way of finding gst
|
|
int ids70;
|
|
double ts70, ds70, tfrac, c1, thgr70, fk5r, c1p2p;
|
|
const double twopi = 2.0 * pi;
|
|
|
|
/* ----------------------- earth constants ---------------------- */
|
|
// sgp4fix identify constants and allow alternate values
|
|
getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
|
|
x2o3 = 2.0 / 3.0;
|
|
|
|
/* ------------- calculate auxillary epoch quantities ---------- */
|
|
eccsq = ecco * ecco;
|
|
omeosq = 1.0 - eccsq;
|
|
rteosq = sqrt(omeosq);
|
|
cosio = cos(inclo);
|
|
cosio2 = cosio * cosio;
|
|
|
|
/* ------------------ un-kozai the mean motion ----------------- */
|
|
ak = pow(xke / no, x2o3);
|
|
d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq);
|
|
del = d1 / (ak * ak);
|
|
adel = ak * (1.0 - del * del - del *
|
|
(1.0 / 3.0 + 134.0 * del * del / 81.0));
|
|
del = d1/(adel * adel);
|
|
no = no / (1.0 + del);
|
|
|
|
ao = pow(xke / no, x2o3);
|
|
sinio = sin(inclo);
|
|
po = ao * omeosq;
|
|
con42 = 1.0 - 5.0 * cosio2;
|
|
con41 = -con42-cosio2-cosio2;
|
|
ainv = 1.0 / ao;
|
|
posq = po * po;
|
|
rp = ao * (1.0 - ecco);
|
|
method = 'n';
|
|
|
|
// sgp4fix modern approach to finding sidereal timew
|
|
// gsto = gstime(epoch + 2433281.5);
|
|
|
|
// sgp4fix use old way of finding gst
|
|
// count integer number of days from 0 jan 1970
|
|
ts70 = epoch - 7305.0;
|
|
ids70 = floor(ts70 + 1.0e-8);
|
|
ds70 = ids70;
|
|
tfrac = ts70 - ds70;
|
|
// find greenwich location at epoch
|
|
c1 = 1.72027916940703639e-2;
|
|
thgr70= 1.7321343856509374;
|
|
fk5r = 5.07551419432269442e-15;
|
|
c1p2p = c1 + twopi;
|
|
gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi);
|
|
if ( gsto < 0.0 )
|
|
gsto = gsto + twopi;
|
|
|
|
//#include "debug5.cpp"
|
|
} // end initl
|
|
|
|
/*-----------------------------------------------------------------------------
|
|
*
|
|
* procedure sgp4init
|
|
*
|
|
* this procedure initializes variables for sgp4.
|
|
*
|
|
* author : david vallado 719-573-2600 28 jun 2005
|
|
*
|
|
* inputs :
|
|
* satn - satellite number
|
|
* bstar - sgp4 type drag coefficient kg/m2er
|
|
* ecco - eccentricity
|
|
* epoch - epoch time in days from jan 0, 1950. 0 hr
|
|
* argpo - argument of perigee (output if ds)
|
|
* inclo - inclination
|
|
* mo - mean anomaly (output if ds)
|
|
* no - mean motion
|
|
* nodeo - right ascension of ascending node
|
|
*
|
|
* outputs :
|
|
* satrec - common values for subsequent calls
|
|
* return code - non-zero on error.
|
|
* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
|
|
* 2 - mean motion less than 0.0
|
|
* 3 - pert elements, ecc < 0.0 or ecc > 1.0
|
|
* 4 - semi-latus rectum < 0.0
|
|
* 5 - epoch elements are sub-orbital
|
|
* 6 - satellite has decayed
|
|
*
|
|
* locals :
|
|
* cnodm , snodm , cosim , sinim , cosomm , sinomm
|
|
* cc1sq , cc2 , cc3
|
|
* coef , coef1
|
|
* cosio4 -
|
|
* day -
|
|
* dndt -
|
|
* em - eccentricity
|
|
* emsq - eccentricity squared
|
|
* eeta -
|
|
* etasq -
|
|
* gam -
|
|
* argpm - argument of perigee
|
|
* nodem -
|
|
* inclm - inclination
|
|
* mm - mean anomaly
|
|
* nm - mean motion
|
|
* perige - perigee
|
|
* pinvsq -
|
|
* psisq -
|
|
* qzms24 -
|
|
* rtemsq -
|
|
* s1, s2, s3, s4, s5, s6, s7 -
|
|
* sfour -
|
|
* ss1, ss2, ss3, ss4, ss5, ss6, ss7 -
|
|
* sz1, sz2, sz3
|
|
* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 -
|
|
* tc -
|
|
* temp -
|
|
* temp1, temp2, temp3 -
|
|
* tsi -
|
|
* xpidot -
|
|
* xhdot1 -
|
|
* z1, z2, z3 -
|
|
* z11, z12, z13, z21, z22, z23, z31, z32, z33 -
|
|
*
|
|
* coupling :
|
|
* getgravconst-
|
|
* initl -
|
|
* dscom -
|
|
* dpper -
|
|
* dsinit -
|
|
* sgp4 -
|
|
*
|
|
* references :
|
|
* hoots, roehrich, norad spacetrack report #3 1980
|
|
* hoots, norad spacetrack report #6 1986
|
|
* hoots, schumacher and glover 2004
|
|
* vallado, crawford, hujsak, kelso 2006
|
|
----------------------------------------------------------------------------*/
|
|
|
|
int sgp4init
|
|
(
|
|
gravconsttype whichconst, const int satn, const double epoch,
|
|
const double xbstar, const double xecco, const double xargpo,
|
|
const double xinclo, const double xmo, const double xno,
|
|
const double xnodeo, elsetrec& satrec
|
|
)
|
|
{
|
|
/* --------------------- local variables ------------------------ */
|
|
double ao, ainv, con42, cosio, sinio, cosio2, eccsq,
|
|
omeosq, posq, rp, rteosq,
|
|
cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq ,
|
|
cc2 , cc3 , coef , coef1 , cosio4, day , dndt ,
|
|
em , emsq , eeta , etasq , gam , argpm , nodem ,
|
|
inclm , mm , nm , perige, pinvsq, psisq , qzms24,
|
|
rtemsq, s1 , s2 , s3 , s4 , s5 , s6 ,
|
|
s7 , sfour , ss1 = 0 , ss2 = 0 , ss3 = 0 , ss4 = 0 , ss5 = 0 ,
|
|
ss6 = 0 , ss7 = 0 , sz1 = 0 , sz2 = 0 , sz3 = 0 , sz11 = 0 , sz12 = 0 ,
|
|
sz13 = 0 , sz21 = 0 , sz22 = 0 , sz23 = 0 , sz31 = 0 , sz32 = 0 , sz33 = 0 ,
|
|
tc , temp , temp1 , temp2 , temp3 , tsi , xpidot,
|
|
xhdot1, z1 , z2 , z3 , z11 , z12 , z13 ,
|
|
z21 , z22 , z23 , z31 , z32 , z33,
|
|
qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3],
|
|
tumin, mu, radiusearthkm, xke, j3;
|
|
|
|
/* ------------------------ initialization --------------------- */
|
|
// sgp4fix divisor for divide by zero check on inclination
|
|
const double temp4 = 1.0 + cos(pi-1.0e-9);
|
|
|
|
/* ----------- set all near earth variables to zero ------------ */
|
|
satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0;
|
|
satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0;
|
|
satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0;
|
|
satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0;
|
|
satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0;
|
|
satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0;
|
|
satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0;
|
|
satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0;
|
|
satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0;
|
|
|
|
/* ----------- set all deep space variables to zero ------------ */
|
|
satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0;
|
|
satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0;
|
|
satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0;
|
|
satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0;
|
|
satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0;
|
|
satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0;
|
|
satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0;
|
|
satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0;
|
|
satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0;
|
|
satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0;
|
|
satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0;
|
|
satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0;
|
|
satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0;
|
|
satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0;
|
|
satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0;
|
|
satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0;
|
|
satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0;
|
|
satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0;
|
|
satrec.xli = 0.0; satrec.xni = 0.0;
|
|
|
|
// sgp4fix - note the following variables are also passed directly via satrec.
|
|
// it is possible to streamline the sgp4init call by deleting the "x"
|
|
// variables, but the user would need to set the satrec.* values first. we
|
|
// include the additional assignments in case twoline2rv is not used.
|
|
satrec.bstar = xbstar;
|
|
satrec.ecco = xecco;
|
|
satrec.argpo = xargpo;
|
|
satrec.inclo = xinclo;
|
|
satrec.mo = xmo;
|
|
satrec.no = xno;
|
|
satrec.nodeo = xnodeo;
|
|
|
|
/* ------------------------ earth constants ----------------------- */
|
|
// sgp4fix identify constants and allow alternate values
|
|
getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
|
|
ss = 78.0 / radiusearthkm + 1.0;
|
|
qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4);
|
|
x2o3 = 2.0 / 3.0;
|
|
|
|
satrec.init = 'y';
|
|
satrec.t = 0.0;
|
|
|
|
initl
|
|
(
|
|
satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method,
|
|
ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq,
|
|
posq, rp, rteosq, sinio, satrec.gsto
|
|
);
|
|
satrec.error = 0;
|
|
|
|
if (rp < 1.0)
|
|
{
|
|
// printf("# *** satn%d epoch elts sub-orbital ***\n", satn);
|
|
satrec.error = 5;
|
|
}
|
|
|
|
if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0))
|
|
{
|
|
satrec.isimp = 0;
|
|
if (rp < (220.0 / radiusearthkm + 1.0))
|
|
satrec.isimp = 1;
|
|
sfour = ss;
|
|
qzms24 = qzms2t;
|
|
perige = (rp - 1.0) * radiusearthkm;
|
|
|
|
/* - for perigees below 156 km, s and qoms2t are altered - */
|
|
if (perige < 156.0)
|
|
{
|
|
sfour = perige - 78.0;
|
|
if (perige < 98.0)
|
|
sfour = 20.0;
|
|
qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0);
|
|
sfour = sfour / radiusearthkm + 1.0;
|
|
}
|
|
pinvsq = 1.0 / posq;
|
|
|
|
tsi = 1.0 / (ao - sfour);
|
|
satrec.eta = ao * satrec.ecco * tsi;
|
|
etasq = satrec.eta * satrec.eta;
|
|
eeta = satrec.ecco * satrec.eta;
|
|
psisq = fabs(1.0 - etasq);
|
|
coef = qzms24 * pow(tsi, 4.0);
|
|
coef1 = coef / pow(psisq, 3.5);
|
|
cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta *
|
|
(4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 *
|
|
(8.0 + 3.0 * etasq * (8.0 + etasq)));
|
|
satrec.cc1 = satrec.bstar * cc2;
|
|
cc3 = 0.0;
|
|
if (satrec.ecco > 1.0e-4)
|
|
cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco;
|
|
satrec.x1mth2 = 1.0 - cosio2;
|
|
satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq *
|
|
(satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco *
|
|
(0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) *
|
|
(-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq *
|
|
(1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 *
|
|
(2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo)));
|
|
satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 *
|
|
(etasq + eeta) + eeta * etasq);
|
|
cosio4 = cosio2 * cosio2;
|
|
temp1 = 1.5 * j2 * pinvsq * satrec.no;
|
|
temp2 = 0.5 * temp1 * j2 * pinvsq;
|
|
temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no;
|
|
satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 *
|
|
temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4);
|
|
satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 *
|
|
(7.0 - 114.0 * cosio2 + 395.0 * cosio4) +
|
|
temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4);
|
|
xhdot1 = -temp1 * cosio;
|
|
satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) +
|
|
2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio;
|
|
xpidot = satrec.argpdot+ satrec.nodedot;
|
|
satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo);
|
|
satrec.xmcof = 0.0;
|
|
if (satrec.ecco > 1.0e-4)
|
|
satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta;
|
|
satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1;
|
|
satrec.t2cof = 1.5 * satrec.cc1;
|
|
// sgp4fix for divide by zero with xinco = 180 deg
|
|
if (fabs(cosio+1.0) > 1.5e-12)
|
|
satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio);
|
|
else
|
|
satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4;
|
|
satrec.aycof = -0.5 * j3oj2 * sinio;
|
|
satrec.delmo = pow((1.0 + satrec.eta * cos(satrec.mo)), 3);
|
|
satrec.sinmao = sin(satrec.mo);
|
|
satrec.x7thm1 = 7.0 * cosio2 - 1.0;
|
|
|
|
/* --------------- deep space initialization ------------- */
|
|
if ((2*pi / satrec.no) >= 225.0)
|
|
{
|
|
satrec.method = 'd';
|
|
satrec.isimp = 1;
|
|
tc = 0.0;
|
|
inclm = satrec.inclo;
|
|
|
|
dscom
|
|
(
|
|
epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo,
|
|
satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm,
|
|
day, satrec.e3, satrec.ee2, em, emsq, gam,
|
|
satrec.peo, satrec.pgho, satrec.pho, satrec.pinco,
|
|
satrec.plo, rtemsq, satrec.se2, satrec.se3,
|
|
satrec.sgh2, satrec.sgh3, satrec.sgh4,
|
|
satrec.sh2, satrec.sh3, satrec.si2, satrec.si3,
|
|
satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5,
|
|
s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3,
|
|
sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33,
|
|
satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2,
|
|
satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2,
|
|
satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11,
|
|
z12, z13, z21, z22, z23, z31, z32, z33,
|
|
satrec.zmol, satrec.zmos
|
|
);
|
|
dpper
|
|
(
|
|
satrec.e3, satrec.ee2, satrec.peo, satrec.pgho,
|
|
satrec.pho, satrec.pinco, satrec.plo, satrec.se2,
|
|
satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4,
|
|
satrec.sh2, satrec.sh3, satrec.si2, satrec.si3,
|
|
satrec.sl2, satrec.sl3, satrec.sl4, satrec.t,
|
|
satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2,
|
|
satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2,
|
|
satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init,
|
|
satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo
|
|
);
|
|
|
|
argpm = 0.0;
|
|
nodem = 0.0;
|
|
mm = 0.0;
|
|
|
|
dsinit
|
|
(
|
|
whichconst,
|
|
cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4,
|
|
ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc,
|
|
satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo,
|
|
satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33,
|
|
satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem,
|
|
satrec.irez, satrec.atime,
|
|
satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 ,
|
|
satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232,
|
|
satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt,
|
|
satrec.dmdt, dndt, satrec.dnodt, satrec.domdt ,
|
|
satrec.del1, satrec.del2, satrec.del3, satrec.xfact,
|
|
satrec.xlamo, satrec.xli, satrec.xni
|
|
);
|
|
}
|
|
|
|
/* ----------- set variables if not deep space ----------- */
|
|
if (satrec.isimp != 1)
|
|
{
|
|
cc1sq = satrec.cc1 * satrec.cc1;
|
|
satrec.d2 = 4.0 * ao * tsi * cc1sq;
|
|
temp = satrec.d2 * tsi * satrec.cc1 / 3.0;
|
|
satrec.d3 = (17.0 * ao + sfour) * temp;
|
|
satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) *
|
|
satrec.cc1;
|
|
satrec.t3cof = satrec.d2 + 2.0 * cc1sq;
|
|
satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 *
|
|
(12.0 * satrec.d2 + 10.0 * cc1sq));
|
|
satrec.t5cof = 0.2 * (3.0 * satrec.d4 +
|
|
12.0 * satrec.cc1 * satrec.d3 +
|
|
6.0 * satrec.d2 * satrec.d2 +
|
|
15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq));
|
|
}
|
|
} // if omeosq = 0 ...
|
|
|
|
/* finally propogate to zero epoch to initialise all others. */
|
|
if(satrec.error == 0)
|
|
sgp4(whichconst, satrec, 0.0, r, v);
|
|
|
|
satrec.init = 'n';
|
|
|
|
//#include "debug6.cpp"
|
|
return satrec.error;
|
|
} // end sgp4init
|
|
|
|
/*-----------------------------------------------------------------------------
|
|
*
|
|
* procedure sgp4
|
|
*
|
|
* this procedure is the sgp4 prediction model from space command. this is an
|
|
* updated and combined version of sgp4 and sdp4, which were originally
|
|
* published separately in spacetrack report #3. this version follows the
|
|
* methodology from the aiaa paper (2006) describing the history and
|
|
* development of the code.
|
|
*
|
|
* author : david vallado 719-573-2600 28 jun 2005
|
|
*
|
|
* inputs :
|
|
* satrec - initialised structure from sgp4init() call.
|
|
* tsince - time eince epoch (minutes)
|
|
*
|
|
* outputs :
|
|
* r - position vector km
|
|
* v - velocity km/sec
|
|
* return code - non-zero on error.
|
|
* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er
|
|
* 2 - mean motion less than 0.0
|
|
* 3 - pert elements, ecc < 0.0 or ecc > 1.0
|
|
* 4 - semi-latus rectum < 0.0
|
|
* 5 - epoch elements are sub-orbital
|
|
* 6 - satellite has decayed
|
|
*
|
|
* locals :
|
|
* am -
|
|
* axnl, aynl -
|
|
* betal -
|
|
* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u ,
|
|
* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip ,
|
|
* cosisq , cossu , sinsu , cosu , sinu
|
|
* delm -
|
|
* delomg -
|
|
* dndt -
|
|
* eccm -
|
|
* emsq -
|
|
* ecose -
|
|
* el2 -
|
|
* eo1 -
|
|
* eccp -
|
|
* esine -
|
|
* argpm -
|
|
* argpp -
|
|
* omgadf -
|
|
* pl -
|
|
* r -
|
|
* rtemsq -
|
|
* rdotl -
|
|
* rl -
|
|
* rvdot -
|
|
* rvdotl -
|
|
* su -
|
|
* t2 , t3 , t4 , tc
|
|
* tem5, temp , temp1 , temp2 , tempa , tempe , templ
|
|
* u , ux , uy , uz , vx , vy , vz
|
|
* inclm - inclination
|
|
* mm - mean anomaly
|
|
* nm - mean motion
|
|
* nodem - right asc of ascending node
|
|
* xinc -
|
|
* xincp -
|
|
* xl -
|
|
* xlm -
|
|
* mp -
|
|
* xmdf -
|
|
* xmx -
|
|
* xmy -
|
|
* nodedf -
|
|
* xnode -
|
|
* nodep -
|
|
* np -
|
|
*
|
|
* coupling :
|
|
* getgravconst-
|
|
* dpper
|
|
* dpspace
|
|
*
|
|
* references :
|
|
* hoots, roehrich, norad spacetrack report #3 1980
|
|
* hoots, norad spacetrack report #6 1986
|
|
* hoots, schumacher and glover 2004
|
|
* vallado, crawford, hujsak, kelso 2006
|
|
----------------------------------------------------------------------------*/
|
|
|
|
int sgp4
|
|
(
|
|
gravconsttype whichconst, elsetrec& satrec, double tsince,
|
|
double r[3], double v[3]
|
|
)
|
|
{
|
|
double am , axnl , aynl , betal , cosim , cnod ,
|
|
cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu,
|
|
delm , delomg, em , emsq , ecose , el2 , eo1 ,
|
|
ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0,
|
|
mvt , rdotl , rl , rvdot , rvdotl, sinim ,
|
|
sin2u, sineo1, sini , sinip , sinsu , sinu ,
|
|
snod , su , t2 , t3 , t4 , tem5 , temp,
|
|
temp1, temp2 , tempa, tempe , templ , u , ux ,
|
|
uy , uz , vx , vy , vz , inclm , mm ,
|
|
nm , nodem, xinc , xincp , xl , xlm , mp ,
|
|
xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt,
|
|
twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm,
|
|
mu, vkmpersec;
|
|
int ktr;
|
|
|
|
/* ------------------ set mathematical constants --------------- */
|
|
// sgp4fix divisor for divide by zero check on inclination
|
|
const double temp4 = 1.0 + cos(pi-1.0e-9);
|
|
twopi = 2.0 * pi;
|
|
x2o3 = 2.0 / 3.0;
|
|
// sgp4fix identify constants and allow alternate values
|
|
getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 );
|
|
vkmpersec = radiusearthkm * xke/60.0;
|
|
|
|
/* --------------------- clear sgp4 error flag ----------------- */
|
|
satrec.t = tsince;
|
|
satrec.error = 0;
|
|
|
|
/* ------- update for secular gravity and atmospheric drag ----- */
|
|
xmdf = satrec.mo + satrec.mdot * satrec.t;
|
|
argpdf = satrec.argpo + satrec.argpdot * satrec.t;
|
|
nodedf = satrec.nodeo + satrec.nodedot * satrec.t;
|
|
argpm = argpdf;
|
|
mm = xmdf;
|
|
t2 = satrec.t * satrec.t;
|
|
nodem = nodedf + satrec.nodecf * t2;
|
|
tempa = 1.0 - satrec.cc1 * satrec.t;
|
|
tempe = satrec.bstar * satrec.cc4 * satrec.t;
|
|
templ = satrec.t2cof * t2;
|
|
|
|
if (satrec.isimp != 1)
|
|
{
|
|
delomg = satrec.omgcof * satrec.t;
|
|
delm = satrec.xmcof *
|
|
(pow((1.0 + satrec.eta * cos(xmdf)), 3) -
|
|
satrec.delmo);
|
|
temp = delomg + delm;
|
|
mm = xmdf + temp;
|
|
argpm = argpdf - temp;
|
|
t3 = t2 * satrec.t;
|
|
t4 = t3 * satrec.t;
|
|
tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 -
|
|
satrec.d4 * t4;
|
|
tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) -
|
|
satrec.sinmao);
|
|
templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof +
|
|
satrec.t * satrec.t5cof);
|
|
}
|
|
|
|
nm = satrec.no;
|
|
em = satrec.ecco;
|
|
inclm = satrec.inclo;
|
|
if (satrec.method == 'd')
|
|
{
|
|
tc = satrec.t;
|
|
dspace
|
|
(
|
|
satrec.irez,
|
|
satrec.d2201, satrec.d2211, satrec.d3210,
|
|
satrec.d3222, satrec.d4410, satrec.d4422,
|
|
satrec.d5220, satrec.d5232, satrec.d5421,
|
|
satrec.d5433, satrec.dedt, satrec.del1,
|
|
satrec.del2, satrec.del3, satrec.didt,
|
|
satrec.dmdt, satrec.dnodt, satrec.domdt,
|
|
satrec.argpo, satrec.argpdot, satrec.t, tc,
|
|
satrec.gsto, satrec.xfact, satrec.xlamo,
|
|
satrec.no, satrec.atime,
|
|
em, argpm, inclm, satrec.xli, mm, satrec.xni,
|
|
nodem, dndt, nm
|
|
);
|
|
} // if method = d
|
|
|
|
if (nm <= 0.0)
|
|
{
|
|
// printf("# error nm %f\n", nm);
|
|
satrec.error = 2;
|
|
}
|
|
am = pow((xke / nm),x2o3) * tempa * tempa;
|
|
nm = xke / pow(am, 1.5);
|
|
em = em - tempe;
|
|
|
|
// fix tolerance for error recognition
|
|
if ((em >= 1.0) || (em < -0.001) || (am < 0.95))
|
|
{
|
|
// printf("# error em %f\n", em);
|
|
satrec.error = 1;
|
|
}
|
|
if (em < 0.0)
|
|
em = 1.0e-6;
|
|
mm = mm + satrec.no * templ;
|
|
xlm = mm + argpm + nodem;
|
|
emsq = em * em;
|
|
temp = 1.0 - emsq;
|
|
|
|
nodem = fmod(nodem, twopi);
|
|
argpm = fmod(argpm, twopi);
|
|
xlm = fmod(xlm, twopi);
|
|
mm = fmod(xlm - argpm - nodem, twopi);
|
|
|
|
/* ----------------- compute extra mean quantities ------------- */
|
|
sinim = sin(inclm);
|
|
cosim = cos(inclm);
|
|
|
|
/* -------------------- add lunar-solar periodics -------------- */
|
|
ep = em;
|
|
xincp = inclm;
|
|
argpp = argpm;
|
|
nodep = nodem;
|
|
mp = mm;
|
|
sinip = sinim;
|
|
cosip = cosim;
|
|
if (satrec.method == 'd')
|
|
{
|
|
dpper
|
|
(
|
|
satrec.e3, satrec.ee2, satrec.peo,
|
|
satrec.pgho, satrec.pho, satrec.pinco,
|
|
satrec.plo, satrec.se2, satrec.se3,
|
|
satrec.sgh2, satrec.sgh3, satrec.sgh4,
|
|
satrec.sh2, satrec.sh3, satrec.si2,
|
|
satrec.si3, satrec.sl2, satrec.sl3,
|
|
satrec.sl4, satrec.t, satrec.xgh2,
|
|
satrec.xgh3, satrec.xgh4, satrec.xh2,
|
|
satrec.xh3, satrec.xi2, satrec.xi3,
|
|
satrec.xl2, satrec.xl3, satrec.xl4,
|
|
satrec.zmol, satrec.zmos, satrec.inclo,
|
|
'n', ep, xincp, nodep, argpp, mp
|
|
);
|
|
if (xincp < 0.0)
|
|
{
|
|
xincp = -xincp;
|
|
nodep = nodep + pi;
|
|
argpp = argpp - pi;
|
|
}
|
|
if ((ep < 0.0 ) || ( ep > 1.0))
|
|
{
|
|
// printf("# error ep %f\n", ep);
|
|
satrec.error = 3;
|
|
}
|
|
} // if method = d
|
|
|
|
/* -------------------- long period periodics ------------------ */
|
|
if (satrec.method == 'd')
|
|
{
|
|
sinip = sin(xincp);
|
|
cosip = cos(xincp);
|
|
satrec.aycof = -0.5*j3oj2*sinip;
|
|
// sgp4fix for divide by zero for xincp = 180 deg
|
|
if (fabs(cosip+1.0) > 1.5e-12)
|
|
satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip);
|
|
else
|
|
satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4;
|
|
}
|
|
axnl = ep * cos(argpp);
|
|
temp = 1.0 / (am * (1.0 - ep * ep));
|
|
aynl = ep* sin(argpp) + temp * satrec.aycof;
|
|
xl = mp + argpp + nodep + temp * satrec.xlcof * axnl;
|
|
|
|
/* --------------------- solve kepler's equation --------------- */
|
|
u = fmod(xl - nodep, twopi);
|
|
eo1 = u;
|
|
tem5 = 9999.9;
|
|
ktr = 1;
|
|
// sgp4fix for kepler iteration
|
|
// the following iteration needs better limits on corrections
|
|
while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) )
|
|
{
|
|
sineo1 = sin(eo1);
|
|
coseo1 = cos(eo1);
|
|
tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl;
|
|
tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5;
|
|
if(fabs(tem5) >= 0.95)
|
|
tem5 = tem5 > 0.0 ? 0.95 : -0.95;
|
|
eo1 = eo1 + tem5;
|
|
ktr = ktr + 1;
|
|
}
|
|
|
|
/* ------------- short period preliminary quantities ----------- */
|
|
ecose = axnl*coseo1 + aynl*sineo1;
|
|
esine = axnl*sineo1 - aynl*coseo1;
|
|
el2 = axnl*axnl + aynl*aynl;
|
|
pl = am*(1.0-el2);
|
|
if (pl < 0.0)
|
|
{
|
|
// printf("# error pl %f\n", pl);
|
|
satrec.error = 4;
|
|
}
|
|
else
|
|
{
|
|
rl = am * (1.0 - ecose);
|
|
rdotl = sqrt(am) * esine/rl;
|
|
rvdotl = sqrt(pl) / rl;
|
|
betal = sqrt(1.0 - el2);
|
|
temp = esine / (1.0 + betal);
|
|
sinu = am / rl * (sineo1 - aynl - axnl * temp);
|
|
cosu = am / rl * (coseo1 - axnl + aynl * temp);
|
|
su = atan2(sinu, cosu);
|
|
sin2u = (cosu + cosu) * sinu;
|
|
cos2u = 1.0 - 2.0 * sinu * sinu;
|
|
temp = 1.0 / pl;
|
|
temp1 = 0.5 * j2 * temp;
|
|
temp2 = temp1 * temp;
|
|
|
|
/* -------------- update for short period periodics ------------ */
|
|
if (satrec.method == 'd')
|
|
{
|
|
cosisq = cosip * cosip;
|
|
satrec.con41 = 3.0*cosisq - 1.0;
|
|
satrec.x1mth2 = 1.0 - cosisq;
|
|
satrec.x7thm1 = 7.0*cosisq - 1.0;
|
|
}
|
|
mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) +
|
|
0.5 * temp1 * satrec.x1mth2 * cos2u;
|
|
su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u;
|
|
xnode = nodep + 1.5 * temp2 * cosip * sin2u;
|
|
xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u;
|
|
mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke;
|
|
rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u +
|
|
1.5 * satrec.con41) / xke;
|
|
|
|
/* --------------------- orientation vectors ------------------- */
|
|
sinsu = sin(su);
|
|
cossu = cos(su);
|
|
snod = sin(xnode);
|
|
cnod = cos(xnode);
|
|
sini = sin(xinc);
|
|
cosi = cos(xinc);
|
|
xmx = -snod * cosi;
|
|
xmy = cnod * cosi;
|
|
ux = xmx * sinsu + cnod * cossu;
|
|
uy = xmy * sinsu + snod * cossu;
|
|
uz = sini * sinsu;
|
|
vx = xmx * cossu - cnod * sinsu;
|
|
vy = xmy * cossu - snod * sinsu;
|
|
vz = sini * cossu;
|
|
|
|
/* --------- position and velocity (in km and km/sec) ---------- */
|
|
r[0] = (mrt * ux)* radiusearthkm;
|
|
r[1] = (mrt * uy)* radiusearthkm;
|
|
r[2] = (mrt * uz)* radiusearthkm;
|
|
v[0] = (mvt * ux + rvdot * vx) * vkmpersec;
|
|
v[1] = (mvt * uy + rvdot * vy) * vkmpersec;
|
|
v[2] = (mvt * uz + rvdot * vz) * vkmpersec;
|
|
} // if pl > 0
|
|
|
|
// sgp4fix for decaying satellites
|
|
if (mrt < 1.0)
|
|
{
|
|
// printf("# decay condition %11.6f \n",mrt);
|
|
satrec.error = 6;
|
|
}
|
|
|
|
|
|
//#include "debug7.cpp"
|
|
return satrec.error;
|
|
} // end sgp4
|
|
|
|
|
|
/* -----------------------------------------------------------------------------
|
|
*
|
|
* function gstime
|
|
*
|
|
* this function finds the greenwich sidereal time.
|
|
*
|
|
* author : david vallado 719-573-2600 1 mar 2001
|
|
*
|
|
* inputs description range / units
|
|
* jdut1 - julian date in ut1 days from 4713 bc
|
|
*
|
|
* outputs :
|
|
* gstime - greenwich sidereal time 0 to 2pi rad
|
|
*
|
|
* locals :
|
|
* temp - temporary variable for doubles rad
|
|
* tut1 - julian centuries from the
|
|
* jan 1, 2000 12 h epoch (ut1)
|
|
*
|
|
* coupling :
|
|
* none
|
|
*
|
|
* references :
|
|
* vallado 2004, 191, eq 3-45
|
|
* --------------------------------------------------------------------------- */
|
|
|
|
double gstime
|
|
(
|
|
double jdut1
|
|
)
|
|
{
|
|
const double twopi = 2.0 * pi;
|
|
const double deg2rad = pi / 180.0;
|
|
double temp, tut1;
|
|
|
|
tut1 = (jdut1 - 2451545.0) / 36525.0;
|
|
temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 +
|
|
(876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec
|
|
temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad
|
|
|
|
// ------------------------ check quadrants ---------------------
|
|
if (temp < 0.0)
|
|
temp += twopi;
|
|
|
|
return temp;
|
|
} // end gstime
|
|
|
|
/* -----------------------------------------------------------------------------
|
|
*
|
|
* function getgravconst
|
|
*
|
|
* this function gets constants for the propagator. note that mu is identified to
|
|
* facilitiate comparisons with newer models. the common useage is wgs72.
|
|
*
|
|
* author : david vallado 719-573-2600 21 jul 2006
|
|
*
|
|
* inputs :
|
|
* whichconst - which set of constants to use wgs72old, wgs72, wgs84
|
|
*
|
|
* outputs :
|
|
* tumin - minutes in one time unit
|
|
* mu - earth gravitational parameter
|
|
* radiusearthkm - radius of the earth in km
|
|
* xke - reciprocal of tumin
|
|
* j2, j3, j4 - un-normalized zonal harmonic values
|
|
* j3oj2 - j3 divided by j2
|
|
*
|
|
* locals :
|
|
*
|
|
* coupling :
|
|
* none
|
|
*
|
|
* references :
|
|
* norad spacetrack report #3
|
|
* vallado, crawford, hujsak, kelso 2006
|
|
--------------------------------------------------------------------------- */
|
|
|
|
void getgravconst
|
|
(
|
|
gravconsttype whichconst,
|
|
double& tumin,
|
|
double& mu,
|
|
double& radiusearthkm,
|
|
double& xke,
|
|
double& j2,
|
|
double& j3,
|
|
double& j4,
|
|
double& j3oj2
|
|
)
|
|
{
|
|
|
|
switch (whichconst)
|
|
{
|
|
// -- wgs-72 low precision str#3 constants --
|
|
case wgs72old:
|
|
mu = 398600.79964; // in km3 / s2
|
|
radiusearthkm = 6378.135; // km
|
|
xke = 0.0743669161;
|
|
tumin = 1.0 / xke;
|
|
j2 = 0.001082616;
|
|
j3 = -0.00000253881;
|
|
j4 = -0.00000165597;
|
|
j3oj2 = j3 / j2;
|
|
break;
|
|
// ------------ wgs-72 constants ------------
|
|
case wgs72:
|
|
mu = 398600.8; // in km3 / s2
|
|
radiusearthkm = 6378.135; // km
|
|
xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu);
|
|
tumin = 1.0 / xke;
|
|
j2 = 0.001082616;
|
|
j3 = -0.00000253881;
|
|
j4 = -0.00000165597;
|
|
j3oj2 = j3 / j2;
|
|
break;
|
|
case wgs84:
|
|
// ------------ wgs-84 constants ------------
|
|
mu = 398600.5; // in km3 / s2
|
|
radiusearthkm = 6378.137; // km
|
|
xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu);
|
|
tumin = 1.0 / xke;
|
|
j2 = 0.00108262998905;
|
|
j3 = -0.00000253215306;
|
|
j4 = -0.00000161098761;
|
|
j3oj2 = j3 / j2;
|
|
break;
|
|
default:
|
|
fprintf(stderr,"unknown gravity option (%d)\n",whichconst);
|
|
break;
|
|
}
|
|
|
|
} // end getgravconst
|
|
|
|
|
|
|
|
|
|
|