📄 sgp4unit.cpp
字号:
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, thgr, thgro;
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 -
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -