1 #include <stdlib.h>
2 #include <math.h>
3 #undef SING
4
5 #include "sattypes.h"
6 #include "vector.h"
7 #include "satspec.h"
8
9 /* SDP4 3 NOV 80 */
10 /* SUBROUTINE SDP4(IFLAG,TSINCE)
11 COMMON/E1/XMO,XNODEO,OMEGAO,EO,XINCL,XNO,XNDT2O,
12 1 XNDD6O,BSTAR,X,Y,Z,XDOT,YDOT,ZDOT,EPOCH,DS50
13 COMMON/C1/CK2,CK4,E6A,QOMS2T,S,TOTHRD,
14 1 XJ3,XKE,XKMPER,XMNPDA,AE
15 DOUBLE PRECISION EPOCH, DS50
16 */
17
18 #define XMO (sat->elem->se_XMO)
19 #define XNODEO (sat->elem->se_XNODEO)
20 #define OMEGAO (sat->elem->se_OMEGAO)
21 #define EO (sat->elem->se_EO)
22 #define XINCL (sat->elem->se_XINCL)
23 #define XNO (sat->elem->se_XNO)
24 #define XNDT20 (sat->elem->se_XNDT20)
25 #define XNDD60 (sat->elem->se_XNDD60)
26 #define BSTAR (sat->elem->se_BSTAR)
27 #define EPOCH (sat->elem->se_EPOCH)
28
29 #define CK2 (5.413080e-04)
30 #define CK4 (6.209887e-07)
31 #define QOMS2T (1.880279e-09)
32 #define S (1.012229e+00)
33
34 #define AE (1.0)
35 #define DE2RA (.174532925E-1)
36 #define E6A (1.E-6)
37 #define PI (3.14159265)
38 #define PIO2 (1.57079633)
39 #define QO (120.0)
40 #define SO (78.0)
41 #define TOTHRD (.66666667)
42 #define TWOPI (6.2831853)
43 #define X3PIO2 (4.71238898)
44 #define XJ2 (1.082616E-3)
45 #define XJ3 (-.253881E-5)
46 #define XJ4 (-1.65597E-6)
47 #define XKE (.743669161E-1)
48 #define XKMPER (6378.135)
49 #define XMNPDA (1440.)
50
51 /* int IFLAG; */
52
53 #define X (pos->sl_X)
54 #define XDOT (pos->sl_XDOT)
55 #define Y (pos->sl_Y)
56 #define YDOT (pos->sl_YDOT)
57 #define Z (pos->sl_Z)
58 #define ZDOT (pos->sl_ZDOT)
59
60 /* sat->prop.sdp4-> */
61 #define AODP (sat->prop.sdp4->sdp4_AODP)
62 #define AYCOF (sat->prop.sdp4->sdp4_AYCOF)
63 #define BETAO (sat->prop.sdp4->sdp4_BETAO)
64 #define BETAO2 (sat->prop.sdp4->sdp4_BETAO2)
65 #define C1 (sat->prop.sdp4->sdp4_C1)
66 #define C4 (sat->prop.sdp4->sdp4_C4)
67 #define COSG (sat->prop.sdp4->sdp4_COSG)
68 #define COSIO (sat->prop.sdp4->sdp4_COSIO)
69 #define EOSQ (sat->prop.sdp4->sdp4_EOSQ)
70 #define OMGDOT (sat->prop.sdp4->sdp4_OMGDOT)
71 #define SING (sat->prop.sdp4->sdp4_SING)
72 #define SINIO (sat->prop.sdp4->sdp4_SINIO)
73 #define T2COF (sat->prop.sdp4->sdp4_T2COF)
74 #define THETA2 (sat->prop.sdp4->sdp4_THETA2)
75 #define X1MTH2 (sat->prop.sdp4->sdp4_X1MTH2)
76 #define X3THM1 (sat->prop.sdp4->sdp4_X3THM1)
77 #define X7THM1 (sat->prop.sdp4->sdp4_X7THM1)
78 #define XLCOF (sat->prop.sdp4->sdp4_XLCOF)
79 #define XMDOT (sat->prop.sdp4->sdp4_XMDOT)
80 #define XNODCF (sat->prop.sdp4->sdp4_XNODCF)
81 #define XNODOT (sat->prop.sdp4->sdp4_XNODOT)
82 #define XNODP (sat->prop.sdp4->sdp4_XNODP)
83
84 #define XMDF_seco (sat->prop.sdp4->sdp4_XMDF_seco)
85 #define OMGADF_seco (sat->prop.sdp4->sdp4_OMGADF_seco)
86 #define XNODE_seco (sat->prop.sdp4->sdp4_XNODE_seco)
87 #define EM_seco (sat->prop.sdp4->sdp4_EM_seco)
88 #define XINC_seco (sat->prop.sdp4->sdp4_XINC_seco)
89 #define XN_seco (sat->prop.sdp4->sdp4_XN_seco)
90
91 #define E_pero (sat->prop.sdp4->sdp4_E_pero)
92 #define XINC_pero (sat->prop.sdp4->sdp4_XINC_pero)
93 #define OMGADF_pero (sat->prop.sdp4->sdp4_OMGADF_pero)
94 #define XNODE_pero (sat->prop.sdp4->sdp4_XNODE_pero)
95 #define XMAM_pero (sat->prop.sdp4->sdp4_XMAM_pero)
96
97 void
sdp4(SatData * sat,Vec3 * pos,Vec3 * dpos,double TSINCE)98 sdp4 (SatData *sat, Vec3 *pos, Vec3 *dpos, double TSINCE)
99 {
100 int i;
101
102 /* private temporary variables used only in init section */
103 double A1,A3OVK2,AO,C2,COEF,COEF1,DEL1,DELO,EETA,ETA,
104 ETASQ,PERIGE,PINVSQ,PSISQ,QOMS24,S4,THETA4,TSI,X1M5TH,XHDOT1;
105
106 /* private temporary variables */
107 double A,AXN,AYN,AYNL,BETA,BETAL,CAPU,COS2U,COSEPW=0,
108 COSIK,COSNOK,COSU,COSUK,E,ECOSE,ELSQ,EM=0,EPW,ESINE,OMGADF,PL,
109 R,RDOT,RDOTK,RFDOT,RFDOTK,RK,SIN2U,SINEPW=0,SINIK,SINNOK,
110 SINU,SINUK,TEMP,TEMP1,TEMP2,TEMP3=0,TEMP4=0,TEMP5=0,TEMP6=0,TEMPA,
111 TEMPE,TEMPL,TSQ,U,UK,UX,UY,UZ,VX,VY,VZ,XINC=0,XINCK,XL,XLL,XLT,
112 XMAM,XMDF,XMX,XMY,XN,XNODDF,XNODE,XNODEK;
113
114 #if 0
115 A1=A3OVK2=AO=C2=COEF=COEF1=DEL1=DELO=EETA=ETA = signaling_nan();
116 ETASQ=PERIGE=PINVSQ=PSISQ=QOMS24=S4=THETA4=TSI=X1M5TH=XHDOT1 = signaling_nan();
117
118 A=AXN=AYN=AYNL=BETA=BETAL=CAPU=COS2U=COSEPW = signaling_nan();
119 COSIK=COSNOK=COSU=COSUK=E=ECOSE=ELSQ=EM=EPW=ESINE=OMGADF=PL = signaling_nan();
120 R=RDOT=RDOTK=RFDOT=RFDOTK=RK=SIN2U=SINEPW=SINIK=SINNOK = signaling_nan();
121 SINU=SINUK=TEMP=TEMP1=TEMP2=TEMP3=TEMP4=TEMP5=TEMP6=TEMPA = signaling_nan();
122 TEMPE=TEMPL=TSQ=U=UK=UX=UY=UZ=VX=VY=VZ=XINC=XINCK=XL=XLL=XLT = signaling_nan();
123 XMAM=XMDF=XMX=XMY=XN=XNODDF=XNODE=XNODEK = signaling_nan();
124 #endif
125
126 if(TSINCE != 0.0 && !sat->prop.sdp4) {
127 /*
128 * Yes, this is a recursive call.
129 */
130 sdp4(sat, pos, dpos, 0.0);
131 }
132
133 /* IF (IFLAG .EQ. 0) GO TO 100 */
134 /* if(!IFLAG) */
135 if(!sat->prop.sdp4) {
136 sat->prop.sdp4 = (struct sdp4_data *) malloc(sizeof(struct sdp4_data));
137
138 /* init_sdp4(sat->prop.sdp4); */
139
140 /* RECOVER ORIGINAL MEAN MOTION (XNODP) AND SEMIMAJOR AXIS (AODP) */
141 /* FROM INPUT ELEMENTS */
142
143 A1=pow((XKE/XNO), TOTHRD);
144 COSIO=cos(XINCL);
145 THETA2=COSIO*COSIO;
146 X3THM1=3.0 * THETA2 - 1.0;
147 EOSQ = EO * EO;
148 BETAO2 = 1.0 - EOSQ;
149 BETAO = sqrt(BETAO2);
150 DEL1 = 1.5 * CK2 * X3THM1 / (A1 * A1 * BETAO * BETAO2);
151 AO = A1 * (1.0 - DEL1 * (0.5 * TOTHRD +
152 DEL1 * (1.0 + 134.0 / 81.0 * DEL1)));
153 DELO = 1.5 * CK2 * X3THM1 / (AO * AO * BETAO * BETAO2);
154 XNODP = XNO / (1.0 + DELO);
155 AODP = AO / (1.0 - DELO);
156
157 /* INITIALIZATION */
158
159 /* FOR PERIGEE BELOW 156 KM, THE VALUES OF
160 * S AND QOMS2T ARE ALTERED */
161
162 S4 = S;
163 QOMS24 = QOMS2T;
164 PERIGE = (AODP * (1.0 - EO) - AE) * XKMPER;
165
166 /* IF(PERIGE .GE. 156.) GO TO 10 */
167
168 if(PERIGE < 156.0) {
169 S4 = PERIGE - 78.0;
170
171 if(PERIGE <= 98.0) { /* GO TO 9 */
172 S4 = 20.0;
173 }
174
175 QOMS24 = pow((120.0 - S4) * AE / XKMPER, 4.0); /* 9 */
176 S4 = S4 / XKMPER + AE;
177 }
178 PINVSQ = 1.0 / (AODP * AODP * BETAO2 * BETAO2); /* 10 */
179 SING = sin(OMEGAO);
180 COSG = cos(OMEGAO);
181 TSI = 1.0 / (AODP - S4);
182 ETA = AODP * EO * TSI;
183 ETASQ = ETA * ETA;
184 EETA = EO * ETA;
185 PSISQ = fabs(1.0 - ETASQ);
186 COEF = QOMS24 * pow(TSI, 4.0);
187 COEF1 = COEF / pow(PSISQ, 3.5);
188 C2 = COEF1 * XNODP * (AODP * (1.0 + 1.5 * ETASQ +
189 EETA * (4.0 + ETASQ)) +
190 .75 * CK2 * TSI / PSISQ * X3THM1 *
191 (8.0 + 3.0 * ETASQ * (8.0 + ETASQ)));
192 C1 = BSTAR * C2;
193 SINIO = sin(XINCL);
194 A3OVK2 = -XJ3 / CK2 * AE * AE * AE; /* A3OVK2=-XJ3/CK2*AE**3; */
195 X1MTH2 = 1.0 - THETA2;
196 C4 = 2.0 * XNODP * COEF1 * AODP * BETAO2 *
197 (ETA * (2.0 + .5 * ETASQ) + EO * (.5 + 2.0 * ETASQ) -
198 2.0 * CK2 * TSI / (AODP * PSISQ) *
199 (-3.0 * X3THM1 * (1.0 - 2.0 * EETA + ETASQ *
200 (1.5 - .5 * EETA)) +
201 .75 * X1MTH2 * (2.0 * ETASQ - EETA *
202 (1.0 + ETASQ)) * cos(2.0 * OMEGAO)));
203 THETA4 = THETA2 * THETA2;
204 TEMP1 = 3.0 * CK2 * PINVSQ * XNODP;
205 TEMP2 = TEMP1 * CK2 * PINVSQ;
206 TEMP3 = 1.25 * CK4 * PINVSQ * PINVSQ * XNODP;
207 XMDOT = XNODP + 0.5 * TEMP1 * BETAO * X3THM1 + .0625 * TEMP2 * BETAO *
208 (13.0 - 78.0 * THETA2 + 137.0 * THETA4);
209 X1M5TH=1.0 - 5.0 * THETA2;
210 OMGDOT = -.5 * TEMP1 * X1M5TH + .0625 * TEMP2 *
211 (7.0 - 114.0 * THETA2 + 395.0 * THETA4) +
212 TEMP3 * (3.0 - 36.0 * THETA2 + 49.0 * THETA4);
213 XHDOT1 = -TEMP1 * COSIO;
214 XNODOT = XHDOT1 + (.5 * TEMP2 * (4.0 - 19.0 * THETA2) +
215 2.0 * TEMP3 * (3.0 - 7.0 * THETA2)) * COSIO;
216 XNODCF = 3.5 * BETAO2 * XHDOT1 * C1;
217 T2COF = 1.5 * C1;
218 XLCOF = .125 * A3OVK2 * SINIO * (3.0 + 5.0 * COSIO) / (1.0 + COSIO);
219 AYCOF = .25 * A3OVK2 * SINIO;
220 X7THM1 = 7.0 * THETA2 - 1.0;
221 /* 90 IFLAG=0 */
222
223 #ifdef SDP_DEEP_DEBUG
224 printf("calling dpinit\n");
225 printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
226 EOSQ,SINIO,COSIO,BETAO,AODP,THETA2,
227 SING,COSG,BETAO2,XMDOT,OMGDOT,XNODOT,XNODP);
228 #endif
229 dpinit(sat, EOSQ, SINIO, COSIO, BETAO, AODP, THETA2,
230 SING, COSG, BETAO2, XMDOT, OMGDOT, XNODOT, XNODP);
231
232 /* CALL DPINIT(EOSQ,SINIO,COSIO,BETAO,AODP,THETA2,
233 1 SING,COSG,BETAO2,XMDOT,OMGDOT,XNODOT,XNODP) */
234
235 /* UPDATE FOR SECULAR GRAVITY AND ATMOSPHERIC DRAG */
236 }
237
238 XMDF = XMO + XMDOT * TSINCE; /* 100 */
239 OMGADF = OMEGAO + OMGDOT * TSINCE;
240 XNODDF = XNODEO + XNODOT * TSINCE;
241 TSQ = TSINCE * TSINCE;
242 XNODE = XNODDF + XNODCF * TSQ;
243 TEMPA = 1.0 - C1 * TSINCE;
244 TEMPE = BSTAR * C4 * TSINCE;
245 TEMPL = T2COF * TSQ;
246 XN = XNODP;
247
248 if(TSINCE == 0.0) {
249 XMDF_seco = XMDF;
250 OMGADF_seco = OMGADF;
251 XNODE_seco = XNODE;
252 EM_seco = EM;
253 XINC_seco = XINC;
254 XN_seco = XN;
255 }
256
257 dpsec(sat, &XMDF, &OMGADF, &XNODE, &EM, &XINC, &XN, TSINCE);
258
259 if(TSINCE == 0.0) {
260 XMDF_seco = XMDF - XMDF_seco;
261 OMGADF_seco = OMGADF - OMGADF_seco;
262 XNODE_seco = XNODE - XNODE_seco;
263 EM_seco = EM - EM_seco;
264 XINC_seco = XINC - XINC_seco;
265 XN_seco = XN - XN_seco;
266
267 #if 0
268 printf("XMDF_seco = %e\n", XMDF_seco);
269 printf("OMGADF_seco = %e\n", OMGADF_seco);
270 printf("XNODE_seco = %e\n", XNODE_seco);
271 printf("EM_seco = %e\n", EM_seco);
272 printf("XINC_seco = %e\n", XINC_seco);
273 printf("XN_seco = %e\n", XN_seco);
274 #endif
275 }
276
277 /*
278 XMDF -= XMDF_seco;
279 OMGADF -= OMGADF_seco;
280 XNODE -= XNODE_seco;
281 EM -= EM_seco;
282 XINC -= XINC_seco;
283 XN -= XN_seco;
284 */
285
286 A = pow(XKE/XN, TOTHRD) * TEMPA * TEMPA;
287 E = EM - TEMPE;
288 #ifdef SDP_DEEP_DEBUG
289 printf("*** E = %f\n", E);
290 #endif
291 XMAM = XMDF + XNODP * TEMPL;
292 /* CALL DPPER(E,XINC,OMGADF,XNODE,XMAM) */
293
294 #ifdef SDP_DEEP_DEBUG
295 printf("%12s %12s %12s %12s %12s\n",
296 "E", "XINC", "OMGADF", "XNODE", "XMAM");
297 printf("%12f %12f %12f %12f %12f\n",
298 E, XINC, OMGADF, XNODE, XMAM);
299 #endif
300
301 if(TSINCE == 0.0) {
302 E_pero = E;
303 XINC_pero = XINC;
304 OMGADF_pero = OMGADF;
305 XNODE_pero = XNODE;
306 XMAM_pero = XMAM;
307 }
308
309 dpper(sat, &E, &XINC, &OMGADF, &XNODE, &XMAM, TSINCE);
310
311 if(TSINCE == 0.0) {
312 E_pero = E - E_pero;
313 XINC_pero = XINC - XINC_pero;
314 OMGADF_pero = OMGADF - OMGADF_pero;
315 XNODE_pero = XNODE - XNODE_pero;
316 XMAM_pero = XMAM - XMAM_pero;
317
318 #if 0
319 printf("E_pero = %e\n", E_pero);
320 printf("XINC_pero = %e\n", XINC_pero);
321 printf("OMGADF_pero = %e\n", OMGADF_pero);
322 printf("XNODE_pero = %e\n", XNODE_pero);
323 printf("XMAM_pero = %e\n\n", XMAM_pero);
324 #endif
325 }
326
327 /*
328 E -= E_pero;
329 XINC -= XINC_pero;
330 OMGADF -= OMGADF_pero;
331 XNODE -= XNODE_pero;
332 XMAM -= XMAM_pero;
333 */
334 XL = XMAM + OMGADF + XNODE;
335 BETA = sqrt(1.0 - E * E);
336 XN = XKE / pow(A, 1.5);
337
338 /* LONG PERIOD PERIODICS */
339
340 AXN = E * cos(OMGADF);
341 TEMP=1./(A*BETA*BETA);
342 XLL=TEMP*XLCOF*AXN;
343 AYNL=TEMP*AYCOF;
344 XLT=XL+XLL;
345 AYN=E*sin(OMGADF)+AYNL;
346
347 /* SOLVE KEPLERS EQUATION */
348
349 CAPU=fmod(XLT-XNODE, TWOPI);
350 TEMP2=CAPU;
351 /* DO 130 I=1,10*/
352 for(i = 1; i < 10; i++) {
353 SINEPW=sin(TEMP2);
354 COSEPW=cos(TEMP2);
355 TEMP3=AXN*SINEPW;
356 TEMP4=AYN*COSEPW;
357 TEMP5=AXN*COSEPW;
358 TEMP6=AYN*SINEPW;
359 EPW=(CAPU-TEMP4+TEMP3-TEMP2)/(1.-TEMP5-TEMP6)+TEMP2;
360 /* IF(ABS(EPW-TEMP2) .LE. E6A) GO TO 140 */
361 if(fabs(EPW-TEMP2) <= E6A)
362 break;
363 TEMP2=EPW; /* 130 */
364 }
365
366 /* SHORT PERIOD PRELIMINARY QUANTITIES */
367
368 ECOSE=TEMP5+TEMP6; /* 140 */
369 ESINE=TEMP3-TEMP4;
370 ELSQ=AXN*AXN+AYN*AYN;
371 TEMP=1.-ELSQ;
372 PL=A*TEMP;
373 R=A*(1.-ECOSE);
374 TEMP1=1./R;
375 RDOT=XKE*sqrt(A)*ESINE*TEMP1;
376 RFDOT=XKE*sqrt(PL)*TEMP1;
377 TEMP2=A*TEMP1;
378 BETAL=sqrt(TEMP);
379 TEMP3=1./(1.+BETAL);
380 COSU=TEMP2*(COSEPW-AXN+AYN*ESINE*TEMP3);
381 SINU=TEMP2*(SINEPW-AYN-AXN*ESINE*TEMP3);
382 U=actan(SINU,COSU);
383 SIN2U=2.*SINU*COSU;
384 COS2U=2.*COSU*COSU-1.0;
385 TEMP=1./PL;
386 TEMP1=CK2*TEMP;
387 TEMP2=TEMP1*TEMP;
388
389 /* UPDATE FOR SHORT PERIODICS */
390
391 RK=R*(1.-1.5*TEMP2*BETAL*X3THM1)+.5*TEMP1*X1MTH2*COS2U;
392 UK=U - .25 * TEMP2 * X7THM1 * SIN2U;
393 XNODEK=XNODE+1.5*TEMP2*COSIO*SIN2U;
394 XINCK=XINC+1.5*TEMP2*COSIO*SINIO*COS2U;
395 RDOTK=RDOT-XN*TEMP1*X1MTH2*SIN2U;
396 RFDOTK=RFDOT+XN*TEMP1*(X1MTH2*COS2U+1.5*X3THM1);
397
398 /* ORIENTATION VECTORS */
399 SINUK=sin(UK);
400 COSUK=cos(UK);
401 SINIK=sin(XINCK);
402 COSIK=cos(XINCK);
403 SINNOK=sin(XNODEK);
404 COSNOK=cos(XNODEK);
405 XMX=-SINNOK*COSIK;
406 XMY=COSNOK*COSIK;
407 UX=XMX*SINUK+COSNOK*COSUK;
408 UY=XMY*SINUK+SINNOK*COSUK;
409 UZ=SINIK*SINUK;
410 VX=XMX*COSUK-COSNOK*SINUK;
411 VY=XMY*COSUK-SINNOK*SINUK;
412 VZ=SINIK*COSUK;
413 #if 0
414 printf("UX = %f VX = %f RK = %f RDOTK = %f RFDOTK = %f\n",
415 UX, VX, RK, RDOTK, RFDOTK);
416 #endif
417 /* POSITION AND VELOCITY */
418
419 pos->x = RK*UX;
420 pos->y = RK*UY;
421 pos->z = RK*UZ;
422 dpos->x = RDOTK*UX+RFDOTK*VX;
423 dpos->y = RDOTK*UY+RFDOTK*VY;
424 dpos->z = RDOTK*UZ+RFDOTK*VZ;
425 /* RETURN
426 END */
427 }
428
429