1 /* -*- Mode: C; tab-width: 4; indent-tabs-mode: t; c-basic-offset: 4 -*- */
2 /*
3 * Unit SGP4SDP4
4 * Author: Dr TS Kelso
5 * Original Version: 1991 Oct 30
6 * Current Revision: 1992 Sep 03
7 * Version: 1.50
8 * Copyright: 1991-1992, All Rights Reserved
9 *
10 * Ported to C by: Neoklis Kyriazis April 10 2001
11 * Reentrancy mods by Alexandru Csete OZ9AEC
12 */
13
14 #include "sgp4sdp4.h"
15
16 /* SGP4 */
17 /* This function is used to calculate the position and velocity */
18 /* of near-earth (period < 225 minutes) satellites. tsince is */
19 /* time since epoch in minutes, tle is a pointer to a tle_t */
20 /* structure with Keplerian orbital elements and pos and vel */
21 /* are vector_t structures returning ECI satellite position and */
22 /* velocity. Use Convert_Sat_State() to convert to km and km/s.*/
23 void
SGP4(sat_t * sat,double tsince)24 SGP4 (sat_t *sat, double tsince)
25 {
26 double
27 cosuk,sinuk,rfdotk,vx,vy,vz,ux,uy,uz,xmy,xmx,
28 cosnok,sinnok,cosik,sinik,rdotk,xinck,xnodek,uk,
29 rk,cos2u,sin2u,u,sinu,cosu,betal,rfdot,rdot,r,pl,
30 elsq,esine,ecose,epw,cosepw,x1m5th,xhdot1,tfour,
31 sinepw,capu,ayn,xlt,aynl,xll,axn,xn,beta,xl,e,a,
32 tcube,delm,delomg,templ,tempe,tempa,xnode,tsq,xmp,
33 omega,xnoddf,omgadf,xmdf,a1,a3ovk2,ao,betao,betao2,
34 c1sq,c2,c3,coef,coef1,del1,delo,eeta,eosq,etasq,
35 perige,pinvsq,psisq,qoms24,s4,temp,temp1,temp2,
36 temp3,temp4,temp5,temp6,theta2,theta4,tsi;
37
38 int i;
39
40 /* Initialization */
41 if (~sat->flags & SGP4_INITIALIZED_FLAG) {
42 //if (!(sat->flags & SGP4_INITIALIZED_FLAG)) {
43
44 sat->flags |= SGP4_INITIALIZED_FLAG;
45
46 //g_print ("SAT %d INITIALISED.\n", sat->tle.catnr);
47
48 /* Recover original mean motion (xnodp) and */
49 /* semimajor axis (aodp) from input elements. */
50 a1 = pow (xke/sat->tle.xno, tothrd);
51 sat->sgps.cosio = cos (sat->tle.xincl);
52 theta2 = sat->sgps.cosio * sat->sgps.cosio;
53 sat->sgps.x3thm1 = 3 * theta2 - 1.0;
54 eosq = sat->tle.eo * sat->tle.eo;
55 betao2 = 1 - eosq;
56 betao = sqrt (betao2);
57 del1 = 1.5 * ck2 * sat->sgps.x3thm1 / (a1*a1*betao*betao2);
58 ao = a1*(1-del1*(0.5*tothrd+del1*(1+134.0/81.0*del1)));
59 delo = 1.5 * ck2 * sat->sgps.x3thm1 / (ao*ao*betao*betao2);
60 sat->sgps.xnodp = sat->tle.xno / (1.0 + delo);
61 sat->sgps.aodp = ao / (1.0 - delo);
62
63 /* For perigee less than 220 kilometers, the "simple" flag is set */
64 /* and the equations are truncated to linear variation in sqrt a */
65 /* and quadratic variation in mean anomaly. Also, the c3 term, */
66 /* the delta omega term, and the delta m term are dropped. */
67 if ((sat->sgps.aodp * (1.0 - sat->tle.eo) / ae) < (220.0 / xkmper + ae))
68 sat->flags |= SIMPLE_FLAG;
69 else
70 sat->flags &= ~SIMPLE_FLAG;
71
72 /* For perigee below 156 km, the */
73 /* values of s and qoms2t are altered. */
74 s4 = __s__;
75 qoms24 = qoms2t;
76 perige = (sat->sgps.aodp * (1 - sat->tle.eo) - ae) * xkmper;
77 if (perige < 156.0) {
78 if (perige <= 98.0)
79 s4 = 20.0;
80 else
81 s4 = perige - 78.0;
82 qoms24 = pow ((120.0 - s4) * ae / xkmper, 4);
83 s4 = s4 / xkmper + ae;
84 }; /* FIXME FIXME: End of if(perige <= 98) NO WAY!!!! */
85
86 pinvsq = 1.0 / (sat->sgps.aodp * sat->sgps.aodp * betao2 * betao2);
87 tsi = 1.0 / (sat->sgps.aodp - s4);
88 sat->sgps.eta = sat->sgps.aodp * sat->tle.eo * tsi;
89 etasq = sat->sgps.eta * sat->sgps.eta;
90 eeta = sat->tle.eo * sat->sgps.eta;
91 psisq = fabs (1.0 - etasq);
92 coef = qoms24 * pow (tsi, 4);
93 coef1 = coef / pow (psisq, 3.5);
94 c2 = coef1 * sat->sgps.xnodp * (sat->sgps.aodp *
95 (1.0 + 1.5 * etasq + eeta * (4.0 + etasq)) +
96 0.75 * ck2 * tsi / psisq * sat->sgps.x3thm1 *
97 (8.0 + 3.0 * etasq * (8 + etasq)));
98 sat->sgps.c1 = c2 * sat->tle.bstar;
99 sat->sgps.sinio = sin (sat->tle.xincl);
100 a3ovk2 = -xj3 / ck2 * pow (ae, 3);
101 c3 = coef * tsi * a3ovk2 * sat->sgps.xnodp * ae * sat->sgps.sinio / sat->tle.eo;
102 sat->sgps.x1mth2 = 1.0 - theta2;
103 sat->sgps.c4 = 2.0 * sat->sgps.xnodp * coef1 * sat->sgps.aodp * betao2 *
104 (sat->sgps.eta * (2.0 + 0.5 * etasq) +
105 sat->tle.eo * (0.5 + 2.0 * etasq) -
106 2.0 * ck2 * tsi / (sat->sgps.aodp * psisq) *
107 (-3.0 * sat->sgps.x3thm1 * (1.0 - 2.0 * eeta + etasq * (1.5 - 0.5 * eeta)) +
108 0.75 * sat->sgps.x1mth2 * (2.0 * etasq - eeta * (1.0 + etasq)) *
109 cos (2.0 * sat->tle.omegao)));
110 sat->sgps.c5 = 2.0 * coef1 * sat->sgps.aodp * betao2 *
111 (1.0 + 2.75 * (etasq + eeta) + eeta * etasq);
112 theta4 = theta2 * theta2;
113 temp1 = 3.0 * ck2 * pinvsq * sat->sgps.xnodp;
114 temp2 = temp1 * ck2 * pinvsq;
115 temp3 = 1.25 * ck4 * pinvsq * pinvsq * sat->sgps.xnodp;
116 sat->sgps.xmdot = sat->sgps.xnodp + 0.5 * temp1 * betao * sat->sgps.x3thm1 +
117 0.0625 * temp2 * betao * (13.0 - 78.0 * theta2 + 137.0 * theta4);
118 x1m5th = 1.0 - 5.0 * theta2;
119 sat->sgps.omgdot = -0.5 * temp1 * x1m5th +
120 0.0625 * temp2 * (7.0 - 114.0 * theta2 + 395.0 * theta4) +
121 temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4);
122 xhdot1 = -temp1 * sat->sgps.cosio;
123 sat->sgps.xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * theta2) +
124 2.0 * temp3 * (3.0 - 7.0 * theta2)) * sat->sgps.cosio;
125 sat->sgps.omgcof = sat->tle.bstar * c3 * cos (sat->tle.omegao);
126 sat->sgps.xmcof = -tothrd * coef * sat->tle.bstar * ae / eeta;
127 sat->sgps.xnodcf = 3.5 * betao2 * xhdot1 * sat->sgps.c1;
128 sat->sgps.t2cof = 1.5 * sat->sgps.c1;
129 sat->sgps.xlcof = 0.125 * a3ovk2 * sat->sgps.sinio *
130 (3.0 + 5.0 * sat->sgps.cosio) / (1.0 + sat->sgps.cosio);
131 sat->sgps.aycof = 0.25 * a3ovk2 * sat->sgps.sinio;
132 sat->sgps.delmo = pow (1.0 + sat->sgps.eta * cos (sat->tle.xmo), 3);
133 sat->sgps.sinmo = sin (sat->tle.xmo);
134 sat->sgps.x7thm1 = 7.0 * theta2 - 1.0;
135 if (~sat->flags & SIMPLE_FLAG) {
136 c1sq = sat->sgps.c1 * sat->sgps.c1;
137 sat->sgps.d2 = 4.0 * sat->sgps.aodp * tsi * c1sq;
138 temp = sat->sgps.d2 * tsi * sat->sgps.c1 / 3.0;
139 sat->sgps.d3 = (17.0 * sat->sgps.aodp + s4) * temp;
140 sat->sgps.d4 = 0.5 * temp * sat->sgps.aodp * tsi *
141 (221.0 * sat->sgps.aodp + 31.0 * s4) * sat->sgps.c1;
142 sat->sgps.t3cof = sat->sgps.d2 + 2.0 * c1sq;
143 sat->sgps.t4cof = 0.25 * (3.0 * sat->sgps.d3 + sat->sgps.c1 *
144 (12.0 * sat->sgps.d2 + 10.0 * c1sq));
145 sat->sgps.t5cof = 0.2 * (3.0 * sat->sgps.d4 +
146 12.0 * sat->sgps.c1 * sat->sgps.d3 +
147 6.0 * sat->sgps.d2 * sat->sgps.d2 +
148 15.0 * c1sq * (2.0 * sat->sgps.d2 + c1sq));
149 }; /* End of if (isFlagClear(SIMPLE_FLAG)) */
150 }; /* End of SGP4() initialization */
151
152 /* Update for secular gravity and atmospheric drag. */
153 xmdf = sat->tle.xmo + sat->sgps.xmdot * tsince;
154 omgadf = sat->tle.omegao + sat->sgps.omgdot * tsince;
155 xnoddf = sat->tle.xnodeo + sat->sgps.xnodot * tsince;
156 omega = omgadf;
157 xmp = xmdf;
158 tsq = tsince*tsince;
159 xnode = xnoddf + sat->sgps.xnodcf * tsq;
160 tempa = 1.0 - sat->sgps.c1 * tsince;
161 tempe = sat->tle.bstar * sat->sgps.c4 * tsince;
162 templ = sat->sgps.t2cof * tsq;
163 if (~sat->flags & SIMPLE_FLAG) {
164 delomg = sat->sgps.omgcof * tsince;
165 delm = sat->sgps.xmcof * (pow (1 + sat->sgps.eta * cos (xmdf), 3) - sat->sgps.delmo);
166 temp = delomg + delm;
167 xmp = xmdf + temp;
168 omega = omgadf - temp;
169 tcube = tsq * tsince;
170 tfour = tsince * tcube;
171 tempa = tempa - sat->sgps.d2 * tsq - sat->sgps.d3 * tcube - sat->sgps.d4 * tfour;
172 tempe = tempe + sat->tle.bstar * sat->sgps.c5 * (sin (xmp) - sat->sgps.sinmo);
173 templ = templ + sat->sgps.t3cof * tcube + tfour *
174 (sat->sgps.t4cof + tsince * sat->sgps.t5cof);
175 }; /* End of if (isFlagClear(SIMPLE_FLAG)) */
176
177 a = sat->sgps.aodp * pow (tempa, 2);
178 e = sat->tle.eo - tempe;
179 xl = xmp + omega + xnode + sat->sgps.xnodp * templ;
180 beta = sqrt (1.0 - e*e);
181 xn = xke / pow (a, 1.5);
182
183 /* Long period periodics */
184 axn = e * cos (omega);
185 temp = 1.0 / (a * beta * beta);
186 xll = temp * sat->sgps.xlcof * axn;
187 aynl = temp * sat->sgps.aycof;
188 xlt = xl + xll;
189 ayn = e * sin (omega) + aynl;
190
191 /* Solve Kepler's' Equation */
192 capu = FMod2p (xlt - xnode);
193 temp2 = capu;
194
195 i = 0;
196 do {
197 sinepw = sin (temp2);
198 cosepw = cos (temp2);
199 temp3 = axn * sinepw;
200 temp4 = ayn * cosepw;
201 temp5 = axn * cosepw;
202 temp6 = ayn * sinepw;
203 epw = (capu - temp4 + temp3 - temp2) / (1.0 - temp5 - temp6) + temp2;
204 if (fabs (epw - temp2) <= e6a)
205 break;
206 temp2 = epw;
207 }
208 while( i++ < 10 );
209
210 /* Short period preliminary quantities */
211 ecose = temp5 + temp6;
212 esine = temp3 - temp4;
213 elsq = axn*axn + ayn*ayn;
214 temp = 1.0 - elsq;
215 pl = a * temp;
216 r = a * (1.0 - ecose);
217 temp1 = 1.0 / r;
218 rdot = xke * sqrt (a) * esine * temp1;
219 rfdot = xke * sqrt (pl) * temp1;
220 temp2 = a * temp1;
221 betal = sqrt (temp);
222 temp3 = 1.0 / (1.0 + betal);
223 cosu = temp2 * (cosepw - axn + ayn * esine * temp3);
224 sinu = temp2 * (sinepw - ayn - axn * esine * temp3);
225 u = AcTan (sinu, cosu);
226 sin2u = 2.0 * sinu * cosu;
227 cos2u = 2.0 * cosu * cosu - 1.0;
228 temp = 1.0 / pl;
229 temp1 = ck2 * temp;
230 temp2 = temp1 * temp;
231
232 /* Update for short periodics */
233 rk = r * (1.0 - 1.5 * temp2 * betal * sat->sgps.x3thm1) +
234 0.5 * temp1 * sat->sgps.x1mth2 * cos2u;
235 uk = u - 0.25 * temp2 * sat->sgps.x7thm1 * sin2u;
236 xnodek = xnode + 1.5 * temp2 * sat->sgps.cosio * sin2u;
237 xinck = sat->tle.xincl + 1.5 * temp2 * sat->sgps.cosio * sat->sgps.sinio * cos2u;
238 rdotk = rdot - xn * temp1 * sat->sgps.x1mth2 * sin2u;
239 rfdotk = rfdot + xn * temp1 * (sat->sgps.x1mth2 * cos2u + 1.5 * sat->sgps.x3thm1);
240
241
242 /* Orientation vectors */
243 sinuk = sin (uk);
244 cosuk = cos (uk);
245 sinik = sin (xinck);
246 cosik = cos (xinck);
247 sinnok = sin (xnodek);
248 cosnok = cos (xnodek);
249 xmx = -sinnok * cosik;
250 xmy = cosnok * cosik;
251 ux = xmx * sinuk + cosnok * cosuk;
252 uy = xmy * sinuk + sinnok * cosuk;
253 uz = sinik * sinuk;
254 vx = xmx * cosuk - cosnok * sinuk;
255 vy = xmy * cosuk - sinnok * sinuk;
256 vz = sinik * cosuk;
257
258 /* Position and velocity */
259 sat->pos.x = rk*ux;
260 sat->pos.y = rk*uy;
261 sat->pos.z = rk*uz;
262 sat->vel.x = rdotk*ux+rfdotk*vx;
263 sat->vel.y = rdotk*uy+rfdotk*vy;
264 sat->vel.z = rdotk*uz+rfdotk*vz;
265
266 sat->phase = xlt - xnode - omgadf + twopi;
267 if (sat->phase < 0)
268 sat->phase += twopi;
269 sat->phase = FMod2p (sat->phase);
270
271 sat->tle.omegao1 = omega;
272 sat->tle.xincl1 = xinck;
273 sat->tle.xnodeo1 = xnodek;
274
275 } /*SGP4*/
276
277 /*------------------------------------------------------------------*/
278
279 /* SDP4 */
280 /* This function is used to calculate the position and velocity */
281 /* of deep-space (period > 225 minutes) satellites. tsince is */
282 /* time since epoch in minutes, tle is a pointer to a tle_t */
283 /* structure with Keplerian orbital elements and pos and vel */
284 /* are vector_t structures returning ECI satellite position and */
285 /* velocity. Use Convert_Sat_State() to convert to km and km/s. */
286 void
SDP4(sat_t * sat,double tsince)287 SDP4 (sat_t *sat, double tsince)
288 {
289 int i;
290
291
292 double
293 a,axn,ayn,aynl,beta,betal,capu,cos2u,cosepw,cosik,
294 cosnok,cosu,cosuk,ecose,elsq,epw,esine,pl,theta4,
295 rdot,rdotk,rfdot,rfdotk,rk,sin2u,sinepw,sinik,
296 sinnok,sinu,sinuk,tempe,templ,tsq,u,uk,ux,uy,uz,
297 vx,vy,vz,xinck,xl,xlt,xmam,xmdf,xmx,xmy,xnoddf,
298 xnodek,xll,a1,a3ovk2,ao,c2,coef,coef1,x1m5th,
299 xhdot1,del1,r,delo,eeta,eta,etasq,perige,
300 psisq,tsi,qoms24,s4,pinvsq,temp,tempa,temp1,
301 temp2,temp3,temp4,temp5,temp6;
302
303
304 /* Initialization */
305 if (~sat->flags & SDP4_INITIALIZED_FLAG) {
306
307 sat->flags |= SDP4_INITIALIZED_FLAG;
308
309 /* Recover original mean motion (xnodp) and */
310 /* semimajor axis (aodp) from input elements. */
311 a1 = pow (xke / sat->tle.xno, tothrd);
312 sat->deep_arg.cosio = cos (sat->tle.xincl);
313 sat->deep_arg.theta2 = sat->deep_arg.cosio * sat->deep_arg.cosio;
314 sat->sgps.x3thm1 = 3.0 * sat->deep_arg.theta2 - 1.0;
315 sat->deep_arg.eosq = sat->tle.eo * sat->tle.eo;
316 sat->deep_arg.betao2 = 1.0 - sat->deep_arg.eosq;
317 sat->deep_arg.betao = sqrt (sat->deep_arg.betao2);
318 del1 = 1.5 * ck2 * sat->sgps.x3thm1 /
319 (a1 * a1 * sat->deep_arg.betao * sat->deep_arg.betao2);
320 ao = a1 * (1.0 - del1 * (0.5 * tothrd + del1 * (1.0 + 134.0 / 81.0 * del1)));
321 delo = 1.5 * ck2 * sat->sgps.x3thm1 /
322 (ao * ao * sat->deep_arg.betao * sat->deep_arg.betao2);
323 sat->deep_arg.xnodp = sat->tle.xno / (1.0 + delo);
324 sat->deep_arg.aodp = ao / (1.0 - delo);
325
326 /* For perigee below 156 km, the values */
327 /* of s and qoms2t are altered. */
328 s4 = __s__;
329 qoms24 = qoms2t;
330 perige = (sat->deep_arg.aodp * (1.0 - sat->tle.eo) - ae) * xkmper;
331 if (perige < 156.0) {
332 if (perige <= 98.0)
333 s4 = 20.0;
334 else
335 s4 = perige - 78.0;
336 qoms24 = pow ((120.0 - s4) * ae / xkmper, 4);
337 s4 = s4 / xkmper + ae;
338 }
339 pinvsq = 1.0 / (sat->deep_arg.aodp * sat->deep_arg.aodp *
340 sat->deep_arg.betao2 * sat->deep_arg.betao2);
341 sat->deep_arg.sing = sin (sat->tle.omegao);
342 sat->deep_arg.cosg = cos (sat->tle.omegao);
343 tsi = 1.0 / (sat->deep_arg.aodp - s4);
344 eta = sat->deep_arg.aodp * sat->tle.eo * tsi;
345 etasq = eta * eta;
346 eeta = sat->tle.eo * eta;
347 psisq = fabs (1.0 - etasq);
348 coef = qoms24 * pow (tsi, 4);
349 coef1 = coef / pow (psisq, 3.5);
350 c2 = coef1 * sat->deep_arg.xnodp * (sat->deep_arg.aodp *
351 (1.0 + 1.5 * etasq + eeta *
352 (4.0 + etasq)) + 0.75 * ck2 * tsi / psisq *
353 sat->sgps.x3thm1 * (8.0 + 3.0 * etasq *
354 (8.0 + etasq)));
355 sat->sgps.c1 = sat->tle.bstar * c2;
356 sat->deep_arg.sinio = sin (sat->tle.xincl);
357 a3ovk2 = -xj3 / ck2 * pow (ae, 3);
358 sat->sgps.x1mth2 = 1.0 - sat->deep_arg.theta2;
359 sat->sgps.c4 = 2.0 * sat->deep_arg.xnodp * coef1 *
360 sat->deep_arg.aodp * sat->deep_arg.betao2 *
361 (eta * (2.0 + 0.5 * etasq) + sat->tle.eo *
362 (0.5 + 2.0 * etasq) - 2.0 * ck2 * tsi /
363 (sat->deep_arg.aodp * psisq) * (-3.0 * sat->sgps.x3thm1 *
364 (1.0 - 2.0 * eeta + etasq *
365 (1.5 - 0.5 * eeta)) +
366 0.75 * sat->sgps.x1mth2 *
367 (2.0 * etasq - eeta * (1.0 + etasq)) *
368 cos (2.0 * sat->tle.omegao)));
369 theta4 = sat->deep_arg.theta2 * sat->deep_arg.theta2;
370 temp1 = 3.0 * ck2 * pinvsq * sat->deep_arg.xnodp;
371 temp2 = temp1 * ck2 * pinvsq;
372 temp3 = 1.25 * ck4 * pinvsq * pinvsq * sat->deep_arg.xnodp;
373 sat->deep_arg.xmdot = sat->deep_arg.xnodp + 0.5 * temp1 * sat->deep_arg.betao *
374 sat->sgps.x3thm1 + 0.0625 * temp2 * sat->deep_arg.betao *
375 (13.0 - 78.0 * sat->deep_arg.theta2 + 137.0 * theta4);
376 x1m5th = 1.0 - 5.0 * sat->deep_arg.theta2;
377 sat->deep_arg.omgdot = -0.5 * temp1 * x1m5th + 0.0625 * temp2 *
378 (7.0 - 114.0 * sat->deep_arg.theta2 + 395.0 * theta4) +
379 temp3 * (3.0 - 36.0 * sat->deep_arg.theta2 + 49.0 * theta4);
380 xhdot1 = -temp1 * sat->deep_arg.cosio;
381 sat->deep_arg.xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * sat->deep_arg.theta2) +
382 2.0 * temp3 * (3.0 - 7.0 * sat->deep_arg.theta2)) *
383 sat->deep_arg.cosio;
384 sat->sgps.xnodcf = 3.5 * sat->deep_arg.betao2 * xhdot1 * sat->sgps.c1;
385 sat->sgps.t2cof = 1.5 * sat->sgps.c1;
386 sat->sgps.xlcof = 0.125 * a3ovk2 * sat->deep_arg.sinio *
387 (3.0 + 5.0 * sat->deep_arg.cosio) / (1.0 + sat->deep_arg.cosio);
388 sat->sgps.aycof = 0.25 * a3ovk2 * sat->deep_arg.sinio;
389 sat->sgps.x7thm1 = 7.0 * sat->deep_arg.theta2 - 1.0;
390
391 /* initialize Deep() */
392 Deep (dpinit, sat);
393 }; /*End of SDP4() initialization */
394
395 /* Update for secular gravity and atmospheric drag */
396 xmdf = sat->tle.xmo + sat->deep_arg.xmdot * tsince;
397 sat->deep_arg.omgadf = sat->tle.omegao + sat->deep_arg.omgdot * tsince;
398 xnoddf = sat->tle.xnodeo + sat->deep_arg.xnodot * tsince;
399 tsq = tsince * tsince;
400 sat->deep_arg.xnode = xnoddf + sat->sgps.xnodcf * tsq;
401 tempa = 1.0 - sat->sgps.c1 * tsince;
402 tempe = sat->tle.bstar * sat->sgps.c4 * tsince;
403 templ = sat->sgps.t2cof * tsq;
404 sat->deep_arg.xn = sat->deep_arg.xnodp;
405
406 /* Update for deep-space secular effects */
407 sat->deep_arg.xll = xmdf;
408 sat->deep_arg.t = tsince;
409
410 Deep (dpsec, sat);
411
412 xmdf = sat->deep_arg.xll;
413 a = pow (xke / sat->deep_arg.xn, tothrd) * tempa * tempa;
414 sat->deep_arg.em = sat->deep_arg.em - tempe;
415 xmam = xmdf + sat->deep_arg.xnodp * templ;
416
417 /* Update for deep-space periodic effects */
418 sat->deep_arg.xll = xmam;
419
420 Deep (dpper, sat);
421
422 xmam = sat->deep_arg.xll;
423 xl = xmam + sat->deep_arg.omgadf + sat->deep_arg.xnode;
424 beta = sqrt (1.0 - sat->deep_arg.em * sat->deep_arg.em);
425 sat->deep_arg.xn = xke / pow( a, 1.5);
426
427 /* Long period periodics */
428 axn = sat->deep_arg.em * cos (sat->deep_arg.omgadf);
429 temp = 1.0 / (a * beta * beta);
430 xll = temp * sat->sgps.xlcof * axn;
431 aynl = temp * sat->sgps.aycof;
432 xlt = xl + xll;
433 ayn = sat->deep_arg.em * sin (sat->deep_arg.omgadf) + aynl;
434
435 /* Solve Kepler's Equation */
436 capu = FMod2p (xlt - sat->deep_arg.xnode);
437 temp2 = capu;
438
439 i = 0;
440 do {
441 sinepw = sin (temp2);
442 cosepw = cos (temp2);
443 temp3 = axn * sinepw;
444 temp4 = ayn * cosepw;
445 temp5 = axn * cosepw;
446 temp6 = ayn * sinepw;
447 epw = (capu - temp4 + temp3 - temp2) / (1.0 - temp5 - temp6) + temp2;
448 if (fabs (epw-temp2) <= e6a)
449 break;
450 temp2 = epw;
451 }
452 while (i++ < 10);
453
454 /* Short period preliminary quantities */
455 ecose = temp5 + temp6;
456 esine = temp3 - temp4;
457 elsq = axn * axn + ayn * ayn;
458 temp = 1.0 - elsq;
459 pl = a * temp;
460 r = a * (1.0 - ecose);
461 temp1 = 1.0 / r;
462 rdot = xke * sqrt (a) * esine * temp1;
463 rfdot = xke * sqrt (pl) *temp1;
464 temp2 = a * temp1;
465 betal = sqrt (temp);
466 temp3 = 1.0 / (1.0 + betal);
467 cosu = temp2 * (cosepw - axn + ayn * esine * temp3);
468 sinu = temp2 * (sinepw - ayn - axn * esine * temp3);
469 u = AcTan (sinu, cosu);
470 sin2u = 2.0 * sinu * cosu;
471 cos2u = 2.0 * cosu * cosu - 1.0;
472 temp = 1.0 / pl;
473 temp1 = ck2 * temp;
474 temp2 = temp1 * temp;
475
476 /* Update for short periodics */
477 rk = r * (1.0 - 1.5 * temp2 * betal * sat->sgps.x3thm1) +
478 0.5 * temp1 * sat->sgps.x1mth2 * cos2u;
479 uk = u - 0.25 * temp2 * sat->sgps.x7thm1 * sin2u;
480 xnodek = sat->deep_arg.xnode + 1.5 * temp2 * sat->deep_arg.cosio * sin2u;
481 xinck = sat->deep_arg.xinc + 1.5 * temp2 *
482 sat->deep_arg.cosio * sat->deep_arg.sinio * cos2u;
483 rdotk = rdot - sat->deep_arg.xn * temp1 * sat->sgps.x1mth2 * sin2u;
484 rfdotk = rfdot + sat->deep_arg.xn * temp1 *
485 (sat->sgps.x1mth2 * cos2u + 1.5 * sat->sgps.x3thm1);
486
487 /* Orientation vectors */
488 sinuk = sin (uk);
489 cosuk = cos (uk);
490 sinik = sin (xinck);
491 cosik = cos (xinck);
492 sinnok = sin (xnodek);
493 cosnok = cos (xnodek);
494 xmx = -sinnok * cosik;
495 xmy = cosnok * cosik;
496 ux = xmx * sinuk + cosnok * cosuk;
497 uy = xmy * sinuk + sinnok * cosuk;
498 uz = sinik * sinuk;
499 vx = xmx * cosuk - cosnok * sinuk;
500 vy = xmy * cosuk - sinnok * sinuk;
501 vz = sinik*cosuk;
502
503 /* Position and velocity */
504 sat->pos.x = rk * ux;
505 sat->pos.y = rk * uy;
506 sat->pos.z = rk * uz;
507 sat->vel.x = rdotk * ux + rfdotk * vx;
508 sat->vel.y = rdotk * uy + rfdotk * vy;
509 sat->vel.z = rdotk * uz + rfdotk * vz;
510
511 /* Phase in rads */
512 sat->phase = xlt - sat->deep_arg.xnode - sat->deep_arg.omgadf + twopi;
513 if (sat->phase < 0.0)
514 sat->phase += twopi;
515 sat->phase = FMod2p (sat->phase);
516
517 sat->tle.omegao1 = sat->deep_arg.omgadf;
518 sat->tle.xincl1 = sat->deep_arg.xinc;
519 sat->tle.xnodeo1 = sat->deep_arg.xnode;
520 } /* SDP4 */
521
522 /*------------------------------------------------------------------*/
523
524 /* DEEP */
525 /* This function is used by SDP4 to add lunar and solar */
526 /* perturbation effects to deep-space orbit objects. */
527 void
Deep(int ientry,sat_t * sat)528 Deep (int ientry, sat_t *sat)
529 {
530
531 double
532 a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,ainv2,alfdp,aqnv,
533 sgh,sini2,sinis,sinok,sh,si,sil,day,betdp,dalf,
534 bfact,c,cc,cosis,cosok,cosq,ctem,f322,zx,zy,
535 dbet,dls,eoc,eq,f2,f220,f221,f3,f311,f321,xnoh,
536 f330,f441,f442,f522,f523,f542,f543,g200,g201,
537 g211,pgh,ph,s1,s2,s3,s4,s5,s6,s7,se,sel,ses,xls,
538 g300,g310,g322,g410,g422,g520,g521,g532,g533,gam,
539 sinq,sinzf,sis,sl,sll,sls,stem,temp,temp1,x1,x2,
540 x2li,x2omi,x3,x4,x5,x6,x7,x8,xl,xldot,xmao,xnddt,
541 xndot,xno2,xnodce,xnoi,xomi,xpidot,z1,z11,z12,z13,
542 z2,z21,z22,z23,z3,z31,z32,z33,ze,zf,zm,zn,
543 zsing,zsinh,zsini,zcosg,zcosh,zcosi,delt=0,ft=0;
544
545 switch (ientry) {
546 case dpinit : /* Entrance for deep space initialization */
547 sat->dps.thgr = ThetaG (sat->tle.epoch, &sat->deep_arg);
548 eq = sat->tle.eo;
549 sat->dps.xnq = sat->deep_arg.xnodp;
550 aqnv = 1.0 / sat->deep_arg.aodp;
551 sat->dps.xqncl = sat->tle.xincl;
552 xmao = sat->tle.xmo;
553 xpidot = sat->deep_arg.omgdot + sat->deep_arg.xnodot;
554 sinq = sin (sat->tle.xnodeo);
555 cosq = cos (sat->tle.xnodeo);
556 sat->dps.omegaq = sat->tle.omegao;
557 sat->dps.preep = 0;
558
559 /* Initialize lunar solar terms */
560 day = sat->deep_arg.ds50 + 18261.5; /*Days since 1900 Jan 0.5*/
561 if (day != sat->dps.preep) {
562 sat->dps.preep = day;
563 xnodce = 4.5236020 - 9.2422029E-4 * day;
564 stem = sin (xnodce);
565 ctem = cos (xnodce);
566 sat->dps.zcosil = 0.91375164 - 0.03568096 * ctem;
567 sat->dps.zsinil = sqrt (1.0 - sat->dps.zcosil * sat->dps.zcosil);
568 sat->dps.zsinhl = 0.089683511 * stem / sat->dps.zsinil;
569 sat->dps.zcoshl = sqrt (1.0 - sat->dps.zsinhl * sat->dps.zsinhl);
570 c = 4.7199672 + 0.22997150 * day;
571 gam = 5.8351514 + 0.0019443680 * day;
572 sat->dps.zmol = FMod2p (c - gam);
573 zx = 0.39785416 * stem / sat->dps.zsinil;
574 zy = sat->dps.zcoshl * ctem + 0.91744867 * sat->dps.zsinhl * stem;
575 zx = AcTan (zx,zy);
576 zx = gam + zx - xnodce;
577 sat->dps.zcosgl = cos (zx);
578 sat->dps.zsingl = sin (zx);
579 sat->dps.zmos = 6.2565837 + 0.017201977 * day;
580 sat->dps.zmos = FMod2p (sat->dps.zmos);
581 } /* End if(day != preep) */
582
583 /* Do solar terms */
584 sat->dps.savtsn = 1E20;
585 zcosg = zcosgs;
586 zsing = zsings;
587 zcosi = zcosis;
588 zsini = zsinis;
589 zcosh = cosq;
590 zsinh = sinq;
591 cc = c1ss;
592 zn = zns;
593 ze = zes;
594 xnoi = 1.0 / sat->dps.xnq;
595
596 /* Loop breaks when Solar terms are done a second */
597 /* time, after Lunar terms are initialized */
598 for(;;) {
599 /* Solar terms done again after Lunar terms are done */
600 a1 = zcosg * zcosh + zsing * zcosi * zsinh;
601 a3 = -zsing * zcosh + zcosg * zcosi * zsinh;
602 a7 = -zcosg * zsinh + zsing * zcosi * zcosh;
603 a8 = zsing * zsini;
604 a9 = zsing * zsinh + zcosg * zcosi * zcosh;
605 a10 = zcosg * zsini;
606 a2 = sat->deep_arg.cosio * a7 + sat->deep_arg.sinio * a8;
607 a4 = sat->deep_arg.cosio * a9 + sat->deep_arg.sinio * a10;
608 a5 = -sat->deep_arg.sinio * a7 + sat->deep_arg.cosio * a8;
609 a6 = -sat->deep_arg.sinio*a9+ sat->deep_arg.cosio*a10;
610 x1 = a1*sat->deep_arg.cosg+a2*sat->deep_arg.sing;
611 x2 = a3*sat->deep_arg.cosg+a4*sat->deep_arg.sing;
612 x3 = -a1*sat->deep_arg.sing+a2*sat->deep_arg.cosg;
613 x4 = -a3*sat->deep_arg.sing+a4*sat->deep_arg.cosg;
614 x5 = a5*sat->deep_arg.sing;
615 x6 = a6*sat->deep_arg.sing;
616 x7 = a5*sat->deep_arg.cosg;
617 x8 = a6*sat->deep_arg.cosg;
618 z31 = 12*x1*x1-3*x3*x3;
619 z32 = 24*x1*x2-6*x3*x4;
620 z33 = 12*x2*x2-3*x4*x4;
621 z1 = 3*(a1*a1+a2*a2)+z31*sat->deep_arg.eosq;
622 z2 = 6*(a1*a3+a2*a4)+z32*sat->deep_arg.eosq;
623 z3 = 3*(a3*a3+a4*a4)+z33*sat->deep_arg.eosq;
624 z11 = -6*a1*a5+sat->deep_arg.eosq*(-24*x1*x7-6*x3*x5);
625 z12 = -6*(a1*a6+a3*a5)+ sat->deep_arg.eosq*
626 (-24*(x2*x7+x1*x8)-6*(x3*x6+x4*x5));
627 z13 = -6*a3*a6+sat->deep_arg.eosq*(-24*x2*x8-6*x4*x6);
628 z21 = 6*a2*a5+sat->deep_arg.eosq*(24*x1*x5-6*x3*x7);
629 z22 = 6*(a4*a5+a2*a6)+ sat->deep_arg.eosq*
630 (24*(x2*x5+x1*x6)-6*(x4*x7+x3*x8));
631 z23 = 6*a4*a6+sat->deep_arg.eosq*(24*x2*x6-6*x4*x8);
632 z1 = z1+z1+sat->deep_arg.betao2*z31;
633 z2 = z2+z2+sat->deep_arg.betao2*z32;
634 z3 = z3+z3+sat->deep_arg.betao2*z33;
635 s3 = cc*xnoi;
636 s2 = -0.5*s3/sat->deep_arg.betao;
637 s4 = s3*sat->deep_arg.betao;
638 s1 = -15*eq*s4;
639 s5 = x1*x3+x2*x4;
640 s6 = x2*x3+x1*x4;
641 s7 = x2*x4-x1*x3;
642 se = s1*zn*s5;
643 si = s2*zn*(z11+z13);
644 sl = -zn*s3*(z1+z3-14-6*sat->deep_arg.eosq);
645 sgh = s4*zn*(z31+z33-6);
646 sh = -zn*s2*(z21+z23);
647 if (sat->dps.xqncl < 5.2359877E-2)
648 sh = 0;
649 sat->dps.ee2 = 2*s1*s6;
650 sat->dps.e3 = 2*s1*s7;
651 sat->dps.xi2 = 2*s2*z12;
652 sat->dps.xi3 = 2*s2*(z13-z11);
653 sat->dps.xl2 = -2*s3*z2;
654 sat->dps.xl3 = -2*s3*(z3-z1);
655 sat->dps.xl4 = -2*s3*(-21-9*sat->deep_arg.eosq)*ze;
656 sat->dps.xgh2 = 2*s4*z32;
657 sat->dps.xgh3 = 2*s4*(z33-z31);
658 sat->dps.xgh4 = -18*s4*ze;
659 sat->dps.xh2 = -2*s2*z22;
660 sat->dps.xh3 = -2*s2*(z23-z21);
661
662 if (sat->flags & LUNAR_TERMS_DONE_FLAG)
663 break;
664
665 /* Do lunar terms */
666 sat->dps.sse = se;
667 sat->dps.ssi = si;
668 sat->dps.ssl = sl;
669 sat->dps.ssh = sh/sat->deep_arg.sinio;
670 sat->dps.ssg = sgh-sat->deep_arg.cosio*sat->dps.ssh;
671 sat->dps.se2 = sat->dps.ee2;
672 sat->dps.si2 = sat->dps.xi2;
673 sat->dps.sl2 = sat->dps.xl2;
674 sat->dps.sgh2 = sat->dps.xgh2;
675 sat->dps.sh2 = sat->dps.xh2;
676 sat->dps.se3 = sat->dps.e3;
677 sat->dps.si3 = sat->dps.xi3;
678 sat->dps.sl3 = sat->dps.xl3;
679 sat->dps.sgh3 = sat->dps.xgh3;
680 sat->dps.sh3 = sat->dps.xh3;
681 sat->dps.sl4 = sat->dps.xl4;
682 sat->dps.sgh4 = sat->dps.xgh4;
683 zcosg = sat->dps.zcosgl;
684 zsing = sat->dps.zsingl;
685 zcosi = sat->dps.zcosil;
686 zsini = sat->dps.zsinil;
687 zcosh = sat->dps.zcoshl*cosq+sat->dps.zsinhl*sinq;
688 zsinh = sinq*sat->dps.zcoshl-cosq*sat->dps.zsinhl;
689 zn = znl;
690 cc = c1l;
691 ze = zel;
692 sat->flags |= LUNAR_TERMS_DONE_FLAG;
693 } /* End of for(;;) */
694
695 sat->dps.sse = sat->dps.sse+se;
696 sat->dps.ssi = sat->dps.ssi+si;
697 sat->dps.ssl = sat->dps.ssl+sl;
698 sat->dps.ssg = sat->dps.ssg+sgh-sat->deep_arg.cosio/sat->deep_arg.sinio*sh;
699 sat->dps.ssh = sat->dps.ssh+sh/sat->deep_arg.sinio;
700
701 /* Geopotential resonance initialization for 12 hour orbits */
702 sat->flags &= ~RESONANCE_FLAG;
703 sat->flags &= ~SYNCHRONOUS_FLAG;
704
705 if( !((sat->dps.xnq < 0.0052359877) && (sat->dps.xnq > 0.0034906585)) ) {
706 if( (sat->dps.xnq < 0.00826) || (sat->dps.xnq > 0.00924) )
707 return;
708 if (eq < 0.5)
709 return;
710 sat->flags |= RESONANCE_FLAG;
711 eoc = eq*sat->deep_arg.eosq;
712 g201 = -0.306-(eq-0.64)*0.440;
713 if (eq <= 0.65) {
714 g211 = 3.616-13.247*eq+16.290*sat->deep_arg.eosq;
715 g310 = -19.302+117.390*eq-228.419*
716 sat->deep_arg.eosq+156.591*eoc;
717 g322 = -18.9068+109.7927*eq-214.6334*
718 sat->deep_arg.eosq+146.5816*eoc;
719 g410 = -41.122+242.694*eq-471.094*
720 sat->deep_arg.eosq+313.953*eoc;
721 g422 = -146.407+841.880*eq-1629.014*
722 sat->deep_arg.eosq+1083.435*eoc;
723 g520 = -532.114+3017.977*eq-5740*
724 sat->deep_arg.eosq+3708.276*eoc;
725 }
726 else {
727 g211 = -72.099+331.819*eq-508.738*
728 sat->deep_arg.eosq+266.724*eoc;
729 g310 = -346.844+1582.851*eq-2415.925*
730 sat->deep_arg.eosq+1246.113*eoc;
731 g322 = -342.585+1554.908*eq-2366.899*
732 sat->deep_arg.eosq+1215.972*eoc;
733 g410 = -1052.797+4758.686*eq-7193.992*
734 sat->deep_arg.eosq+3651.957*eoc;
735 g422 = -3581.69+16178.11*eq-24462.77*
736 sat->deep_arg.eosq+ 12422.52*eoc;
737 if (eq <= 0.715)
738 g520 = 1464.74-4664.75*eq+3763.64*sat->deep_arg.eosq;
739 else
740 g520 = -5149.66+29936.92*eq-54087.36*
741 sat->deep_arg.eosq+31324.56*eoc;
742 } /* End if (eq <= 0.65) */
743
744 if (eq < 0.7) {
745 g533 = -919.2277+4988.61*eq-9064.77*
746 sat->deep_arg.eosq+5542.21*eoc;
747 g521 = -822.71072+4568.6173*eq-8491.4146*
748 sat->deep_arg.eosq+5337.524*eoc;
749 g532 = -853.666+4690.25*eq-8624.77*
750 sat->deep_arg.eosq+ 5341.4*eoc;
751 }
752 else {
753 g533 = -37995.78+161616.52*eq-229838.2*
754 sat->deep_arg.eosq+109377.94*eoc;
755 g521 = -51752.104+218913.95*eq-309468.16*
756 sat->deep_arg.eosq+146349.42*eoc;
757 g532 = -40023.88+170470.89*eq-242699.48*
758 sat->deep_arg.eosq+115605.82*eoc;
759 } /* End if (eq <= 0.7) */
760
761 sini2 = sat->deep_arg.sinio*sat->deep_arg.sinio;
762 f220 = 0.75*(1+2*sat->deep_arg.cosio+sat->deep_arg.theta2);
763 f221 = 1.5*sini2;
764 f321 = 1.875*sat->deep_arg.sinio*(1-2*\
765 sat->deep_arg.cosio-3*sat->deep_arg.theta2);
766 f322 = -1.875*sat->deep_arg.sinio*(1+2*
767 sat->deep_arg.cosio-3*sat->deep_arg.theta2);
768 f441 = 35*sini2*f220;
769 f442 = 39.3750*sini2*sini2;
770 f522 = 9.84375*sat->deep_arg.sinio*(sini2*(1-2*sat->deep_arg.cosio-5*
771 sat->deep_arg.theta2)+0.33333333*(-2+4*sat->deep_arg.cosio+
772 6*sat->deep_arg.theta2));
773 f523 = sat->deep_arg.sinio*(4.92187512*sini2*(-2-4*
774 sat->deep_arg.cosio+10*sat->deep_arg.theta2)+6.56250012
775 *(1+2*sat->deep_arg.cosio-3*sat->deep_arg.theta2));
776 f542 = 29.53125*sat->deep_arg.sinio*(2-8*
777 sat->deep_arg.cosio+sat->deep_arg.theta2*
778 (-12+8*sat->deep_arg.cosio+10*sat->deep_arg.theta2));
779 f543 = 29.53125*sat->deep_arg.sinio*(-2-8*sat->deep_arg.cosio+
780 sat->deep_arg.theta2*(12+8*sat->deep_arg.cosio-10*
781 sat->deep_arg.theta2));
782 xno2 = sat->dps.xnq*sat->dps.xnq;
783 ainv2 = aqnv*aqnv;
784 temp1 = 3*xno2*ainv2;
785 temp = temp1*root22;
786 sat->dps.d2201 = temp*f220*g201;
787 sat->dps.d2211 = temp*f221*g211;
788 temp1 = temp1*aqnv;
789 temp = temp1*root32;
790 sat->dps.d3210 = temp*f321*g310;
791 sat->dps.d3222 = temp*f322*g322;
792 temp1 = temp1*aqnv;
793 temp = 2*temp1*root44;
794 sat->dps.d4410 = temp*f441*g410;
795 sat->dps.d4422 = temp*f442*g422;
796 temp1 = temp1*aqnv;
797 temp = temp1*root52;
798 sat->dps.d5220 = temp*f522*g520;
799 sat->dps.d5232 = temp*f523*g532;
800 temp = 2*temp1*root54;
801 sat->dps.d5421 = temp*f542*g521;
802 sat->dps.d5433 = temp*f543*g533;
803 sat->dps.xlamo = xmao+sat->tle.xnodeo+sat->tle.xnodeo-sat->dps.thgr-sat->dps.thgr;
804 bfact = sat->deep_arg.xmdot+sat->deep_arg.xnodot+
805 sat->deep_arg.xnodot-thdt-thdt;
806 bfact = bfact+sat->dps.ssl+sat->dps.ssh+sat->dps.ssh;
807 } /* if( !(sat->dps.xnq < 0.0052359877) && (sat->dps.xnq > 0.0034906585) ) */
808 else {
809 sat->flags |= RESONANCE_FLAG;
810 sat->flags |= SYNCHRONOUS_FLAG;
811 /* Synchronous resonance terms initialization */
812 g200 = 1+sat->deep_arg.eosq*(-2.5+0.8125*sat->deep_arg.eosq);
813 g310 = 1+2*sat->deep_arg.eosq;
814 g300 = 1+sat->deep_arg.eosq*(-6+6.60937*sat->deep_arg.eosq);
815 f220 = 0.75*(1+sat->deep_arg.cosio)*(1+sat->deep_arg.cosio);
816 f311 = 0.9375*sat->deep_arg.sinio*sat->deep_arg.sinio*
817 (1+3*sat->deep_arg.cosio)-0.75*(1+sat->deep_arg.cosio);
818 f330 = 1+sat->deep_arg.cosio;
819 f330 = 1.875*f330*f330*f330;
820 sat->dps.del1 = 3*sat->dps.xnq*sat->dps.xnq*aqnv*aqnv;
821 sat->dps.del2 = 2*sat->dps.del1*f220*g200*q22;
822 sat->dps.del3 = 3*sat->dps.del1*f330*g300*q33*aqnv;
823 sat->dps.del1 = sat->dps.del1*f311*g310*q31*aqnv;
824 sat->dps.fasx2 = 0.13130908;
825 sat->dps.fasx4 = 2.8843198;
826 sat->dps.fasx6 = 0.37448087;
827 sat->dps.xlamo = xmao+sat->tle.xnodeo+sat->tle.omegao-sat->dps.thgr;
828 bfact = sat->deep_arg.xmdot+xpidot-thdt;
829 bfact = bfact+sat->dps.ssl+sat->dps.ssg+sat->dps.ssh;
830 } /* End if( !(xnq < 0.0052359877) && (xnq > 0.0034906585) ) */
831
832 sat->dps.xfact = bfact-sat->dps.xnq;
833
834 /* Initialize integrator */
835 sat->dps.xli = sat->dps.xlamo;
836 sat->dps.xni = sat->dps.xnq;
837 sat->dps.atime = 0;
838 sat->dps.stepp = 720;
839 sat->dps.stepn = -720;
840 sat->dps.step2 = 259200;
841 /* End case dpinit: */
842 return;
843
844 case dpsec: /* Entrance for deep space secular effects */
845 sat->deep_arg.xll = sat->deep_arg.xll+sat->dps.ssl*sat->deep_arg.t;
846 sat->deep_arg.omgadf = sat->deep_arg.omgadf+sat->dps.ssg*sat->deep_arg.t;
847 sat->deep_arg.xnode = sat->deep_arg.xnode+sat->dps.ssh*sat->deep_arg.t;
848 sat->deep_arg.em = sat->tle.eo+sat->dps.sse*sat->deep_arg.t;
849 sat->deep_arg.xinc = sat->tle.xincl+sat->dps.ssi*sat->deep_arg.t;
850 if (sat->deep_arg.xinc < 0) {
851 sat->deep_arg.xinc = -sat->deep_arg.xinc;
852 sat->deep_arg.xnode = sat->deep_arg.xnode + pi;
853 sat->deep_arg.omgadf = sat->deep_arg.omgadf-pi;
854 }
855 if( ~sat->flags & RESONANCE_FLAG ) return;
856
857 do {
858 if( (sat->dps.atime == 0) ||
859 ((sat->deep_arg.t >= 0) && (sat->dps.atime < 0)) ||
860 ((sat->deep_arg.t < 0) && (sat->dps.atime >= 0)) ) {
861 /* Epoch restart */
862 if( sat->deep_arg.t >= 0 )
863 delt = sat->dps.stepp;
864 else
865 delt = sat->dps.stepn;
866
867 sat->dps.atime = 0;
868 sat->dps.xni = sat->dps.xnq;
869 sat->dps.xli = sat->dps.xlamo;
870 }
871 else {
872 if( fabs(sat->deep_arg.t) >= fabs(sat->dps.atime) ) {
873 if ( sat->deep_arg.t > 0 )
874 delt = sat->dps.stepp;
875 else
876 delt = sat->dps.stepn;
877 }
878 }
879
880 do {
881 if ( fabs(sat->deep_arg.t-sat->dps.atime) >= sat->dps.stepp ) {
882 sat->flags |= DO_LOOP_FLAG;
883 sat->flags &= ~EPOCH_RESTART_FLAG;
884 }
885 else {
886 ft = sat->deep_arg.t-sat->dps.atime;
887 sat->flags &= ~DO_LOOP_FLAG;
888 }
889
890 if( fabs(sat->deep_arg.t) < fabs(sat->dps.atime) ) {
891 if (sat->deep_arg.t >= 0)
892 delt = sat->dps.stepn;
893 else
894 delt = sat->dps.stepp;
895 sat->flags |= (DO_LOOP_FLAG | EPOCH_RESTART_FLAG);
896 }
897
898 /* Dot terms calculated */
899 if (sat->flags & SYNCHRONOUS_FLAG) {
900 xndot = sat->dps.del1*sin(sat->dps.xli-sat->dps.fasx2)+sat->dps.del2*sin(2*(sat->dps.xli-sat->dps.fasx4))
901 +sat->dps.del3*sin(3*(sat->dps.xli-sat->dps.fasx6));
902 xnddt = sat->dps.del1*cos(sat->dps.xli-sat->dps.fasx2)+2*sat->dps.del2*cos(2*(sat->dps.xli-sat->dps.fasx4))
903 +3*sat->dps.del3*cos(3*(sat->dps.xli-sat->dps.fasx6));
904 }
905 else {
906 xomi = sat->dps.omegaq+sat->deep_arg.omgdot*sat->dps.atime;
907 x2omi = xomi+xomi;
908 x2li = sat->dps.xli+sat->dps.xli;
909 xndot = sat->dps.d2201*sin(x2omi+sat->dps.xli-g22)
910 +sat->dps.d2211*sin(sat->dps.xli-g22)
911 +sat->dps.d3210*sin(xomi+sat->dps.xli-g32)
912 +sat->dps.d3222*sin(-xomi+sat->dps.xli-g32)
913 +sat->dps.d4410*sin(x2omi+x2li-g44)
914 +sat->dps.d4422*sin(x2li-g44)
915 +sat->dps.d5220*sin(xomi+sat->dps.xli-g52)
916 +sat->dps.d5232*sin(-xomi+sat->dps.xli-g52)
917 +sat->dps.d5421*sin(xomi+x2li-g54)
918 +sat->dps.d5433*sin(-xomi+x2li-g54);
919 xnddt = sat->dps.d2201*cos(x2omi+sat->dps.xli-g22)
920 +sat->dps.d2211*cos(sat->dps.xli-g22)
921 +sat->dps.d3210*cos(xomi+sat->dps.xli-g32)
922 +sat->dps.d3222*cos(-xomi+sat->dps.xli-g32)
923 +sat->dps.d5220*cos(xomi+sat->dps.xli-g52)
924 +sat->dps.d5232*cos(-xomi+sat->dps.xli-g52)
925 +2*(sat->dps.d4410*cos(x2omi+x2li-g44)
926 +sat->dps.d4422*cos(x2li-g44)
927 +sat->dps.d5421*cos(xomi+x2li-g54)
928 +sat->dps.d5433*cos(-xomi+x2li-g54));
929 } /* End of if (isFlagSet(SYNCHRONOUS_FLAG)) */
930
931 xldot = sat->dps.xni+sat->dps.xfact;
932 xnddt = xnddt*xldot;
933
934 if(sat->flags & DO_LOOP_FLAG) {
935 sat->dps.xli = sat->dps.xli+xldot*delt+xndot*sat->dps.step2;
936 sat->dps.xni = sat->dps.xni+xndot*delt+xnddt*sat->dps.step2;
937 sat->dps.atime = sat->dps.atime+delt;
938 }
939 }
940 while ( (sat->flags & DO_LOOP_FLAG) &&
941 (~sat->flags & EPOCH_RESTART_FLAG));
942 }
943 while ((sat->flags & DO_LOOP_FLAG) && (sat->flags & EPOCH_RESTART_FLAG));
944
945 sat->deep_arg.xn = sat->dps.xni+xndot*ft+xnddt*ft*ft*0.5;
946 xl = sat->dps.xli+xldot*ft+xndot*ft*ft*0.5;
947 temp = -sat->deep_arg.xnode+sat->dps.thgr+sat->deep_arg.t*thdt;
948
949 if (~sat->flags & SYNCHRONOUS_FLAG)
950 sat->deep_arg.xll = xl+temp+temp;
951 else
952 sat->deep_arg.xll = xl-sat->deep_arg.omgadf+temp;
953
954 return;
955 /*End case dpsec: */
956
957 case dpper: /* Entrance for lunar-solar periodics */
958 sinis = sin(sat->deep_arg.xinc);
959 cosis = cos(sat->deep_arg.xinc);
960 if (fabs(sat->dps.savtsn-sat->deep_arg.t) >= 30) {
961 sat->dps.savtsn = sat->deep_arg.t;
962 zm = sat->dps.zmos+zns*sat->deep_arg.t;
963 zf = zm+2*zes*sin(zm);
964 sinzf = sin(zf);
965 f2 = 0.5*sinzf*sinzf-0.25;
966 f3 = -0.5*sinzf*cos(zf);
967 ses = sat->dps.se2*f2+sat->dps.se3*f3;
968 sis = sat->dps.si2*f2+sat->dps.si3*f3;
969 sls = sat->dps.sl2*f2+sat->dps.sl3*f3+sat->dps.sl4*sinzf;
970 sat->dps.sghs = sat->dps.sgh2*f2+sat->dps.sgh3*f3+sat->dps.sgh4*sinzf;
971 sat->dps.shs = sat->dps.sh2*f2+sat->dps.sh3*f3;
972 zm = sat->dps.zmol+znl*sat->deep_arg.t;
973 zf = zm+2*zel*sin(zm);
974 sinzf = sin(zf);
975 f2 = 0.5*sinzf*sinzf-0.25;
976 f3 = -0.5*sinzf*cos(zf);
977 sel = sat->dps.ee2*f2+sat->dps.e3*f3;
978 sil = sat->dps.xi2*f2+sat->dps.xi3*f3;
979 sll = sat->dps.xl2*f2+sat->dps.xl3*f3+sat->dps.xl4*sinzf;
980 sat->dps.sghl = sat->dps.xgh2*f2+sat->dps.xgh3*f3+sat->dps.xgh4*sinzf;
981 sat->dps.sh1 = sat->dps.xh2*f2+sat->dps.xh3*f3;
982 sat->dps.pe = ses+sel;
983 sat->dps.pinc = sis+sil;
984 sat->dps.pl = sls+sll;
985 }
986
987 pgh = sat->dps.sghs+sat->dps.sghl;
988 ph = sat->dps.shs+sat->dps.sh1;
989 sat->deep_arg.xinc = sat->deep_arg.xinc+sat->dps.pinc;
990 sat->deep_arg.em = sat->deep_arg.em+sat->dps.pe;
991
992 if (sat->dps.xqncl >= 0.2) {
993 /* Apply periodics directly */
994 ph = ph/sat->deep_arg.sinio;
995 pgh = pgh-sat->deep_arg.cosio*ph;
996 sat->deep_arg.omgadf = sat->deep_arg.omgadf+pgh;
997 sat->deep_arg.xnode = sat->deep_arg.xnode+ph;
998 sat->deep_arg.xll = sat->deep_arg.xll+sat->dps.pl;
999 }
1000 else {
1001 /* Apply periodics with Lyddane modification */
1002 sinok = sin(sat->deep_arg.xnode);
1003 cosok = cos(sat->deep_arg.xnode);
1004 alfdp = sinis*sinok;
1005 betdp = sinis*cosok;
1006 dalf = ph*cosok+sat->dps.pinc*cosis*sinok;
1007 dbet = -ph*sinok+sat->dps.pinc*cosis*cosok;
1008 alfdp = alfdp+dalf;
1009 betdp = betdp+dbet;
1010 sat->deep_arg.xnode = FMod2p(sat->deep_arg.xnode);
1011 xls = sat->deep_arg.xll+sat->deep_arg.omgadf+cosis*sat->deep_arg.xnode;
1012 dls = sat->dps.pl+pgh-sat->dps.pinc*sat->deep_arg.xnode*sinis;
1013 xls = xls+dls;
1014 xnoh = sat->deep_arg.xnode;
1015 sat->deep_arg.xnode = AcTan(alfdp,betdp);
1016
1017 /* This is a patch to Lyddane modification */
1018 /* suggested by Rob Matson. */
1019 if(fabs(xnoh-sat->deep_arg.xnode) > pi) {
1020 if(sat->deep_arg.xnode < xnoh)
1021 sat->deep_arg.xnode +=twopi;
1022 else
1023 sat->deep_arg.xnode -=twopi;
1024 }
1025
1026 sat->deep_arg.xll = sat->deep_arg.xll+sat->dps.pl;
1027 sat->deep_arg.omgadf = xls-sat->deep_arg.xll-cos(sat->deep_arg.xinc)*
1028 sat->deep_arg.xnode;
1029 } /* End case dpper: */
1030 return;
1031
1032 } /* End switch(ientry) */
1033
1034 } /* End of Deep() */
1035
1036 /*------------------------------------------------------------------*/
1037
1038 /* Functions for testing and setting/clearing flags */
1039
1040 /* An int variable holding the single-bit flags */
1041 static int Flags = 0;
1042
1043 int
isFlagSet(int flag)1044 isFlagSet(int flag)
1045 {
1046 return (Flags & flag);
1047 }
1048
1049 int
isFlagClear(int flag)1050 isFlagClear(int flag)
1051 {
1052 return (~Flags & flag);
1053 }
1054
1055 void
SetFlag(int flag)1056 SetFlag(int flag)
1057 {
1058 Flags |= flag;
1059 }
1060
1061 void
ClearFlag(int flag)1062 ClearFlag(int flag)
1063 {
1064 Flags &= ~flag;
1065 }
1066
1067 /*------------------------------------------------------------------*/
1068