// // cNoradBase.cpp // // Historical Note: // The equations used here (and in derived classes) to determine satellite // ECI coordinates/velocity come from the December, 1980 NORAD document // "Space Track Report No. 3". The report details 6 orbital models and // provides FORTRAN IV implementations of each. The classes here // implement only two of the orbital models: SGP4 and SDP4. These two models, // one for "near-earth" objects and one for "deep space" objects, are widely // used in satellite tracking software and can produce very accurate results // when used with current NORAD two-line element datum. // // The NORAD FORTRAN IV SGP4/SDP4 implementations were converted to Pascal by // Dr. TS Kelso in 1995. In 1996 these routines were ported in a straight- // forward manner to C++ by Varol Okan. The SGP4/SDP4 classes here were // written by Michael F. Henry in 2002-03 and are a modern C++ re-write of // the work done by Okan. In addition to introducing an object-oriented // architecture, the last residues of the original FORTRAN code (such as // labels and gotos) were eradicated. // // For excellent information on the underlying physics of orbits, visible // satellite observations, current NORAD TLE data, and other related material, // see http://www.celestrak.com which is maintained by Dr. TS Kelso. // // Copyright (c) 2003 Michael F. Henry // // mfh 12/07/2003 // #include "stdafx.h" #include "cNoradBase.h" #include "cOrbit.h" #include "coord.h" #include "cEci.h" #include "cVector.h" #include "cJulian.h" ////////////////////////////////////////////////////////////////////////////// cNoradBase::cNoradBase(const cOrbit &orbit) : m_Orbit(orbit) { Initialize(); } cNoradBase::~cNoradBase(void) { } cNoradBase& cNoradBase::operator=(const cNoradBase &b) { // m_Orbit is a "const" member var, so cast away its // "const-ness" in order to complete the assigment. *(const_cast(&m_Orbit)) = b.m_Orbit; return *this; } ////////////////////////////////////////////////////////////////////////////// // Initialize() // Perform the initialization of member variables, specifically the variables // used by derived-class objects to calculate ECI coordinates. void cNoradBase::Initialize() { // Initialize any variables which are time-independent when // calculating the ECI coordinates of the satellite. m_satInc = m_Orbit.Inclination(); m_satEcc = m_Orbit.Eccentricity(); m_cosio = cos(m_satInc); m_theta2 = m_cosio * m_cosio; m_x3thm1 = 3.0 * m_theta2 - 1.0; m_eosq = m_satEcc * m_satEcc; m_betao2 = 1.0 - m_eosq; m_betao = sqrt(m_betao2); // The "recovered" semi-minor axis and mean motion. m_aodp = m_Orbit.SemiMinor(); m_xnodp = m_Orbit.mnMotionRec(); // For perigee below 156 km, the values of S and QOMS2T are altered. m_perigee = XKMPER_WGS72 * (m_aodp * (1.0 - m_satEcc) - AE); m_s4 = S; m_qoms24 = QOMS2T; if (m_perigee < 156.0) { m_s4 = m_perigee - 78.0; if (m_perigee <= 98.0) { m_s4 = 20.0; } m_qoms24 = pow((120.0 - m_s4) * AE / XKMPER_WGS72, 4.0); m_s4 = m_s4 / XKMPER_WGS72 + AE; } const double pinvsq = 1.0 / (m_aodp * m_aodp * m_betao2 * m_betao2); m_tsi = 1.0 / (m_aodp - m_s4); m_eta = m_aodp * m_satEcc * m_tsi; m_etasq = m_eta * m_eta; m_eeta = m_satEcc * m_eta; const double psisq = fabs(1.0 - m_etasq); m_coef = m_qoms24 * pow(m_tsi,4.0); m_coef1 = m_coef / pow(psisq,3.5); const double c2 = m_coef1 * m_xnodp * (m_aodp * (1.0 + 1.5 * m_etasq + m_eeta * (4.0 + m_etasq)) + 0.75 * CK2 * m_tsi / psisq * m_x3thm1 * (8.0 + 3.0 * m_etasq * (8.0 + m_etasq))); m_c1 = m_Orbit.BStar() * c2; m_sinio = sin(m_satInc); const double a3ovk2 = -XJ3 / CK2 * pow(AE,3.0); m_c3 = m_coef * m_tsi * a3ovk2 * m_xnodp * AE * m_sinio / m_satEcc; m_x1mth2 = 1.0 - m_theta2; m_c4 = 2.0 * m_xnodp * m_coef1 * m_aodp * m_betao2 * (m_eta * (2.0 + 0.5 * m_etasq) + m_satEcc * (0.5 + 2.0 * m_etasq) - 2.0 * CK2 * m_tsi / (m_aodp * psisq) * (-3.0 * m_x3thm1 * (1.0 - 2.0 * m_eeta + m_etasq * (1.5 - 0.5 * m_eeta)) + 0.75 * m_x1mth2 * (2.0 * m_etasq - m_eeta * (1.0 + m_etasq)) * cos(2.0 * m_Orbit.ArgPerigee()))); const double theta4 = m_theta2 * m_theta2; const double temp1 = 3.0 * CK2 * pinvsq * m_xnodp; const double temp2 = temp1 * CK2 * pinvsq; const double temp3 = 1.25 * CK4 * pinvsq * pinvsq * m_xnodp; m_xmdot = m_xnodp + 0.5 * temp1 * m_betao * m_x3thm1 + 0.0625 * temp2 * m_betao * (13.0 - 78.0 * m_theta2 + 137.0 * theta4); const double x1m5th = 1.0 - 5.0 * m_theta2; m_omgdot = -0.5 * temp1 * x1m5th + 0.0625 * temp2 * (7.0 - 114.0 * m_theta2 + 395.0 * theta4) + temp3 * (3.0 - 36.0 * m_theta2 + 49.0 * theta4); const double xhdot1 = -temp1 * m_cosio; m_xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * m_theta2) + 2.0 * temp3 * (3.0 - 7.0 * m_theta2)) * m_cosio; m_xnodcf = 3.5 * m_betao2 * xhdot1 * m_c1; m_t2cof = 1.5 * m_c1; m_xlcof = 0.125 * a3ovk2 * m_sinio * (3.0 + 5.0 * m_cosio) / (1.0 + m_cosio); m_aycof = 0.25 * a3ovk2 * m_sinio; m_x7thm1 = 7.0 * m_theta2 - 1.0; } ////////////////////////////////////////////////////////////////////////////// bool cNoradBase::FinalPosition(double incl, double omega, double e, double a, double xl, double xnode, double xn, double tsince, cEci &eci) { if ((e * e) > 1.0) { // error in satellite data return false; } double beta = sqrt(1.0 - e * e); // Long period periodics double axn = e * cos(omega); double temp = 1.0 / (a * beta * beta); double xll = temp * m_xlcof * axn; double aynl = temp * m_aycof; double xlt = xl + xll; double ayn = e * sin(omega) + aynl; // Solve Kepler's Equation double capu = Fmod2p(xlt - xnode); double temp2 = capu; double temp3 = 0.0; double temp4 = 0.0; double temp5 = 0.0; double temp6 = 0.0; double sinepw = 0.0; double cosepw = 0.0; bool fDone = false; for (int i = 1; (i <= 10) && !fDone; i++) { sinepw = sin(temp2); cosepw = cos(temp2); temp3 = axn * sinepw; temp4 = ayn * cosepw; temp5 = axn * cosepw; temp6 = ayn * sinepw; double epw = (capu - temp4 + temp3 - temp2) / (1.0 - temp5 - temp6) + temp2; if (fabs(epw - temp2) <= E6A) fDone = true; else temp2 = epw; } // Short period preliminary quantities double ecose = temp5 + temp6; double esine = temp3 - temp4; double elsq = axn * axn + ayn * ayn; temp = 1.0 - elsq; double pl = a * temp; double r = a * (1.0 - ecose); double temp1 = 1.0 / r; double rdot = XKE * sqrt(a) * esine * temp1; double rfdot = XKE * sqrt(pl) * temp1; temp2 = a * temp1; double betal = sqrt(temp); temp3 = 1.0 / (1.0 + betal); double cosu = temp2 * (cosepw - axn + ayn * esine * temp3); double sinu = temp2 * (sinepw - ayn - axn * esine * temp3); double u = AcTan(sinu, cosu); double sin2u = 2.0 * sinu * cosu; double cos2u = 2.0 * cosu * cosu - 1.0; temp = 1.0 / pl; temp1 = CK2 * temp; temp2 = temp1 * temp; // Update for short periodics double rk = r * (1.0 - 1.5 * temp2 * betal * m_x3thm1) + 0.5 * temp1 * m_x1mth2 * cos2u; double uk = u - 0.25 * temp2 * m_x7thm1 * sin2u; double xnodek = xnode + 1.5 * temp2 * m_cosio * sin2u; double xinck = incl + 1.5 * temp2 * m_cosio * m_sinio * cos2u; double rdotk = rdot - xn * temp1 * m_x1mth2 * sin2u; double rfdotk = rfdot + xn * temp1 * (m_x1mth2 * cos2u + 1.5 * m_x3thm1); // Orientation vectors double sinuk = sin(uk); double cosuk = cos(uk); double sinik = sin(xinck); double cosik = cos(xinck); double sinnok = sin(xnodek); double cosnok = cos(xnodek); double xmx = -sinnok * cosik; double xmy = cosnok * cosik; double ux = xmx * sinuk + cosnok * cosuk; double uy = xmy * sinuk + sinnok * cosuk; double uz = sinik * sinuk; double vx = xmx * cosuk - cosnok * sinuk; double vy = xmy * cosuk - sinnok * sinuk; double vz = sinik * cosuk; // Position double x = rk * ux; double y = rk * uy; double z = rk * uz; cVector vecPos(x, y, z); // Validate on altitude double altKm = (vecPos.Magnitude() * (XKMPER_WGS72 / AE)); if ((altKm < XKMPER_WGS72) || (altKm > (2 * GEOSYNC_ALT))) return false; // Velocity double xdot = rdotk * ux + rfdotk * vx; double ydot = rdotk * uy + rfdotk * vy; double zdot = rdotk * uz + rfdotk * vz; cVector vecVel(xdot, ydot, zdot); cJulian gmt = m_Orbit.Epoch(); gmt.addMin(tsince); eci = cEci(vecPos, vecVel, gmt); return true; }