Satellite Observers Workbench. NOT yet complete, just published for forum posters to \"cherry pick\" pieces of code as requiered as an example.
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?
User | Revision | Line number | New 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 | /*------------------------------------------------------------------*/ |