#include #include #include "sattypes.h" #include "vector.h" #include "satspec.h" #define XMO (sat->elem->se_XMO) #define XNODEO (sat->elem->se_XNODEO) #define OMEGAO (sat->elem->se_OMEGAO) #define EO (sat->elem->se_EO) #define XINCL (sat->elem->se_XINCL) #define XNO (sat->elem->se_XNO) #define XNDT20 (sat->elem->se_XNDT20) #define XNDD60 (sat->elem->se_XNDD60) #define BSTAR (sat->elem->se_BSTAR) #define EPOCH (sat->elem->se_EPOCH) #define X (pos->sl_X) #define XDOT (pos->sl_XDOT) #define Y (pos->sl_Y) #define YDOT (pos->sl_YDOT) #define Z (pos->sl_Z) #define ZDOT (pos->sl_ZDOT) #define AODP (sat->prop.sgp4->sgp4_AODP) #define AYCOF (sat->prop.sgp4->sgp4_AYCOF) #define C1 (sat->prop.sgp4->sgp4_C1) #define C4 (sat->prop.sgp4->sgp4_C4) #define C5 (sat->prop.sgp4->sgp4_C5) #define COSIO (sat->prop.sgp4->sgp4_COSIO) #define D2 (sat->prop.sgp4->sgp4_D2) #define D3 (sat->prop.sgp4->sgp4_D3) #define D4 (sat->prop.sgp4->sgp4_D4) #define DELMO (sat->prop.sgp4->sgp4_DELMO) #define ETA (sat->prop.sgp4->sgp4_ETA) #define OMGCOF (sat->prop.sgp4->sgp4_OMGCOF) #define OMGDOT (sat->prop.sgp4->sgp4_OMGDOT) #define SINIO (sat->prop.sgp4->sgp4_SINIO) #define SINMO (sat->prop.sgp4->sgp4_SINMO) #define T2COF (sat->prop.sgp4->sgp4_T2COF) #define T3COF (sat->prop.sgp4->sgp4_T3COF) #define T4COF (sat->prop.sgp4->sgp4_T4COF) #define T5COF (sat->prop.sgp4->sgp4_T5COF) #define X1MTH2 (sat->prop.sgp4->sgp4_X1MTH2) #define X3THM1 (sat->prop.sgp4->sgp4_X3THM1) #define X7THM1 (sat->prop.sgp4->sgp4_X7THM1) #define XLCOF (sat->prop.sgp4->sgp4_XLCOF) #define XMCOF (sat->prop.sgp4->sgp4_XMCOF) #define XMDOT (sat->prop.sgp4->sgp4_XMDOT) #define XNODCF (sat->prop.sgp4->sgp4_XNODCF) #define XNODOT (sat->prop.sgp4->sgp4_XNODOT) #define XNODP (sat->prop.sgp4->sgp4_XNODP) #define CK2 (5.413080e-04) #define CK4 (6.209887e-07) #define QOMS2T (1.880279e-09) #define S (1.012229e+00) #define AE (1.0) #define DE2RA (.174532925E-1) #define E6A (1.E-12) #define PI (3.14159265) #define PIO2 (1.57079633) #define QO (120.0) #define SO (78.0) #define TOTHRD (.66666667) #define TWOPI (6.2831853) #define X3PIO2 (4.71238898) #define XJ2 (1.082616E-3) #define XJ3 (-.253881E-5) #define XJ4 (-1.65597E-6) #define XKE (.743669161E-1) #define XKMPER (6378.135) #define XMNPDA (1440.0) /* compute position and velocity vectors for the satellite defined in sat->elem * at its epoch + TSINCE. */ void sgp4(SatData *sat, Vec3 *pos, Vec3 *dpos, double TSINCE) { int i; double A1, A3OVK2, AO, BETAO, BETAO2, C1SQ, C2, C3, COEF, COEF1, DEL1, DELO, EETA, EOSQ, ETASQ, PERIGE, PINVSQ, PSISQ, QOMS24, S4, TEMP, TEMP1, TEMP2, TEMP3=0, THETA2, THETA4, TSI, X1M5TH, XHDOT1; double A, AXN, AYN, AYNL, BETA, BETAL, CAPU, COS2U, COSEPW=0, COSIK, COSNOK, COSU, COSUK, DELM, DELOMG, E, ECOSE, ELSQ, EPW, ESINE, OMEGA, OMGADF, PL, R, RDOT, RDOTK, RFDOT, RFDOTK, RK, SIN2U, SINEPW=0, SINIK, SINNOK, SINU, SINUK, TCUBE, TEMP4=0, TEMP5=0, TEMP6=0, TEMPA, TEMPE, TEMPL, TFOUR, TSQ, U, UK, UX, UY, UZ, VX, VY, VZ, XINCK, XL, XLL, XLT, XMDF, XMP, XMX, XMY, XN, XNODDF, XNODE, XNODEK; #if 0 A1 = A3OVK2 = AO = BETAO = BETAO2 = C1SQ = C2 = C3 = COEF = COEF1 = DEL1 = DELO = EETA = EOSQ = ETASQ = PERIGE = PINVSQ = PSISQ = QOMS24 = S4 = TEMP = TEMP1 = TEMP2 = TEMP3 = THETA2 = THETA4 = TSI = X1M5TH = XHDOT1 = signaling_nan(); A = AXN = AYN = AYNL = BETA = BETAL = CAPU = COS2U = COSEPW = COSIK = COSNOK = COSU = COSUK = DELM = DELOMG = E = ECOSE = ELSQ = EPW = ESINE = OMEGA = OMGADF = PL = R = RDOT = RDOTK = RFDOT = RFDOTK = RK = SIN2U = SINEPW = SINIK = SINNOK = SINU = SINUK = TCUBE = TEMP4 = TEMP5 = TEMP6 = TEMPA = TEMPE = TEMPL = TFOUR = TSQ = U = UK = UX = UY = UZ = VX = VY = VZ = XINCK = XL = XLL = XLT = XMDF = XMP = XMX = XMY = XN = XNODDF = XNODE = XNODEK = signaling_nan(); #endif if(!sat->prop.sgp4) { sat->prop.sgp4 = (struct sgp4_data *) malloc(sizeof(struct sgp4_data)); /* * RECOVER ORIGINAL MEAN MOTION (XNODP) AND SEMIMAJOR AXIS (AODP) * FROM INPUT ELEMENTS */ A1 = pow((XKE/XNO), TOTHRD); COSIO = cos(XINCL); THETA2 = COSIO * COSIO; X3THM1 = 3.0 * THETA2 - 1.0; EOSQ = EO * EO; BETAO2 = 1.0 - EOSQ; BETAO = sqrt(BETAO2); DEL1 = 1.5 * CK2 * X3THM1 / (A1 * A1 * BETAO * BETAO2); AO = A1 * (1.0 - DEL1 * (.5 * TOTHRD + DEL1 * (1.0 + 134.0 /81.0 * DEL1))); DELO = 1.5 * CK2 * X3THM1 / (AO * AO * BETAO * BETAO2); XNODP = XNO / (1.0 + DELO); AODP=AO / (1.0 - DELO); /* * INITIALIZATION * * FOR PERIGEE LESS THAN 220 KILOMETERS, THE ISIMP FLAG IS SET AND * THE EQUATIONS ARE TRUNCATED TO LINEAR VARIATION IN SQRT A AND * QUADRATIC VARIATION IN MEAN ANOMALY. ALSO, THE C3 TERM, THE * DELTA OMEGA TERM, AND THE DELTA M TERM ARE DROPPED. */ sat->prop.sgp4->sgp4_flags = 0; /* IF((AODP*(1.-EO)/AE) .LT. (220./XKMPER+AE)) ISIMP=1 */ if((AODP * (1.0 - EO) / AE) < (220.0 / XKMPER + AE)) sat->prop.sgp4->sgp4_flags |= SGP4_SIMPLE; /* * FOR PERIGEE BELOW 156 KM, THE VALUES OF * S AND QOMS2T ARE ALTERED */ S4 = S; QOMS24 = QOMS2T; PERIGE = (AODP * (1.0 - EO) - AE) * XKMPER; if(PERIGE < 156.0) { S4 = PERIGE - 78.0; if(PERIGE <= 98.0) S4 = 20.0; QOMS24 = pow(((120.0 - S4) * AE / XKMPER), 4.0); S4 = S4 / XKMPER + AE; } PINVSQ=1.0 / (AODP * AODP * BETAO2 * BETAO2); TSI = 1.0 / (AODP - S4); ETA = AODP * EO * TSI; ETASQ = ETA * ETA; EETA = EO * ETA; PSISQ = fabs(1.0 - ETASQ); COEF = QOMS24 * pow(TSI, 4.0); COEF1 = COEF / pow(PSISQ, 3.5); C2 = COEF1 * XNODP * (AODP * (1.0 + 1.5 * ETASQ + EETA * (4.0 + ETASQ)) + .75 * CK2 * TSI / PSISQ * X3THM1 * (8.0 + 3.0 * ETASQ * (8.0 + ETASQ))); C1 = BSTAR * C2; SINIO = sin(XINCL); A3OVK2 = -XJ3 / CK2 * pow(AE, 3.0); C3 = COEF * TSI * A3OVK2 * XNODP * AE * SINIO / EO; X1MTH2 = 1.0 - THETA2; C4 = 2.0 * XNODP * COEF1 * AODP * BETAO2 * (ETA * (2.0 + .5 * ETASQ) + EO * (.5 + 2.0 * ETASQ) - 2.0 * CK2 * TSI / (AODP * PSISQ) * (-3.0 * X3THM1 * (1.0 - 2.0 * EETA + ETASQ * (1.5 - .5 * EETA)) + .75 * X1MTH2 * (2.0 * ETASQ - EETA * (1.0 + ETASQ)) * cos(2.0 * OMEGAO))); C5 = 2.0 * COEF1 * AODP * BETAO2 * (1.0 + 2.75 * (ETASQ + EETA) + EETA * ETASQ); THETA4 = THETA2 * THETA2; TEMP1 = 3.0 * CK2 * PINVSQ * XNODP; TEMP2 = TEMP1 * CK2 * PINVSQ; TEMP3 = 1.25 * CK4 * PINVSQ * PINVSQ * XNODP; XMDOT = XNODP + .5 * TEMP1 * BETAO * X3THM1 + .0625 * TEMP2 * BETAO * (13.0 - 78.0 * THETA2 + 137.0 * THETA4); X1M5TH = 1.0 - 5.0 * THETA2; OMGDOT = -.5 * TEMP1 * X1M5TH + .0625 * TEMP2 * (7.0 - 114.0 * THETA2 + 395.0 * THETA4) + TEMP3 * (3.0 - 36.0 * THETA2 + 49.0 * THETA4); XHDOT1 = -TEMP1 * COSIO; XNODOT = XHDOT1 + (.5 * TEMP2 * (4.0 - 19.0 * THETA2) + 2.0 * TEMP3 * (3.0 - 7.0 * THETA2)) * COSIO; OMGCOF = BSTAR * C3 * cos(OMEGAO); XMCOF = -TOTHRD * COEF * BSTAR * AE / EETA; XNODCF = 3.5 * BETAO2 * XHDOT1 * C1; T2COF = 1.5 * C1; XLCOF = .125 * A3OVK2 * SINIO * (3.0 + 5.0 *COSIO) / (1.0 + COSIO); AYCOF = .25 * A3OVK2 * SINIO; DELMO = pow(1.0 + ETA * cos(XMO), 3.0); SINMO = sin(XMO); X7THM1 = 7.0 * THETA2 - 1.0; /* IF(ISIMP .EQ. 1) GO TO 90 */ if(!(sat->prop.sgp4->sgp4_flags & SGP4_SIMPLE)) { C1SQ = C1 * C1; D2 = 4.0 * AODP * TSI * C1SQ; TEMP = D2 * TSI * C1 / 3.0; D3 = (17.0 * AODP + S4) * TEMP; D4 = .5 * TEMP * AODP * TSI * (221.0 * AODP + 31.0 * S4) * C1; T3COF = D2 + 2.0 * C1SQ; T4COF = .25 * (3.0 * D3 + C1 * (12.0 * D2 + 10.0 * C1SQ)); T5COF = .2 * (3.0 * D4 + 12.0 * C1 * D3 + 6.0 * D2 * D2 + 15.0 * C1SQ * (2.0 * D2 + C1SQ)); } } /* * UPDATE FOR SECULAR GRAVITY AND ATMOSPHERIC DRAG */ XMDF = XMO + XMDOT * TSINCE; OMGADF = OMEGAO + OMGDOT * TSINCE; XNODDF = XNODEO + XNODOT * TSINCE; OMEGA = OMGADF; XMP = XMDF; TSQ = TSINCE * TSINCE; XNODE = XNODDF + XNODCF * TSQ; TEMPA = 1.0 - C1 * TSINCE; TEMPE = BSTAR * C4 * TSINCE; TEMPL = T2COF * TSQ; if(!(sat->prop.sgp4->sgp4_flags & SGP4_SIMPLE)) { DELOMG = OMGCOF * TSINCE; DELM = XMCOF * (pow(1.0 + ETA * cos(XMDF), 3) - DELMO); TEMP = DELOMG + DELM; XMP = XMDF + TEMP; OMEGA = OMGADF - TEMP; TCUBE = TSQ * TSINCE; TFOUR = TSINCE * TCUBE; TEMPA = TEMPA - D2 * TSQ - D3 * TCUBE - D4 * TFOUR; TEMPE = TEMPE + BSTAR * C5 * (sin(XMP) - SINMO); TEMPL = TEMPL + T3COF * TCUBE + TFOUR * (T4COF + TSINCE * T5COF); } A = AODP * TEMPA * TEMPA; E = EO - TEMPE; XL = XMP + OMEGA + XNODE + XNODP * TEMPL; BETA = sqrt(1.0 - E * E); XN = XKE / pow(A, 1.5); /* * LONG PERIOD PERIODICS */ AXN = E * cos(OMEGA); TEMP = 1.0 / (A * BETA * BETA); XLL = TEMP * XLCOF * AXN; AYNL = TEMP * AYCOF; XLT = XL + XLL; AYN = E * sin(OMEGA) + AYNL; /* * SOLVE KEPLERS EQUATION */ CAPU = fmod(XLT - XNODE, TWOPI); TEMP2 = CAPU; for(i = 0; i < 10; i++) { SINEPW = sin(TEMP2); COSEPW = cos(TEMP2); TEMP3 = AXN * SINEPW; TEMP4 = AYN * COSEPW; TEMP5 = AXN * COSEPW; TEMP6 = AYN * SINEPW; EPW = (CAPU - TEMP4 + TEMP3 - TEMP2) / (1.0 - TEMP5 - TEMP6) + TEMP2; if(fabs(EPW - TEMP2) <= E6A) break; TEMP2 = EPW; } /* * SHORT PERIOD PRELIMINARY QUANTITIES */ ECOSE = TEMP5 + TEMP6; ESINE = TEMP3 - TEMP4; ELSQ = AXN * AXN + AYN * AYN; TEMP = 1.0 - ELSQ; PL = A * TEMP; R = A * (1.0 - ECOSE); TEMP1 = 1.0 / R; RDOT = XKE * sqrt(A) * ESINE * TEMP1; RFDOT = XKE * sqrt(PL) * TEMP1; TEMP2 = A * TEMP1; BETAL = sqrt(TEMP); TEMP3 = 1.0 / (1.0 + BETAL); COSU = TEMP2 * (COSEPW - AXN + AYN * ESINE * TEMP3); SINU = TEMP2 * (SINEPW - AYN - AXN * ESINE * TEMP3); U = actan(SINU, COSU); SIN2U = 2.0 * SINU * COSU; COS2U = 2.0 * COSU * COSU - 1.0; TEMP = 1.0 / PL; TEMP1 = CK2 * TEMP; TEMP2 = TEMP1 * TEMP; /* * UPDATE FOR SHORT PERIODICS */ RK = R * (1.0 - 1.5 * TEMP2 * BETAL * X3THM1) + .5 * TEMP1 * X1MTH2 * COS2U; UK = U - .25 * TEMP2 * X7THM1 * SIN2U; XNODEK = XNODE + 1.5 * TEMP2 * COSIO * SIN2U; XINCK = XINCL + 1.5 * TEMP2 * COSIO * SINIO * COS2U; RDOTK = RDOT - XN * TEMP1 * X1MTH2 * SIN2U; RFDOTK = RFDOT + XN * TEMP1 * (X1MTH2 * COS2U + 1.5 * X3THM1); /* * ORIENTATION VECTORS */ SINUK = sin(UK); COSUK = cos(UK); SINIK = sin(XINCK); COSIK = cos(XINCK); SINNOK = sin(XNODEK); COSNOK = cos(XNODEK); XMX = -SINNOK * COSIK; XMY = COSNOK * COSIK; UX = XMX * SINUK + COSNOK * COSUK; UY = XMY * SINUK + SINNOK * COSUK; UZ = SINIK * SINUK; VX = XMX * COSUK - COSNOK * SINUK; VY = XMY * COSUK - SINNOK * SINUK; VZ = SINIK * COSUK; /* * POSITION AND VELOCITY */ pos->x = RK * UX; pos->y = RK * UY; pos->z = RK * UZ; dpos->x = RDOTK * UX + RFDOTK * VX; dpos->y = RDOTK * UY + RFDOTK * VY; dpos->z = RDOTK * UZ + RFDOTK * VZ; }