1 #include "sgp4.h"
2
3 #include "defs.h"
4 #include "unsorted.h"
5
sgp4_init(const predict_orbital_elements_t * orbital_elements,struct _sgp4 * m)6 void sgp4_init(const predict_orbital_elements_t *orbital_elements, struct _sgp4 *m)
7 {
8 m->simpleFlag = 0;
9
10 //Calculate old TLE field values as used in the original sgp4
11 double temp_tle = TWO_PI/MINUTES_PER_DAY/MINUTES_PER_DAY;
12 m->bstar = orbital_elements->bstar_drag_term / AE;
13 m->xincl = orbital_elements->inclination * M_PI / 180.0;
14 m->xnodeo = orbital_elements->right_ascension * M_PI / 180.0;
15 m->eo = orbital_elements->eccentricity;
16 m->omegao = orbital_elements->argument_of_perigee * M_PI / 180.0;
17 m->xmo = orbital_elements->mean_anomaly * M_PI / 180.0;
18 m->xno = orbital_elements->mean_motion*temp_tle*MINUTES_PER_DAY;
19
20 double x1m5th, xhdot1,
21 a1, a3ovk2, ao,
22 betao, betao2, c1sq, c2, c3, coef, coef1, del1, delo, eeta, eosq,
23 etasq, perigee, pinvsq, psisq, qoms24, s4, temp, temp1, temp2,
24 temp3, theta2, theta4, tsi;
25
26 /* Recover original mean motion (m->xnodp) and */
27 /* semimajor axis (m->aodp) from input elements. */
28
29 a1=pow(XKE/m->xno,TWO_THIRD);
30 m->cosio=cos(m->xincl);
31 theta2=m->cosio*m->cosio;
32 m->x3thm1=3*theta2-1.0;
33 eosq=m->eo*m->eo;
34 betao2=1.0-eosq;
35 betao=sqrt(betao2);
36 del1=1.5*CK2*m->x3thm1/(a1*a1*betao*betao2);
37 ao=a1*(1.0-del1*(0.5*TWO_THIRD+del1*(1.0+134.0/81.0*del1)));
38 delo=1.5*CK2*m->x3thm1/(ao*ao*betao*betao2);
39 m->xnodp=m->xno/(1.0+delo);
40 m->aodp=ao/(1.0-delo);
41
42 /* For perigee less than 220 kilometers, the "simple" */
43 /* flag is set and the equations are truncated to linear */
44 /* variation in sqrt a and quadratic variation in mean */
45 /* anomaly. Also, the c3 term, the delta omega term, and */
46 /* the delta m term are dropped. */
47
48 if ((m->aodp*(1-m->eo)/AE)<(220/EARTH_RADIUS_KM_WGS84+AE))
49 m->simpleFlag = true;
50 else
51 m->simpleFlag = false;
52
53 /* For perigees below 156 km, the */
54 /* values of s and qoms2t are altered. */
55
56 s4=S_DENSITY_PARAM;
57 qoms24=QOMS2T;
58 perigee=(m->aodp*(1-m->eo)-AE)*EARTH_RADIUS_KM_WGS84;
59
60 if (perigee<156.0)
61 {
62 if (perigee<=98.0)
63 s4=20;
64 else
65 s4=perigee-78.0;
66
67 qoms24=pow((120-s4)*AE/EARTH_RADIUS_KM_WGS84,4);
68 s4=s4/EARTH_RADIUS_KM_WGS84+AE;
69 }
70
71 pinvsq=1/(m->aodp*m->aodp*betao2*betao2);
72 tsi=1/(m->aodp-s4);
73 m->eta=m->aodp*m->eo*tsi;
74 etasq=m->eta*m->eta;
75 eeta=m->eo*m->eta;
76 psisq=fabs(1-etasq);
77 coef=qoms24*pow(tsi,4);
78 coef1=coef/pow(psisq,3.5);
79 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)));
80 m->c1=m->bstar*c2;
81 m->sinio=sin(m->xincl);
82 a3ovk2=-J3_HARMONIC_WGS72/CK2*pow(AE,3);
83 c3=coef*tsi*a3ovk2*m->xnodp*AE*m->sinio/m->eo;
84 m->x1mth2=1-theta2;
85
86 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)));
87 m->c5=2*coef1*m->aodp*betao2*(1+2.75*(etasq+eeta)+eeta*etasq);
88
89 theta4=theta2*theta2;
90 temp1=3*CK2*pinvsq*m->xnodp;
91 temp2=temp1*CK2*pinvsq;
92 temp3=1.25*CK4*pinvsq*pinvsq*m->xnodp;
93 m->xmdot=m->xnodp+0.5*temp1*betao*m->x3thm1+0.0625*temp2*betao*(13-78*theta2+137*theta4);
94 x1m5th=1-5*theta2;
95 m->omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7-114*theta2+395*theta4)+temp3*(3-36*theta2+49*theta4);
96 xhdot1=-temp1*m->cosio;
97 m->xnodot=xhdot1+(0.5*temp2*(4-19*theta2)+2*temp3*(3-7*theta2))*m->cosio;
98 m->omgcof=m->bstar*c3*cos(m->omegao);
99 m->xmcof=-TWO_THIRD*coef*m->bstar*AE/eeta;
100 m->xnodcf=3.5*betao2*xhdot1*m->c1;
101 m->t2cof=1.5*m->c1;
102 m->xlcof=0.125*a3ovk2*m->sinio*(3+5*m->cosio)/(1+m->cosio);
103 m->aycof=0.25*a3ovk2*m->sinio;
104 m->delmo=pow(1+m->eta*cos(m->xmo),3);
105 m->sinmo=sin(m->xmo);
106 m->x7thm1=7*theta2-1;
107
108 if (!m->simpleFlag) {
109 c1sq=m->c1*m->c1;
110 m->d2=4*m->aodp*tsi*c1sq;
111 temp=m->d2*tsi*m->c1/3;
112 m->d3=(17*m->aodp+s4)*temp;
113 m->d4=0.5*temp*m->aodp*tsi*(221*m->aodp+31*s4)*m->c1;
114 m->t3cof=m->d2+2*c1sq;
115 m->t4cof=0.25*(3*m->d3+m->c1*(12*m->d2+10*c1sq));
116 m->t5cof=0.2*(3*m->d4+12*m->c1*m->d3+6*m->d2*m->d2+15*c1sq*(2*m->d2+c1sq));
117 }
118 }
119
sgp4_predict(const struct _sgp4 * m,double tsince,struct model_output * output)120 void sgp4_predict(const struct _sgp4 *m, double tsince, struct model_output *output)
121 {
122 double cosuk, sinuk, rfdotk, vx, vy, vz, ux, uy, uz, xmy, xmx, cosnok,
123 sinnok, cosik, sinik, rdotk, xinck, xnodek, uk, rk, cos2u, sin2u,
124 u, sinu, cosu, betal, rfdot, rdot, r, pl, elsq, esine, ecose, epw,
125 cosepw, tfour, sinepw, capu, ayn, xlt, aynl, xll,
126 axn, xn, beta, xl, e, a, tcube, delm, delomg, templ, tempe, tempa,
127 xnode, tsq, xmp, omega, xnoddf, omgadf, xmdf, temp, temp1, temp2,
128 temp3, temp4, temp5, temp6;
129
130 int i;
131
132 /* Update for secular gravity and atmospheric drag. */
133 xmdf=m->xmo+m->xmdot*tsince;
134 omgadf=m->omegao+m->omgdot*tsince;
135 xnoddf=m->xnodeo+m->xnodot*tsince;
136 omega=omgadf;
137 xmp=xmdf;
138 tsq=tsince*tsince;
139 xnode=xnoddf+m->xnodcf*tsq;
140 tempa=1-m->c1*tsince;
141 tempe=m->bstar*m->c4*tsince;
142 templ=m->t2cof*tsq;
143
144 if (!m->simpleFlag) {
145
146 delomg=m->omgcof*tsince;
147 delm=m->xmcof*(pow(1+m->eta*cos(xmdf),3)-m->delmo);
148 temp=delomg+delm;
149 xmp=xmdf+temp;
150 omega=omgadf-temp;
151 tcube=tsq*tsince;
152 tfour=tsince*tcube;
153 tempa=tempa-m->d2*tsq-m->d3*tcube-m->d4*tfour;
154 tempe=tempe+m->bstar*m->c5*(sin(xmp)-m->sinmo);
155 templ=templ+m->t3cof*tcube+tfour*(m->t4cof+tsince*m->t5cof);
156 }
157
158 a=m->aodp*pow(tempa,2);
159 e=m->eo-tempe;
160 xl=xmp+omega+xnode+m->xnodp*templ;
161 beta=sqrt(1-e*e);
162 xn=XKE/pow(a,1.5);
163
164 /* Long period periodics */
165 axn=e*cos(omega);
166 temp=1/(a*beta*beta);
167 xll=temp*m->xlcof*axn;
168 aynl=temp*m->aycof;
169 xlt=xl+xll;
170 ayn=e*sin(omega)+aynl;
171
172 /* Solve Kepler's Equation */
173 capu=FMod2p(xlt-xnode);
174 temp2=capu;
175 i=0;
176
177 do
178 {
179 sinepw=sin(temp2);
180 cosepw=cos(temp2);
181 temp3=axn*sinepw;
182 temp4=ayn*cosepw;
183 temp5=axn*cosepw;
184 temp6=ayn*sinepw;
185 epw=(capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2;
186
187 if (fabs(epw-temp2)<= E6A)
188 break;
189
190 temp2=epw;
191
192 } while (i++<10);
193
194 /* Short period preliminary quantities */
195 ecose=temp5+temp6;
196 esine=temp3-temp4;
197 elsq=axn*axn+ayn*ayn;
198 temp=1-elsq;
199 pl=a*temp;
200 r=a*(1-ecose);
201 temp1=1/r;
202 rdot=XKE*sqrt(a)*esine*temp1;
203 rfdot=XKE*sqrt(pl)*temp1;
204 temp2=a*temp1;
205 betal=sqrt(temp);
206 temp3=1/(1+betal);
207 cosu=temp2*(cosepw-axn+ayn*esine*temp3);
208 sinu=temp2*(sinepw-ayn-axn*esine*temp3);
209 u=atan2(sinu,cosu);
210 sin2u=2*sinu*cosu;
211 cos2u=2*cosu*cosu-1;
212 temp=1/pl;
213 temp1=CK2*temp;
214 temp2=temp1*temp;
215
216 /* Update for short periodics */
217 rk=r*(1-1.5*temp2*betal*m->x3thm1)+0.5*temp1*m->x1mth2*cos2u;
218 uk=u-0.25*temp2*m->x7thm1*sin2u;
219 xnodek=xnode+1.5*temp2*m->cosio*sin2u;
220 xinck=m->xincl+1.5*temp2*m->cosio*m->sinio*cos2u;
221 rdotk=rdot-xn*temp1*m->x1mth2*sin2u;
222 rfdotk=rfdot+xn*temp1*(m->x1mth2*cos2u+1.5*m->x3thm1);
223
224 /* Orientation vectors */
225 sinuk=sin(uk);
226 cosuk=cos(uk);
227 sinik=sin(xinck);
228 cosik=cos(xinck);
229 sinnok=sin(xnodek);
230 cosnok=cos(xnodek);
231 xmx=-sinnok*cosik;
232 xmy=cosnok*cosik;
233 ux=xmx*sinuk+cosnok*cosuk;
234 uy=xmy*sinuk+sinnok*cosuk;
235 uz=sinik*sinuk;
236 vx=xmx*cosuk-cosnok*sinuk;
237 vy=xmy*cosuk-sinnok*sinuk;
238 vz=sinik*cosuk;
239
240 /* Position and velocity */
241 output->pos[0] = rk*ux;
242 output->pos[1] = rk*uy;
243 output->pos[2] = rk*uz;
244 output->vel[0] = rdotk*ux+rfdotk*vx;
245 output->vel[1] = rdotk*uy+rfdotk*vy;
246 output->vel[2] = rdotk*uz+rfdotk*vz;
247
248 /* Phase in radians */
249 output->phase=xlt-xnode-omgadf+TWO_PI;
250
251 if (output->phase<0.0)
252 output->phase+=TWO_PI;
253
254 output->phase=FMod2p(output->phase);
255
256 output->xinck = xinck;
257 output->omgadf = omgadf;
258 output->xnodek = xnodek;
259
260 }
261