source: Sophya/trunk/SophyaExt/XephemAstroLib/sgp4.c@ 2818

Last change on this file since 2818 was 2818, checked in by cmv, 20 years ago

Update de Xephem 3.7 cmv 21/08/2005
File size: 10.9 KB
RevLine 
[1457]1#include <stdlib.h>
2#include <math.h>
3
4#include "sattypes.h"
5#include "vector.h"
6#include "satspec.h"
7
8#define XMO (sat->elem->se_XMO)
9#define XNODEO (sat->elem->se_XNODEO)
10#define OMEGAO (sat->elem->se_OMEGAO)
11#define EO (sat->elem->se_EO)
12#define XINCL (sat->elem->se_XINCL)
13#define XNO (sat->elem->se_XNO)
14#define XNDT20 (sat->elem->se_XNDT20)
15#define XNDD60 (sat->elem->se_XNDD60)
16#define BSTAR (sat->elem->se_BSTAR)
17#define EPOCH (sat->elem->se_EPOCH)
18
19#define X (pos->sl_X)
20#define XDOT (pos->sl_XDOT)
21#define Y (pos->sl_Y)
22#define YDOT (pos->sl_YDOT)
23#define Z (pos->sl_Z)
24#define ZDOT (pos->sl_ZDOT)
25
26#define AODP (sat->prop.sgp4->sgp4_AODP)
27#define AYCOF (sat->prop.sgp4->sgp4_AYCOF)
28#define C1 (sat->prop.sgp4->sgp4_C1)
29#define C4 (sat->prop.sgp4->sgp4_C4)
30#define C5 (sat->prop.sgp4->sgp4_C5)
31#define COSIO (sat->prop.sgp4->sgp4_COSIO)
32#define D2 (sat->prop.sgp4->sgp4_D2)
33#define D3 (sat->prop.sgp4->sgp4_D3)
34#define D4 (sat->prop.sgp4->sgp4_D4)
35#define DELMO (sat->prop.sgp4->sgp4_DELMO)
36#define ETA (sat->prop.sgp4->sgp4_ETA)
37#define OMGCOF (sat->prop.sgp4->sgp4_OMGCOF)
38#define OMGDOT (sat->prop.sgp4->sgp4_OMGDOT)
39#define SINIO (sat->prop.sgp4->sgp4_SINIO)
40#define SINMO (sat->prop.sgp4->sgp4_SINMO)
41#define T2COF (sat->prop.sgp4->sgp4_T2COF)
42#define T3COF (sat->prop.sgp4->sgp4_T3COF)
43#define T4COF (sat->prop.sgp4->sgp4_T4COF)
44#define T5COF (sat->prop.sgp4->sgp4_T5COF)
45#define X1MTH2 (sat->prop.sgp4->sgp4_X1MTH2)
46#define X3THM1 (sat->prop.sgp4->sgp4_X3THM1)
47#define X7THM1 (sat->prop.sgp4->sgp4_X7THM1)
48#define XLCOF (sat->prop.sgp4->sgp4_XLCOF)
49#define XMCOF (sat->prop.sgp4->sgp4_XMCOF)
50#define XMDOT (sat->prop.sgp4->sgp4_XMDOT)
51#define XNODCF (sat->prop.sgp4->sgp4_XNODCF)
52#define XNODOT (sat->prop.sgp4->sgp4_XNODOT)
53#define XNODP (sat->prop.sgp4->sgp4_XNODP)
54
55#define CK2 (5.413080e-04)
56#define CK4 (6.209887e-07)
57#define QOMS2T (1.880279e-09)
58#define S (1.012229e+00)
59
60#define AE (1.0)
61#define DE2RA (.174532925E-1)
62#define E6A (1.E-6)
63#define PI (3.14159265)
64#define PIO2 (1.57079633)
65#define QO (120.0)
66#define SO (78.0)
67#define TOTHRD (.66666667)
68#define TWOPI (6.2831853)
69#define X3PIO2 (4.71238898)
70#define XJ2 (1.082616E-3)
71#define XJ3 (-.253881E-5)
72#define XJ4 (-1.65597E-6)
73#define XKE (.743669161E-1)
74#define XKMPER (6378.135)
75#define XMNPDA (1440.0)
76
77/* compute position and velocity vectors for the satellite defined in sat->elem
78 * at its epoch + TSINCE.
79 */
80void
[2551]81sgp4(SatData *sat, Vec3 *pos, Vec3 *dpos, double TSINCE)
[1457]82{
83 int i;
84
85 double A1, A3OVK2, AO, BETAO, BETAO2, C1SQ, C2, C3, COEF, COEF1,
86 DEL1, DELO, EETA, EOSQ, ETASQ, PERIGE, PINVSQ, PSISQ, QOMS24,
[2551]87 S4, TEMP, TEMP1, TEMP2, TEMP3=0, THETA2, THETA4, TSI, X1M5TH,
[1457]88 XHDOT1;
89
[2551]90 double A, AXN, AYN, AYNL, BETA, BETAL, CAPU, COS2U, COSEPW=0, COSIK,
[1457]91 COSNOK, COSU, COSUK, DELM, DELOMG, E, ECOSE, ELSQ, EPW, ESINE,
92 OMEGA, OMGADF, PL, R, RDOT, RDOTK, RFDOT, RFDOTK, RK, SIN2U,
[2551]93 SINEPW=0, SINIK, SINNOK, SINU, SINUK, TCUBE, TEMP4=0, TEMP5=0, TEMP6=0,
[1457]94 TEMPA, TEMPE, TEMPL, TFOUR, TSQ, U, UK, UX, UY, UZ, VX, VY, VZ,
95 XINCK, XL, XLL, XLT, XMDF, XMP, XMX, XMY, XN, XNODDF, XNODE,
96 XNODEK;
97
98#if 0
99 A1 = A3OVK2 = AO = BETAO = BETAO2 = C1SQ = C2 = C3 = COEF = COEF1 =
100 DEL1 = DELO = EETA = EOSQ = ETASQ = PERIGE = PINVSQ = PSISQ = QOMS24 =
101 S4 = TEMP = TEMP1 = TEMP2 = TEMP3 = THETA2 = THETA4 = TSI = X1M5TH =
102 XHDOT1 = signaling_nan();
103
104 A = AXN = AYN = AYNL = BETA = BETAL = CAPU = COS2U = COSEPW = COSIK =
105 COSNOK = COSU = COSUK = DELM = DELOMG = E = ECOSE = ELSQ = EPW =
106 ESINE = OMEGA = OMGADF = PL = R = RDOT = RDOTK = RFDOT = RFDOTK =
107 RK = SIN2U = SINEPW = SINIK = SINNOK = SINU = SINUK = TCUBE = TEMP4 =
108 TEMP5 = TEMP6 = TEMPA = TEMPE = TEMPL = TFOUR = TSQ = U = UK = UX =
109 UY = UZ = VX = VY = VZ = XINCK = XL = XLL = XLT = XMDF = XMP = XMX =
110 XMY = XN = XNODDF = XNODE = XNODEK = signaling_nan();
111#endif
112
113 if(!sat->prop.sgp4) {
114 sat->prop.sgp4 = (struct sgp4_data *) malloc(sizeof(struct sgp4_data));
115
116 /*
117 * RECOVER ORIGINAL MEAN MOTION (XNODP) AND SEMIMAJOR AXIS (AODP)
118 * FROM INPUT ELEMENTS
119 */
120
121 A1 = pow((XKE/XNO), TOTHRD);
122 COSIO = cos(XINCL);
123 THETA2 = COSIO * COSIO;
124 X3THM1 = 3.0 * THETA2 - 1.0;
125 EOSQ = EO * EO;
126 BETAO2 = 1.0 - EOSQ;
127 BETAO = sqrt(BETAO2);
128 DEL1 = 1.5 * CK2 * X3THM1 / (A1 * A1 * BETAO * BETAO2);
129 AO = A1 * (1.0 - DEL1 * (.5 * TOTHRD +
130 DEL1 * (1.0 + 134.0 /81.0 * DEL1)));
131 DELO = 1.5 * CK2 * X3THM1 / (AO * AO * BETAO * BETAO2);
132 XNODP = XNO / (1.0 + DELO);
133 AODP=AO / (1.0 - DELO);
134
135 /*
136 * INITIALIZATION
137 *
138 * FOR PERIGEE LESS THAN 220 KILOMETERS, THE ISIMP FLAG IS SET AND
139 * THE EQUATIONS ARE TRUNCATED TO LINEAR VARIATION IN SQRT A AND
140 * QUADRATIC VARIATION IN MEAN ANOMALY. ALSO, THE C3 TERM, THE
141 * DELTA OMEGA TERM, AND THE DELTA M TERM ARE DROPPED.
142 */
143
144 sat->prop.sgp4->sgp4_flags = 0;
145
146 /* IF((AODP*(1.-EO)/AE) .LT. (220./XKMPER+AE)) ISIMP=1 */
147
148 if((AODP * (1.0 - EO) / AE) < (220.0 / XKMPER + AE))
149 sat->prop.sgp4->sgp4_flags |= SGP4_SIMPLE;
150
151 /*
152 * FOR PERIGEE BELOW 156 KM, THE VALUES OF
153 * S AND QOMS2T ARE ALTERED
154 */
155
156 S4 = S;
157 QOMS24 = QOMS2T;
158 PERIGE = (AODP * (1.0 - EO) - AE) * XKMPER;
159
160 if(PERIGE < 156.0) {
161 S4 = PERIGE - 78.0;
162
163 if(PERIGE <= 98.0)
164 S4 = 20.0;
165
166 QOMS24 = pow(((120.0 - S4) * AE / XKMPER), 4.0);
167 S4 = S4 / XKMPER + AE;
168 }
169
170 PINVSQ=1.0 / (AODP * AODP * BETAO2 * BETAO2);
171 TSI = 1.0 / (AODP - S4);
172 ETA = AODP * EO * TSI;
173 ETASQ = ETA * ETA;
174 EETA = EO * ETA;
175
176 PSISQ = fabs(1.0 - ETASQ);
177
178 COEF = QOMS24 * pow(TSI, 4.0);
179 COEF1 = COEF / pow(PSISQ, 3.5);
180
181 C2 = COEF1 * XNODP * (AODP * (1.0 + 1.5 * ETASQ +
182 EETA * (4.0 + ETASQ)) + .75 *
183 CK2 * TSI /
184 PSISQ * X3THM1 * (8.0 +
185 3.0 * ETASQ * (8.0 + ETASQ)));
186
187
188 C1 = BSTAR * C2;
189
190 SINIO = sin(XINCL);
191
192 A3OVK2 = -XJ3 / CK2 * pow(AE, 3.0);
193
194 C3 = COEF * TSI * A3OVK2 * XNODP * AE * SINIO / EO;
195
196 X1MTH2 = 1.0 - THETA2;
197 C4 = 2.0 * XNODP * COEF1 * AODP * BETAO2 *
198 (ETA * (2.0 + .5 * ETASQ) +
199 EO * (.5 + 2.0 * ETASQ) -
200 2.0 * CK2 * TSI / (AODP * PSISQ) *
201 (-3.0 * X3THM1 * (1.0 - 2.0 * EETA + ETASQ * (1.5 - .5 * EETA)) +
202 .75 * X1MTH2 * (2.0 * ETASQ - EETA * (1.0 + ETASQ)) *
203 cos(2.0 * OMEGAO)));
204
205 C5 = 2.0 * COEF1 * AODP * BETAO2 * (1.0 +
206 2.75 * (ETASQ + EETA) +
207 EETA * ETASQ);
208 THETA4 = THETA2 * THETA2;
209 TEMP1 = 3.0 * CK2 * PINVSQ * XNODP;
210 TEMP2 = TEMP1 * CK2 * PINVSQ;
211 TEMP3 = 1.25 * CK4 * PINVSQ * PINVSQ * XNODP;
212
213 XMDOT = XNODP +
214 .5 * TEMP1 * BETAO * X3THM1 +
215 .0625 * TEMP2 * BETAO * (13.0 - 78.0 * THETA2 + 137.0 * THETA4);
216
217 X1M5TH = 1.0 - 5.0 * THETA2;
218
219 OMGDOT = -.5 * TEMP1 * X1M5TH +
220 .0625 * TEMP2 * (7.0 - 114.0 * THETA2 + 395.0 * THETA4) +
221 TEMP3 * (3.0 - 36.0 * THETA2 + 49.0 * THETA4);
222
223 XHDOT1 = -TEMP1 * COSIO;
224
225 XNODOT = XHDOT1 + (.5 * TEMP2 * (4.0 - 19.0 * THETA2) +
226 2.0 * TEMP3 * (3.0 - 7.0 * THETA2)) * COSIO;
227
228 OMGCOF = BSTAR * C3 * cos(OMEGAO);
229
230 XMCOF = -TOTHRD * COEF * BSTAR * AE / EETA;
231 XNODCF = 3.5 * BETAO2 * XHDOT1 * C1;
232 T2COF = 1.5 * C1;
233 XLCOF = .125 * A3OVK2 * SINIO * (3.0 + 5.0 *COSIO) / (1.0 + COSIO);
234
235 AYCOF = .25 * A3OVK2 * SINIO;
236 DELMO = pow(1.0 + ETA * cos(XMO), 3.0);
237 SINMO = sin(XMO);
238
239 X7THM1 = 7.0 * THETA2 - 1.0;
240
241/* IF(ISIMP .EQ. 1) GO TO 90 */
242 if(!(sat->prop.sgp4->sgp4_flags & SGP4_SIMPLE)) {
243 C1SQ = C1 * C1;
244 D2 = 4.0 * AODP * TSI * C1SQ;
245 TEMP = D2 * TSI * C1 / 3.0;
246 D3 = (17.0 * AODP + S4) * TEMP;
247 D4 = .5 * TEMP * AODP * TSI * (221.0 * AODP + 31.0 * S4) * C1;
248 T3COF = D2 + 2.0 * C1SQ;
249 T4COF = .25 * (3.0 * D3 + C1 * (12.0 * D2 + 10.0 * C1SQ));
250 T5COF = .2 * (3.0 * D4 +
251 12.0 * C1 * D3 +
252 6.0 * D2 * D2 +
253 15.0 * C1SQ * (2.0 * D2 + C1SQ));
254 }
255 }
256
257 /*
258 * UPDATE FOR SECULAR GRAVITY AND ATMOSPHERIC DRAG
259 */
260
261 XMDF = XMO + XMDOT * TSINCE;
262 OMGADF = OMEGAO + OMGDOT * TSINCE;
263 XNODDF = XNODEO + XNODOT * TSINCE;
264 OMEGA = OMGADF;
265 XMP = XMDF;
266 TSQ = TSINCE * TSINCE;
267 XNODE = XNODDF + XNODCF * TSQ;
268 TEMPA = 1.0 - C1 * TSINCE;
269 TEMPE = BSTAR * C4 * TSINCE;
270 TEMPL = T2COF * TSQ;
271 if(!(sat->prop.sgp4->sgp4_flags & SGP4_SIMPLE)) {
272 DELOMG = OMGCOF * TSINCE;
273 DELM = XMCOF * (pow(1.0 + ETA * cos(XMDF), 3) - DELMO);
274 TEMP = DELOMG + DELM;
275 XMP = XMDF + TEMP;
276 OMEGA = OMGADF - TEMP;
277 TCUBE = TSQ * TSINCE;
278 TFOUR = TSINCE * TCUBE;
279 TEMPA = TEMPA - D2 * TSQ - D3 * TCUBE - D4 * TFOUR;
280 TEMPE = TEMPE + BSTAR * C5 * (sin(XMP) - SINMO);
281 TEMPL = TEMPL + T3COF * TCUBE + TFOUR * (T4COF + TSINCE * T5COF);
282 }
283
284 A = AODP * TEMPA * TEMPA;
285 E = EO - TEMPE;
286 XL = XMP + OMEGA + XNODE + XNODP * TEMPL;
287 BETA = sqrt(1.0 - E * E);
288 XN = XKE / pow(A, 1.5);
289
290 /*
291 * LONG PERIOD PERIODICS
292 */
293
294 AXN = E * cos(OMEGA);
295 TEMP = 1.0 / (A * BETA * BETA);
296 XLL = TEMP * XLCOF * AXN;
297 AYNL = TEMP * AYCOF;
298 XLT = XL + XLL;
299 AYN = E * sin(OMEGA) + AYNL;
300
301 /*
302 * SOLVE KEPLERS EQUATION
303 */
304
305 CAPU = fmod(XLT - XNODE, TWOPI);
306 TEMP2 = CAPU;
307
308 for(i = 0; i < 10; i++) {
309 SINEPW = sin(TEMP2);
310 COSEPW = cos(TEMP2);
311 TEMP3 = AXN * SINEPW;
312 TEMP4 = AYN * COSEPW;
313 TEMP5 = AXN * COSEPW;
314 TEMP6 = AYN * SINEPW;
315 EPW = (CAPU - TEMP4 + TEMP3 - TEMP2) / (1.0 - TEMP5 - TEMP6) + TEMP2;
316
317 if(fabs(EPW - TEMP2) <= E6A)
318 break;
319
320 TEMP2 = EPW;
321 }
322
323 /*
324 * SHORT PERIOD PRELIMINARY QUANTITIES
325 */
326
327 ECOSE = TEMP5 + TEMP6;
328 ESINE = TEMP3 - TEMP4;
329 ELSQ = AXN * AXN + AYN * AYN;
330 TEMP = 1.0 - ELSQ;
331 PL = A * TEMP;
332 R = A * (1.0 - ECOSE);
333
334 TEMP1 = 1.0 / R;
335 RDOT = XKE * sqrt(A) * ESINE * TEMP1;
336 RFDOT = XKE * sqrt(PL) * TEMP1;
337 TEMP2 = A * TEMP1;
338 BETAL = sqrt(TEMP);
339 TEMP3 = 1.0 / (1.0 + BETAL);
340
341 COSU = TEMP2 * (COSEPW - AXN + AYN * ESINE * TEMP3);
342 SINU = TEMP2 * (SINEPW - AYN - AXN * ESINE * TEMP3);
343
344 U = actan(SINU, COSU);
345
346 SIN2U = 2.0 * SINU * COSU;
347 COS2U = 2.0 * COSU * COSU - 1.0;
348
349 TEMP = 1.0 / PL;
350 TEMP1 = CK2 * TEMP;
351 TEMP2 = TEMP1 * TEMP;
352
353 /*
354 * UPDATE FOR SHORT PERIODICS
355 */
356
357 RK = R * (1.0 - 1.5 * TEMP2 * BETAL * X3THM1) +
358 .5 * TEMP1 * X1MTH2 * COS2U;
359
360 UK = U - .25 * TEMP2 * X7THM1 * SIN2U;
361
362 XNODEK = XNODE + 1.5 * TEMP2 * COSIO * SIN2U;
363 XINCK = XINCL + 1.5 * TEMP2 * COSIO * SINIO * COS2U;
364 RDOTK = RDOT - XN * TEMP1 * X1MTH2 * SIN2U;
365 RFDOTK = RFDOT + XN * TEMP1 * (X1MTH2 * COS2U + 1.5 * X3THM1);
366
367 /*
368 * ORIENTATION VECTORS
369 */
370
371 SINUK = sin(UK);
372 COSUK = cos(UK);
373 SINIK = sin(XINCK);
374 COSIK = cos(XINCK);
375 SINNOK = sin(XNODEK);
376 COSNOK = cos(XNODEK);
377
378 XMX = -SINNOK * COSIK;
379 XMY = COSNOK * COSIK;
380 UX = XMX * SINUK + COSNOK * COSUK;
381 UY = XMY * SINUK + SINNOK * COSUK;
382 UZ = SINIK * SINUK;
383 VX = XMX * COSUK - COSNOK * SINUK;
384 VY = XMY * COSUK - SINNOK * SINUK;
385 VZ = SINIK * COSUK;
386
387 /*
388 * POSITION AND VELOCITY
389 */
390
391 pos->x = RK * UX;
392 pos->y = RK * UY;
393 pos->z = RK * UZ;
394
395 dpos->x = RDOTK * UX + RFDOTK * VX;
396 dpos->y = RDOTK * UY + RFDOTK * VY;
397 dpos->z = RDOTK * UZ + RFDOTK * VZ;
398}
399
400/* For RCS Only -- Do Not Edit */
[2818]401static char *rcsid[2] = {(char *)rcsid, "@(#) $RCSfile: sgp4.c,v $ $Date: 2005-08-21 10:02:39 $ $Revision: 1.5 $ $Name: not supported by cvs2svn $"};
Note: See TracBrowser for help on using the repository browser.