mirror of
https://github.com/SatDump/SatDump
synced 2026-08-13 17:47:30 -04:00
260 lines
7.2 KiB
C
260 lines
7.2 KiB
C
#include "sgp4.h"
|
|
|
|
#include "defs.h"
|
|
#include "unsorted.h"
|
|
|
|
void sgp4_init(const predict_orbital_elements_t *orbital_elements, struct _sgp4 *m)
|
|
{
|
|
m->simpleFlag = 0;
|
|
|
|
//Calculate old TLE field values as used in the original sgp4
|
|
double temp_tle = TWO_PI/MINUTES_PER_DAY/MINUTES_PER_DAY;
|
|
m->bstar = orbital_elements->bstar_drag_term / AE;
|
|
m->xincl = orbital_elements->inclination * M_PI / 180.0;
|
|
m->xnodeo = orbital_elements->right_ascension * M_PI / 180.0;
|
|
m->eo = orbital_elements->eccentricity;
|
|
m->omegao = orbital_elements->argument_of_perigee * M_PI / 180.0;
|
|
m->xmo = orbital_elements->mean_anomaly * M_PI / 180.0;
|
|
m->xno = orbital_elements->mean_motion*temp_tle*MINUTES_PER_DAY;
|
|
|
|
double x1m5th, xhdot1,
|
|
a1, a3ovk2, ao,
|
|
betao, betao2, c1sq, c2, c3, coef, coef1, del1, delo, eeta, eosq,
|
|
etasq, perigee, pinvsq, psisq, qoms24, s4, temp, temp1, temp2,
|
|
temp3, theta2, theta4, tsi;
|
|
|
|
/* Recover original mean motion (m->xnodp) and */
|
|
/* semimajor axis (m->aodp) from input elements. */
|
|
|
|
a1=pow(XKE/m->xno,TWO_THIRD);
|
|
m->cosio=cos(m->xincl);
|
|
theta2=m->cosio*m->cosio;
|
|
m->x3thm1=3*theta2-1.0;
|
|
eosq=m->eo*m->eo;
|
|
betao2=1.0-eosq;
|
|
betao=sqrt(betao2);
|
|
del1=1.5*CK2*m->x3thm1/(a1*a1*betao*betao2);
|
|
ao=a1*(1.0-del1*(0.5*TWO_THIRD+del1*(1.0+134.0/81.0*del1)));
|
|
delo=1.5*CK2*m->x3thm1/(ao*ao*betao*betao2);
|
|
m->xnodp=m->xno/(1.0+delo);
|
|
m->aodp=ao/(1.0-delo);
|
|
|
|
/* For perigee less than 220 kilometers, the "simple" */
|
|
/* flag is set and the equations are truncated to linear */
|
|
/* variation in sqrt a and quadratic variation in mean */
|
|
/* anomaly. Also, the c3 term, the delta omega term, and */
|
|
/* the delta m term are dropped. */
|
|
|
|
if ((m->aodp*(1-m->eo)/AE)<(220/EARTH_RADIUS_KM_WGS84+AE))
|
|
m->simpleFlag = true;
|
|
else
|
|
m->simpleFlag = false;
|
|
|
|
/* For perigees below 156 km, the */
|
|
/* values of s and qoms2t are altered. */
|
|
|
|
s4=S_DENSITY_PARAM;
|
|
qoms24=QOMS2T;
|
|
perigee=(m->aodp*(1-m->eo)-AE)*EARTH_RADIUS_KM_WGS84;
|
|
|
|
if (perigee<156.0)
|
|
{
|
|
if (perigee<=98.0)
|
|
s4=20;
|
|
else
|
|
s4=perigee-78.0;
|
|
|
|
qoms24=pow((120-s4)*AE/EARTH_RADIUS_KM_WGS84,4);
|
|
s4=s4/EARTH_RADIUS_KM_WGS84+AE;
|
|
}
|
|
|
|
pinvsq=1/(m->aodp*m->aodp*betao2*betao2);
|
|
tsi=1/(m->aodp-s4);
|
|
m->eta=m->aodp*m->eo*tsi;
|
|
etasq=m->eta*m->eta;
|
|
eeta=m->eo*m->eta;
|
|
psisq=fabs(1-etasq);
|
|
coef=qoms24*pow(tsi,4);
|
|
coef1=coef/pow(psisq,3.5);
|
|
c2=coef1*m->xnodp*(m->aodp*(1+1.5*etasq+eeta*(4+etasq))+0.75*CK2*tsi/psisq*m->x3thm1*(8+3*etasq*(8+etasq)));
|
|
m->c1=m->bstar*c2;
|
|
m->sinio=sin(m->xincl);
|
|
a3ovk2=-J3_HARMONIC_WGS72/CK2*pow(AE,3);
|
|
c3=coef*tsi*a3ovk2*m->xnodp*AE*m->sinio/m->eo;
|
|
m->x1mth2=1-theta2;
|
|
|
|
m->c4=2*m->xnodp*coef1*m->aodp*betao2*(m->eta*(2+0.5*etasq)+m->eo*(0.5+2*etasq)-2*CK2*tsi/(m->aodp*psisq)*(-3*m->x3thm1*(1-2*eeta+etasq*(1.5-0.5*eeta))+0.75*m->x1mth2*(2*etasq-eeta*(1+etasq))*cos(2*m->omegao)));
|
|
m->c5=2*coef1*m->aodp*betao2*(1+2.75*(etasq+eeta)+eeta*etasq);
|
|
|
|
theta4=theta2*theta2;
|
|
temp1=3*CK2*pinvsq*m->xnodp;
|
|
temp2=temp1*CK2*pinvsq;
|
|
temp3=1.25*CK4*pinvsq*pinvsq*m->xnodp;
|
|
m->xmdot=m->xnodp+0.5*temp1*betao*m->x3thm1+0.0625*temp2*betao*(13-78*theta2+137*theta4);
|
|
x1m5th=1-5*theta2;
|
|
m->omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7-114*theta2+395*theta4)+temp3*(3-36*theta2+49*theta4);
|
|
xhdot1=-temp1*m->cosio;
|
|
m->xnodot=xhdot1+(0.5*temp2*(4-19*theta2)+2*temp3*(3-7*theta2))*m->cosio;
|
|
m->omgcof=m->bstar*c3*cos(m->omegao);
|
|
m->xmcof=-TWO_THIRD*coef*m->bstar*AE/eeta;
|
|
m->xnodcf=3.5*betao2*xhdot1*m->c1;
|
|
m->t2cof=1.5*m->c1;
|
|
m->xlcof=0.125*a3ovk2*m->sinio*(3+5*m->cosio)/(1+m->cosio);
|
|
m->aycof=0.25*a3ovk2*m->sinio;
|
|
m->delmo=pow(1+m->eta*cos(m->xmo),3);
|
|
m->sinmo=sin(m->xmo);
|
|
m->x7thm1=7*theta2-1;
|
|
|
|
if (!m->simpleFlag) {
|
|
c1sq=m->c1*m->c1;
|
|
m->d2=4*m->aodp*tsi*c1sq;
|
|
temp=m->d2*tsi*m->c1/3;
|
|
m->d3=(17*m->aodp+s4)*temp;
|
|
m->d4=0.5*temp*m->aodp*tsi*(221*m->aodp+31*s4)*m->c1;
|
|
m->t3cof=m->d2+2*c1sq;
|
|
m->t4cof=0.25*(3*m->d3+m->c1*(12*m->d2+10*c1sq));
|
|
m->t5cof=0.2*(3*m->d4+12*m->c1*m->d3+6*m->d2*m->d2+15*c1sq*(2*m->d2+c1sq));
|
|
}
|
|
}
|
|
|
|
void sgp4_predict(const struct _sgp4 *m, double tsince, struct model_output *output)
|
|
{
|
|
double cosuk, sinuk, rfdotk, vx, vy, vz, ux, uy, uz, xmy, xmx, cosnok,
|
|
sinnok, cosik, sinik, rdotk, xinck, xnodek, uk, rk, cos2u, sin2u,
|
|
u, sinu, cosu, betal, rfdot, rdot, r, pl, elsq, esine, ecose, epw,
|
|
cosepw, tfour, sinepw, capu, ayn, xlt, aynl, xll,
|
|
axn, xn, beta, xl, e, a, tcube, delm, delomg, templ, tempe, tempa,
|
|
xnode, tsq, xmp, omega, xnoddf, omgadf, xmdf, temp, temp1, temp2,
|
|
temp3, temp4, temp5, temp6;
|
|
|
|
int i;
|
|
|
|
/* Update for secular gravity and atmospheric drag. */
|
|
xmdf=m->xmo+m->xmdot*tsince;
|
|
omgadf=m->omegao+m->omgdot*tsince;
|
|
xnoddf=m->xnodeo+m->xnodot*tsince;
|
|
omega=omgadf;
|
|
xmp=xmdf;
|
|
tsq=tsince*tsince;
|
|
xnode=xnoddf+m->xnodcf*tsq;
|
|
tempa=1-m->c1*tsince;
|
|
tempe=m->bstar*m->c4*tsince;
|
|
templ=m->t2cof*tsq;
|
|
|
|
if (!m->simpleFlag) {
|
|
|
|
delomg=m->omgcof*tsince;
|
|
delm=m->xmcof*(pow(1+m->eta*cos(xmdf),3)-m->delmo);
|
|
temp=delomg+delm;
|
|
xmp=xmdf+temp;
|
|
omega=omgadf-temp;
|
|
tcube=tsq*tsince;
|
|
tfour=tsince*tcube;
|
|
tempa=tempa-m->d2*tsq-m->d3*tcube-m->d4*tfour;
|
|
tempe=tempe+m->bstar*m->c5*(sin(xmp)-m->sinmo);
|
|
templ=templ+m->t3cof*tcube+tfour*(m->t4cof+tsince*m->t5cof);
|
|
}
|
|
|
|
a=m->aodp*pow(tempa,2);
|
|
e=m->eo-tempe;
|
|
xl=xmp+omega+xnode+m->xnodp*templ;
|
|
beta=sqrt(1-e*e);
|
|
xn=XKE/pow(a,1.5);
|
|
|
|
/* Long period periodics */
|
|
axn=e*cos(omega);
|
|
temp=1/(a*beta*beta);
|
|
xll=temp*m->xlcof*axn;
|
|
aynl=temp*m->aycof;
|
|
xlt=xl+xll;
|
|
ayn=e*sin(omega)+aynl;
|
|
|
|
/* Solve Kepler's Equation */
|
|
capu=FMod2p(xlt-xnode);
|
|
temp2=capu;
|
|
i=0;
|
|
|
|
do
|
|
{
|
|
sinepw=sin(temp2);
|
|
cosepw=cos(temp2);
|
|
temp3=axn*sinepw;
|
|
temp4=ayn*cosepw;
|
|
temp5=axn*cosepw;
|
|
temp6=ayn*sinepw;
|
|
epw=(capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2;
|
|
|
|
if (fabs(epw-temp2)<= E6A)
|
|
break;
|
|
|
|
temp2=epw;
|
|
|
|
} while (i++<10);
|
|
|
|
/* Short period preliminary quantities */
|
|
ecose=temp5+temp6;
|
|
esine=temp3-temp4;
|
|
elsq=axn*axn+ayn*ayn;
|
|
temp=1-elsq;
|
|
pl=a*temp;
|
|
r=a*(1-ecose);
|
|
temp1=1/r;
|
|
rdot=XKE*sqrt(a)*esine*temp1;
|
|
rfdot=XKE*sqrt(pl)*temp1;
|
|
temp2=a*temp1;
|
|
betal=sqrt(temp);
|
|
temp3=1/(1+betal);
|
|
cosu=temp2*(cosepw-axn+ayn*esine*temp3);
|
|
sinu=temp2*(sinepw-ayn-axn*esine*temp3);
|
|
u=atan2(sinu,cosu);
|
|
sin2u=2*sinu*cosu;
|
|
cos2u=2*cosu*cosu-1;
|
|
temp=1/pl;
|
|
temp1=CK2*temp;
|
|
temp2=temp1*temp;
|
|
|
|
/* Update for short periodics */
|
|
rk=r*(1-1.5*temp2*betal*m->x3thm1)+0.5*temp1*m->x1mth2*cos2u;
|
|
uk=u-0.25*temp2*m->x7thm1*sin2u;
|
|
xnodek=xnode+1.5*temp2*m->cosio*sin2u;
|
|
xinck=m->xincl+1.5*temp2*m->cosio*m->sinio*cos2u;
|
|
rdotk=rdot-xn*temp1*m->x1mth2*sin2u;
|
|
rfdotk=rfdot+xn*temp1*(m->x1mth2*cos2u+1.5*m->x3thm1);
|
|
|
|
/* Orientation vectors */
|
|
sinuk=sin(uk);
|
|
cosuk=cos(uk);
|
|
sinik=sin(xinck);
|
|
cosik=cos(xinck);
|
|
sinnok=sin(xnodek);
|
|
cosnok=cos(xnodek);
|
|
xmx=-sinnok*cosik;
|
|
xmy=cosnok*cosik;
|
|
ux=xmx*sinuk+cosnok*cosuk;
|
|
uy=xmy*sinuk+sinnok*cosuk;
|
|
uz=sinik*sinuk;
|
|
vx=xmx*cosuk-cosnok*sinuk;
|
|
vy=xmy*cosuk-sinnok*sinuk;
|
|
vz=sinik*cosuk;
|
|
|
|
/* Position and velocity */
|
|
output->pos[0] = rk*ux;
|
|
output->pos[1] = rk*uy;
|
|
output->pos[2] = rk*uz;
|
|
output->vel[0] = rdotk*ux+rfdotk*vx;
|
|
output->vel[1] = rdotk*uy+rfdotk*vy;
|
|
output->vel[2] = rdotk*uz+rfdotk*vz;
|
|
|
|
/* Phase in radians */
|
|
output->phase=xlt-xnode-omgadf+TWO_PI;
|
|
|
|
if (output->phase<0.0)
|
|
output->phase+=TWO_PI;
|
|
|
|
output->phase=FMod2p(output->phase);
|
|
|
|
output->xinck = xinck;
|
|
output->omgadf = omgadf;
|
|
output->xnodek = xnodek;
|
|
|
|
}
|