Andy K / Mbed 2 deprecated SOWB

Satellite Observers Workbench. NOT yet complete, just published for forum posters to \"cherry pick\" pieces of code as requiered as an example.

Dependencies:   mbed

sgp4sdp4/sgp4sdp4.c@0:0a841b89d614, 2010-10-11 (annotated)

Committer:
AjK
Date:
Mon Oct 11 10:34:55 2010 +0000
Revision:
0:0a841b89d614
Totally Alpha quality as this project isn\t completed. Just publishing it as it answers many questions asked in the forums

Who changed what in which revision?

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