// // globals.cpp // #include ////////////////////////////////////////////////////////////////////////////// double sqr(const double x) { return (x * x); } ////////////////////////////////////////////////////////////////////////////// double Fmod2p(const double arg) { double modu = fmod(arg, TWOPI); if (modu < 0.0) modu += TWOPI; return modu; } ////////////////////////////////////////////////////////////////////////////// // AcTan() // ArcTangent of sin(x) / cos(x). The advantage of this function over arctan() // is that it returns the correct quadrant of the angle. double AcTan(const double sinx, const double cosx) { double ret; if (cosx == 0.0) { if (sinx > 0.0) ret = PI / 2.0; else ret = 3.0 * PI / 2.0; } else { if (cosx > 0.0) ret = atan(sinx / cosx); else ret = PI + atan(sinx / cosx); } return ret; } ////////////////////////////////////////////////////////////////////////////// double rad2deg(const double r) { const double DEG_PER_RAD = 180.0 / PI; return r * DEG_PER_RAD; } ////////////////////////////////////////////////////////////////////////////// double deg2rad(const double d) { const double RAD_PER_DEG = PI / 180.0; return d * RAD_PER_DEG; } // // coord.cpp // // Copyright (c) 2003 Michael F. Henry // ////////////////////////////////////////////////////////////////////// // cCoordGeo Class ////////////////////////////////////////////////////////////////////// cCoordGeo::cCoordGeo() { m_Lat = 0.0; m_Lon = 0.0; m_Alt = 0.0; } ////////////////////////////////////////////////////////////////////// // cCoordTopo Class ////////////////////////////////////////////////////////////////////// cCoordTopo::cCoordTopo() { m_Az = 0.0; m_El = 0.0; m_Range = 0.0; m_RangeRate = 0.0; } // // cVector.cpp // // Copyright (c) 2001-2003 Michael F. Henry // //***************************************************************************** // Multiply each component in the vector by 'factor'. //***************************************************************************** void cVector::Mul(double factor) { m_x *= factor; m_y *= factor; m_z *= factor; m_w *= fabs(factor); } //***************************************************************************** // Subtract a vector from this one. //***************************************************************************** void cVector::Sub(const cVector& vec) { m_x -= vec.m_x; m_y -= vec.m_y; m_z -= vec.m_z; m_w -= vec.m_w; } //***************************************************************************** // Calculate the angle between this vector and another //***************************************************************************** double cVector::Angle(const cVector& vec) const { return acos(Dot(vec) / (Magnitude() * vec.Magnitude())); } //***************************************************************************** // //***************************************************************************** double cVector::Magnitude() const { return sqrt((m_x * m_x) + (m_y * m_y) + (m_z * m_z)); } //***************************************************************************** // Return the dot product //***************************************************************************** double cVector::Dot(const cVector& vec) const { return (m_x * vec.m_x) + (m_y * vec.m_y) + (m_z * vec.m_z); } // // cJulian.cpp // // This class encapsulates Julian dates with the epoch of 12:00 noon (12:00 UT) // on January 1, 4713 B.C. Some epoch dates: // 01/01/1990 00:00 UTC - 2447892.5 // 01/01/1990 12:00 UTC - 2447893.0 // 01/01/2000 00:00 UTC - 2451544.5 // 01/01/2001 00:00 UTC - 2451910.5 // // Note the Julian day begins at noon, which allows astronomers to have all // the dates in a single observing session the same. // // References: // "Astronomical Formulae for Calculators", Jean Meeus // "Satellite Communications", Dennis Roddy, 2nd Edition, 1995. // // Copyright (c) 2003 Michael F. Henry // // mfh 12/24/2003 // ////////////////////////////////////////////////////////////////////////////// // Create a Julian date object from a time_t object. time_t objects store the // number of seconds since midnight UTC January 1, 1970. cJulian::cJulian(time_t time) { struct tm *ptm = gmtime(&time); assert(ptm); int year = ptm->tm_year + 1900; double day = ptm->tm_yday + 1 + (ptm->tm_hour + ((ptm->tm_min + (ptm->tm_sec / 60.0)) / 60.0)) / 24.0; Initialize(year, day); } ////////////////////////////////////////////////////////////////////////////// // Create a Julian date object from a year and day of year. // Example parameters: year = 2001, day = 1.5 (Jan 1 12h) cJulian::cJulian(int year, double day) { Initialize(year, day); } ////////////////////////////////////////////////////////////////////////////// // Create a Julian date object. cJulian::cJulian(int year, // i.e., 2004 int mon, // 1..12 int day, // 1..31 int hour, // 0..23 int min, // 0..59 double sec /* = 0.0 */) // 0..(59.999999...) { // Calculate N, the day of the year (1..366) int N; int F1 = (int)((275.0 * mon) / 9.0); int F2 = (int)((mon + 9.0) / 12.0); if (IsLeapYear(year)) { // Leap year N = F1 - F2 + day - 30; } else { // Common year N = F1 - (2 * F2) + day - 30; } double dblDay = N + (hour + (min + (sec / 60.0)) / 60.0) / 24.0; Initialize(year, dblDay); } ////////////////////////////////////////////////////////////////////////////// void cJulian::Initialize(int year, double day) { // 1582 A.D.: 10 days removed from calendar // 3000 A.D.: Arbitrary error checking limit assert((year > 1582) && (year < 3000)); assert((day >= 0.0) && (day <= 366.5)); // Now calculate Julian date year--; // Centuries are not leap years unless they divide by 400 int A = (year / 100); int B = 2 - A + (A / 4); double NewYears = (int)(365.25 * year) + (int)(30.6001 * 14) + 1720994.5 + B; // 1720994.5 = Oct 30, year -1 m_Date = NewYears + day; } ////////////////////////////////////////////////////////////////////////////// // getComponent() // Return requested components of date. // Year : Includes the century. // Month: 1..12 // Day : 1..31 including fractional part void cJulian::getComponent(int *pYear, int *pMon /* = NULL */, double *pDOM /* = NULL */) const { assert(pYear != NULL); double jdAdj = getDate() + 0.5; int Z = (int)jdAdj; // integer part double F = jdAdj - Z; // fractional part double alpha = (int)((Z - 1867216.25) / 36524.25); double A = Z + 1 + alpha - (int)(alpha / 4.0); double B = A + 1524.0; int C = (int)((B - 122.1) / 365.25); int D = (int)(C * 365.25); int E = (int)((B - D) / 30.6001); double DOM = B - D - (int)(E * 30.6001) + F; int month = (E < 13.5) ? (E - 1) : (E - 13); int year = (month > 2.5) ? (C - 4716) : (C - 4715); *pYear = year; if (pMon != NULL) *pMon = month; if (pDOM != NULL) *pDOM = DOM; } ////////////////////////////////////////////////////////////////////////////// // toGMST() // Calculate Greenwich Mean Sidereal Time for the Julian date. The return value // is the angle, in radians, measuring eastward from the Vernal Equinox to the // prime meridian. This angle is also referred to as "ThetaG" (Theta GMST). // // References: // The 1992 Astronomical Almanac, page B6. // Explanatory Supplement to the Astronomical Almanac, page 50. // Orbital Coordinate Systems, Part III, Dr. T.S. Kelso, Satellite Times, // Nov/Dec 1995 double cJulian::toGMST() const { const double UT = fmod(m_Date + 0.5, 1.0); const double TU = (FromJan1_12h_2000() - UT) / 36525.0; double GMST = 24110.54841 + TU * (8640184.812866 + TU * (0.093104 - TU * 6.2e-06)); GMST = fmod(GMST + SEC_PER_DAY * OMEGA_E * UT, SEC_PER_DAY); if (GMST < 0.0) GMST += SEC_PER_DAY; // "wrap" negative modulo value return (TWOPI * (GMST / SEC_PER_DAY)); } ////////////////////////////////////////////////////////////////////////////// // toLMST() // Calculate Local Mean Sidereal Time for given longitude (for this date). // The longitude is assumed to be in radians measured west from Greenwich. // The return value is the angle, in radians, measuring eastward from the // Vernal Equinox to the given longitude. double cJulian::toLMST(double lon) const { return fmod(toGMST() + lon, TWOPI); } ////////////////////////////////////////////////////////////////////////////// // toTime() // Convert to type time_t // Avoid using this function as it discards the fractional seconds of the // time component. time_t cJulian::toTime() const { int nYear; int nMonth; double dblDay; getComponent(&nYear, &nMonth, &dblDay); // dblDay is the fractional Julian Day (i.e., 29.5577). // Save the whole number day in nDOM and convert dblDay to // the fractional portion of day. int nDOM = (int)dblDay; dblDay -= nDOM; const int SEC_PER_MIN = 60; const int SEC_PER_HR = 60 * SEC_PER_MIN; const int SEC_PER_DAY = 24 * SEC_PER_HR; int secs = (int)((dblDay * SEC_PER_DAY) + 0.5); // Create a "struct tm" type. // NOTE: // The "struct tm" type has a 1-second resolution. Any fractional // component of the "seconds" time value is discarded. struct tm tGMT; memset(&tGMT, 0, sizeof(tGMT)); tGMT.tm_year = nYear - 1900; // 2001 is 101 tGMT.tm_mon = nMonth - 1; // January is 0 tGMT.tm_mday = nDOM; // First day is 1 tGMT.tm_hour = secs / SEC_PER_HR; tGMT.tm_min = (secs % SEC_PER_HR) / SEC_PER_MIN; tGMT.tm_sec = (secs % SEC_PER_HR) % SEC_PER_MIN; tGMT.tm_isdst = 0; // No conversion desired time_t tEpoch = mktime(&tGMT); if (tEpoch != -1) { // Valid time_t value returned from mktime(). // mktime() expects a local time which means that tEpoch now needs // to be adjusted by the difference between this time zone and GMT. tEpoch -= timezone; } return tEpoch; } // // cTle.cpp // This class encapsulates a single set of standard NORAD two line elements. // // Copyright 1996-2005 Michael F. Henry // // Note: The column offsets are ZERO based. // Name const int TLE_LEN_LINE_DATA = 69; const int TLE_LEN_LINE_NAME = 22; // Line 1 const int TLE1_COL_SATNUM = 2; const int TLE1_LEN_SATNUM = 5; const int TLE1_COL_INTLDESC_A = 9; const int TLE1_LEN_INTLDESC_A = 2; const int TLE1_COL_INTLDESC_B = 11; const int TLE1_LEN_INTLDESC_B = 3; const int TLE1_COL_INTLDESC_C = 14; const int TLE1_LEN_INTLDESC_C = 3; const int TLE1_COL_EPOCH_A = 18; const int TLE1_LEN_EPOCH_A = 2; const int TLE1_COL_EPOCH_B = 20; const int TLE1_LEN_EPOCH_B = 12; const int TLE1_COL_MEANMOTIONDT = 33; const int TLE1_LEN_MEANMOTIONDT = 10; const int TLE1_COL_MEANMOTIONDT2 = 44; const int TLE1_LEN_MEANMOTIONDT2 = 8; const int TLE1_COL_BSTAR = 53; const int TLE1_LEN_BSTAR = 8; const int TLE1_COL_EPHEMTYPE = 62; const int TLE1_LEN_EPHEMTYPE = 1; const int TLE1_COL_ELNUM = 64; const int TLE1_LEN_ELNUM = 4; // Line 2 const int TLE2_COL_SATNUM = 2; const int TLE2_LEN_SATNUM = 5; const int TLE2_COL_INCLINATION = 8; const int TLE2_LEN_INCLINATION = 8; const int TLE2_COL_RAASCENDNODE = 17; const int TLE2_LEN_RAASCENDNODE = 8; const int TLE2_COL_ECCENTRICITY = 26; const int TLE2_LEN_ECCENTRICITY = 7; const int TLE2_COL_ARGPERIGEE = 34; const int TLE2_LEN_ARGPERIGEE = 8; const int TLE2_COL_MEANANOMALY = 43; const int TLE2_LEN_MEANANOMALY = 8; const int TLE2_COL_MEANMOTION = 52; const int TLE2_LEN_MEANMOTION = 11; const int TLE2_COL_REVATEPOCH = 63; const int TLE2_LEN_REVATEPOCH = 5; ///////////////////////////////////////////////////////////////////////////// cTle::cTle(string& strName, string& strLine1, string& strLine2) { m_strName = strName; m_strLine1 = strLine1; m_strLine2 = strLine2; Initialize(); } ///////////////////////////////////////////////////////////////////////////// cTle::cTle(const cTle &tle) { m_strName = tle.m_strName; m_strLine1 = tle.m_strLine1; m_strLine2 = tle.m_strLine2; for (int fld = FLD_FIRST; fld < FLD_LAST; fld++) { m_Field[fld] = tle.m_Field[fld]; } m_mapCache = tle.m_mapCache; } ///////////////////////////////////////////////////////////////////////////// cTle::~cTle() { } ///////////////////////////////////////////////////////////////////////////// // getField() // Return requested field as a double (function return value) or as a text // string (*pstr) in the units requested (eUnit). Set 'bStrUnits' to true // to have units appended to text string. // // Note: numeric return values are cached; asking for the same field more // than once incurs minimal overhead. double cTle::getField(eField fld, eUnits units, /* = U_NATIVE */ string *pstr /* = NULL */, bool bStrUnits /* = false */) const { assert((FLD_FIRST <= fld) && (fld < FLD_LAST)); assert((U_FIRST <= units) && (units < U_LAST)); if (pstr) { // Return requested field in string form. *pstr = m_Field[fld]; if (bStrUnits) *pstr += getUnits(fld); return 0.0; } else { // Return requested field in floating-point form. // Return cache contents if it exists, else populate cache FldKey key = Key(units, fld); if (m_mapCache.find(key) == m_mapCache.end()) { // Value not in cache; add it double valNative = atof(m_Field[fld].c_str()); double valConv = ConvertUnits(valNative, fld, units); m_mapCache[key] = valConv; return valConv; } else { // return cached value return m_mapCache[key]; } } } ////////////////////////////////////////////////////////////////////////////// // Convert the given field into the requested units. It is assumed that // the value being converted is in the TLE format's "native" form. double cTle::ConvertUnits(double valNative, // value to convert eField fld, // what field the value is eUnits units) // what units to convert to { switch (fld) { case FLD_I: case FLD_RAAN: case FLD_ARGPER: case FLD_M: { // The native TLE format is DEGREES if (units == U_RAD) return valNative * RADS_PER_DEG; } case FLD_NORADNUM: case FLD_INTLDESC: case FLD_SET: case FLD_EPOCHYEAR: case FLD_EPOCHDAY: case FLD_ORBITNUM: case FLD_E: case FLD_MMOTION: case FLD_MMOTIONDT: case FLD_MMOTIONDT2: case FLD_BSTAR: case FLD_LAST: { // do nothing } } return valNative; // return value in unconverted native format } ////////////////////////////////////////////////////////////////////////////// string cTle::getUnits(eField fld) const { static const string strDegrees = " degrees"; static const string strRevsPerDay = " revs / day"; static const string strNull; switch (fld) { case FLD_I: case FLD_RAAN: case FLD_ARGPER: case FLD_M: return strDegrees; case FLD_MMOTION: return strRevsPerDay; default: return strNull; } } ///////////////////////////////////////////////////////////////////////////// // ExpToDecimal() // Converts TLE-style exponential notation of the form [ |-]00000[+|-]0 to // decimal notation. Assumes implied decimal point to the left of the first // number in the string, i.e., // " 12345-3" = 0.00012345 // "-23429-5" = -0.0000023429 // " 40436+1" = 4.0436 string cTle::ExpToDecimal(const string &str) { const int COL_EXP_SIGN = 6; const int LEN_EXP = 2; const int LEN_BUFREAL = 32; // max length of buffer to hold floating point // representation of input string. int nMan; int nExp; // sscanf(%d) will read up to the exponent sign sscanf(str.c_str(), "%d", &nMan); double dblMan = nMan; bool bNeg = (nMan < 0); if (bNeg) dblMan *= -1; // Move decimal place to left of first digit while (dblMan >= 1.0) dblMan /= 10.0; if (bNeg) dblMan *= -1; // now read exponent sscanf(str.substr(COL_EXP_SIGN, LEN_EXP).c_str(), "%d", &nExp); double dblVal = dblMan * pow(10.0, nExp); char szVal[LEN_BUFREAL]; snprintf(szVal, sizeof(szVal), "%.9f", dblVal); string strVal = szVal; return strVal; } // ExpToDecimal() ///////////////////////////////////////////////////////////////////////////// // Initialize() // Initialize the string array. void cTle::Initialize() { // Have we already been initialized? if (m_Field[FLD_NORADNUM].size()) return; assert(!m_strName.empty()); assert(!m_strLine1.empty()); assert(!m_strLine2.empty()); m_Field[FLD_NORADNUM] = m_strLine1.substr(TLE1_COL_SATNUM, TLE1_LEN_SATNUM); m_Field[FLD_INTLDESC] = m_strLine1.substr(TLE1_COL_INTLDESC_A, TLE1_LEN_INTLDESC_A + TLE1_LEN_INTLDESC_B + TLE1_LEN_INTLDESC_C); m_Field[FLD_EPOCHYEAR] = m_strLine1.substr(TLE1_COL_EPOCH_A, TLE1_LEN_EPOCH_A); m_Field[FLD_EPOCHDAY] = m_strLine1.substr(TLE1_COL_EPOCH_B, TLE1_LEN_EPOCH_B); if (m_strLine1[TLE1_COL_MEANMOTIONDT] == '-') { // value is negative m_Field[FLD_MMOTIONDT] = "-0"; } else m_Field[FLD_MMOTIONDT] = "0"; m_Field[FLD_MMOTIONDT] += m_strLine1.substr(TLE1_COL_MEANMOTIONDT + 1, TLE1_LEN_MEANMOTIONDT); // decimal point assumed; exponential notation m_Field[FLD_MMOTIONDT2] = ExpToDecimal( m_strLine1.substr(TLE1_COL_MEANMOTIONDT2, TLE1_LEN_MEANMOTIONDT2)); // decimal point assumed; exponential notation m_Field[FLD_BSTAR] = ExpToDecimal(m_strLine1.substr(TLE1_COL_BSTAR, TLE1_LEN_BSTAR)); //TLE1_COL_EPHEMTYPE //TLE1_LEN_EPHEMTYPE m_Field[FLD_SET] = m_strLine1.substr(TLE1_COL_ELNUM, TLE1_LEN_ELNUM); TrimLeft(m_Field[FLD_SET]); //TLE2_COL_SATNUM //TLE2_LEN_SATNUM m_Field[FLD_I] = m_strLine2.substr(TLE2_COL_INCLINATION, TLE2_LEN_INCLINATION); TrimLeft(m_Field[FLD_I]); m_Field[FLD_RAAN] = m_strLine2.substr(TLE2_COL_RAASCENDNODE, TLE2_LEN_RAASCENDNODE); TrimLeft(m_Field[FLD_RAAN]); // decimal point is assumed m_Field[FLD_E] = "0."; m_Field[FLD_E] += m_strLine2.substr(TLE2_COL_ECCENTRICITY, TLE2_LEN_ECCENTRICITY); m_Field[FLD_ARGPER] = m_strLine2.substr(TLE2_COL_ARGPERIGEE, TLE2_LEN_ARGPERIGEE); TrimLeft(m_Field[FLD_ARGPER]); m_Field[FLD_M] = m_strLine2.substr(TLE2_COL_MEANANOMALY, TLE2_LEN_MEANANOMALY); TrimLeft(m_Field[FLD_M]); m_Field[FLD_MMOTION] = m_strLine2.substr(TLE2_COL_MEANMOTION, TLE2_LEN_MEANMOTION); TrimLeft(m_Field[FLD_MMOTION]); m_Field[FLD_ORBITNUM] = m_strLine2.substr(TLE2_COL_REVATEPOCH, TLE2_LEN_REVATEPOCH); TrimLeft(m_Field[FLD_ORBITNUM]); } // InitStrVars() ///////////////////////////////////////////////////////////////////////////// // IsTleFormat() // Returns true if "str" is a valid data line of a two-line element set, // else false. // // To be valid a line must: // Have as the first character the line number // Have as the second character a blank // Be TLE_LEN_LINE_DATA characters long // Have a valid checksum (note: no longer required as of 12/96) // bool cTle::IsValidLine(string& str, eTleLine line) { TrimLeft(str); TrimRight(str); size_t nLen = str.size(); if (nLen != (uint)TLE_LEN_LINE_DATA) return false; // First char in string must be line number if ((str[0] - '0') != line) return false; // Second char in string must be blank if (str[1] != ' ') return false; /* NOTE: 12/96 The requirement that the last char in the line data must be a valid checksum is too restrictive. // Last char in string must be checksum int nSum = CheckSum(str); if (nSum != (str[TLE_LEN_LINE_DATA - 1] - '0')) return false; */ return true; } // IsTleFormat() ///////////////////////////////////////////////////////////////////////////// // CheckSum() // Calculate the check sum for a given line of TLE data, the last character // of which is the current checksum. (Although there is no check here, // the current checksum should match the one we calculate.) // The checksum algorithm: // Each number in the data line is summed, modulo 10. // Non-numeric characters are zero, except minus signs, which are 1. // int cTle::CheckSum(const string& str) { // The length is "- 1" because we don't include the current (existing) // checksum character in the checksum calculation. size_t len = str.size() - 1; int xsum = 0; for (size_t i = 0; i < len; i++) { char ch = str[i]; if (isdigit(ch)) xsum += (ch - '0'); else if (ch == '-') xsum++; } return (xsum % 10); } // CheckSum() ///////////////////////////////////////////////////////////////////////////// void cTle::TrimLeft(string& s) { while (s[0] == ' ') s.erase(0, 1); } ///////////////////////////////////////////////////////////////////////////// void cTle::TrimRight(string& s) { while (s[s.size() - 1] == ' ') s.erase(s.size() - 1); } // // cEci.cpp // // Copyright (c) 2002-2003 Michael F. Henry // ////////////////////////////////////////////////////////////////////// // cEci Class ////////////////////////////////////////////////////////////////////// cEci::cEci(const cVector &pos, const cVector &vel, const cJulian &date, bool IsAeUnits /* = true */) { m_pos = pos; m_vel = vel; m_date = date; m_VecUnits = (IsAeUnits ? UNITS_AE : UNITS_NONE); } ////////////////////////////////////////////////////////////////////// // cEci(cCoordGeo&, cJulian&) // Calculate the ECI coordinates of the location "geo" at time "date". // Assumes geo coordinates are km-based. // Assumes the earth is an oblate spheroid as defined in WGS '72. // Reference: The 1992 Astronomical Almanac, page K11 // Reference: www.celestrak.com (Dr. TS Kelso) cEci::cEci(const cCoordGeo &geo, const cJulian &date) { m_VecUnits = UNITS_KM; double mfactor = TWOPI * (OMEGA_E / SEC_PER_DAY); double lat = geo.m_Lat; double lon = geo.m_Lon; double alt = geo.m_Alt; // Calculate Local Mean Sidereal Time (theta) double theta = date.toLMST(lon); double c = 1.0 / sqrt(1.0 + F * (F - 2.0) * sqr(sin(lat))); double s = sqr(1.0 - F) * c; double achcp = (XKMPER_WGS72 * c + alt) * cos(lat); m_date = date; m_pos.m_x = achcp * cos(theta); // km m_pos.m_y = achcp * sin(theta); // km m_pos.m_z = (XKMPER_WGS72 * s + alt) * sin(lat); // km m_pos.m_w = sqrt(sqr(m_pos.m_x) + sqr(m_pos.m_y) + sqr(m_pos.m_z)); // range, km m_vel.m_x = -mfactor * m_pos.m_y; // km / sec m_vel.m_y = mfactor * m_pos.m_x; m_vel.m_z = 0.0; m_vel.m_w = sqrt(sqr(m_vel.m_x) + // range rate km/sec^2 sqr(m_vel.m_y)); } ////////////////////////////////////////////////////////////////////////////// // toGeo() // Return the corresponding geodetic position (based on the current ECI // coordinates/Julian date). // Assumes the earth is an oblate spheroid as defined in WGS '72. // Side effects: Converts the position and velocity vectors to km-based units. // Reference: The 1992 Astronomical Almanac, page K12. // Reference: www.celestrak.com (Dr. TS Kelso) cCoordGeo cEci::toGeo() { ae2km(); // Vectors must be in kilometer-based units double theta = AcTan(m_pos.m_y, m_pos.m_x); double lon = fmod(theta - m_date.toGMST(), TWOPI); if (lon < 0.0) lon += TWOPI; // "wrap" negative modulo double r = sqrt(sqr(m_pos.m_x) + sqr(m_pos.m_y)); double e2 = F * (2.0 - F); double lat = AcTan(m_pos.m_z, r); const double delta = 1.0e-07; double phi; double c; do { phi = lat; c = 1.0 / sqrt(1.0 - e2 * sqr(sin(phi))); lat = AcTan(m_pos.m_z + XKMPER_WGS72 * c * e2 * sin(phi), r); } while (fabs(lat - phi) > delta); double alt = r / cos(lat) - XKMPER_WGS72 * c; return cCoordGeo(lat, lon, alt); // radians, radians, kilometers } ////////////////////////////////////////////////////////////////////////////// // ae2km() // Convert the position and velocity vector units from AE-based units // to kilometer based units. void cEci::ae2km() { if (UnitsAreAe()) { MulPos(XKMPER_WGS72 / AE); // km MulVel((XKMPER_WGS72 / AE) * (MIN_PER_DAY / 86400)); // km/sec m_VecUnits = UNITS_KM; } } // // 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 // ////////////////////////////////////////////////////////////////////////////// cNoradBase::cNoradBase(const cOrbit &orbit) : m_Orbit(orbit) { Initialize(); } 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; } // // cNoradSGP4.cpp // // NORAD SGP4 implementation. See historical note in cNoradBase.cpp // Copyright (c) 2003 Michael F. Henry // // mfh 12/07/2003 // ////////////////////////////////////////////////////////////////////////////// cNoradSGP4::cNoradSGP4(const cOrbit &orbit) : cNoradBase(orbit) { m_c5 = 2.0 * m_coef1 * m_aodp * m_betao2 * (1.0 + 2.75 * (m_etasq + m_eeta) + m_eeta * m_etasq); m_omgcof = m_Orbit.BStar() * m_c3 * cos(m_Orbit.ArgPerigee()); m_xmcof = -TWOTHRD * m_coef * m_Orbit.BStar() * AE / m_eeta; m_delmo = pow(1.0 + m_eta * cos(m_Orbit.mnAnomaly()), 3.0); m_sinmo = sin(m_Orbit.mnAnomaly()); } ////////////////////////////////////////////////////////////////////////////// // getPosition() // This procedure returns the ECI position and velocity for the satellite // in the orbit at the given number of minutes since the TLE epoch time // using the NORAD Simplified General Perturbation 4, near earth orbit // model. // // tsince - Time in minutes since the TLE epoch (GMT). // eci - ECI object to hold position information. // To convert the returned ECI position vector to km, // multiply each component by: // (XKMPER_WGS72 / AE). // To convert the returned ECI velocity vector to km/sec, // multiply each component by: // (XKMPER_WGS72 / AE) * (MIN_PER_DAY / 86400). bool cNoradSGP4::getPosition(double tsince, cEci &eci) { // For m_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 m_c3 term, the // delta omega term, and the delta m term are dropped. bool isimp = false; if ((m_aodp * (1.0 - m_satEcc) / AE) < (220.0 / XKMPER_WGS72 + AE)) { isimp = true; } double d2 = 0.0; double d3 = 0.0; double d4 = 0.0; double t3cof = 0.0; double t4cof = 0.0; double t5cof = 0.0; if (!isimp) { double c1sq = m_c1 * m_c1; d2 = 4.0 * m_aodp * m_tsi * c1sq; double temp = d2 * m_tsi * m_c1 / 3.0; d3 = (17.0 * m_aodp + m_s4) * temp; d4 = 0.5 * temp * m_aodp * m_tsi * (221.0 * m_aodp + 31.0 * m_s4) * m_c1; t3cof = d2 + 2.0 * c1sq; t4cof = 0.25 * (3.0 * d3 + m_c1 * (12.0 * d2 + 10.0 * c1sq)); t5cof = 0.2 * (3.0 * d4 + 12.0 * m_c1 * d3 + 6.0 * d2 * d2 + 15.0 * c1sq * (2.0 * d2 + c1sq)); } // Update for secular gravity and atmospheric drag. double xmdf = m_Orbit.mnAnomaly() + m_xmdot * tsince; double omgadf = m_Orbit.ArgPerigee() + m_omgdot * tsince; double xnoddf = m_Orbit.RAAN() + m_xnodot * tsince; double omega = omgadf; double xmp = xmdf; double tsq = tsince * tsince; double xnode = xnoddf + m_xnodcf * tsq; double tempa = 1.0 - m_c1 * tsince; double tempe = m_Orbit.BStar() * m_c4 * tsince; double templ = m_t2cof * tsq; if (!isimp) { double delomg = m_omgcof * tsince; double delm = m_xmcof * (pow(1.0 + m_eta * cos(xmdf), 3.0) - m_delmo); double temp = delomg + delm; xmp = xmdf + temp; omega = omgadf - temp; double tcube = tsq * tsince; double tfour = tsince * tcube; tempa = tempa - d2 * tsq - d3 * tcube - d4 * tfour; tempe = tempe + m_Orbit.BStar() * m_c5 * (sin(xmp) - m_sinmo); templ = templ + t3cof * tcube + tfour * (t4cof + tsince * t5cof); } double a = m_aodp * sqr(tempa); double e = m_satEcc - tempe; double xl = xmp + omega + xnode + m_xnodp * templ; double xn = XKE / pow(a, 1.5); return FinalPosition(m_satInc, omgadf, e, a, xl, xnode, xn, tsince, eci); } // // cNoradSDP4.cpp // // NORAD SDP4 implementation. See historical note in cNoradBase.cpp // Copyright (c) 2003 Michael F. Henry // // mfh 12/07/2003 // const double zns = 1.19459E-5; const double c1ss = 2.9864797E-6; const double zes = 0.01675; const double znl = 1.5835218E-4; const double c1l = 4.7968065E-7; const double zel = 0.05490; const double zcosis = 0.91744867; const double zsinis = 0.39785416; const double zsings = -0.98088458; const double zcosgs = 0.1945905; const double q22 = 1.7891679E-6; const double q31 = 2.1460748E-6; const double q33 = 2.2123015E-7; const double g22 = 5.7686396; const double g32 = 0.95240898; const double g44 = 1.8014998; const double g52 = 1.0508330; const double g54 = 4.4108898; const double root22 = 1.7891679E-6; const double root32 = 3.7393792E-7; const double root44 = 7.3636953E-9; const double root52 = 1.1428639E-7; const double root54 = 2.1765803E-9; const double thdt = 4.3752691E-3; ////////////////////////////////////////////////////////////////////////////// cNoradSDP4::cNoradSDP4(const cOrbit &orbit) : cNoradBase(orbit) { m_sing = sin(m_Orbit.ArgPerigee()); m_cosg = cos(m_Orbit.ArgPerigee()); dp_savtsn = 0.0; dp_zmos = 0.0; dp_se2 = 0.0; dp_se3 = 0.0; dp_si2 = 0.0; dp_si3 = 0.0; dp_sl2 = 0.0; dp_sl3 = 0.0; dp_sl4 = 0.0; dp_sghs = 0.0; dp_sgh2 = 0.0; dp_sgh3 = 0.0; dp_sgh4 = 0.0; dp_sh2 = 0.0; dp_sh3 = 0.0; dp_zmol = 0.0; dp_ee2 = 0.0; dp_e3 = 0.0; dp_xi2 = 0.0; dp_xi3 = 0.0; dp_xl2 = 0.0; dp_xl3 = 0.0; dp_xl4 = 0.0; dp_xgh2 = 0.0; dp_xgh3 = 0.0; dp_xgh4 = 0.0; dp_xh2 = 0.0; dp_xh3 = 0.0; dp_xqncl = 0.0; dp_thgr = 0.0; dp_omegaq = 0.0; dp_sse = 0.0; dp_ssi = 0.0; dp_ssl = 0.0; dp_ssh = 0.0; dp_ssg = 0.0; dp_d2201 = 0.0; dp_d2211 = 0.0; dp_d3210 = 0.0; dp_d3222 = 0.0; dp_d4410 = 0.0; dp_d4422 = 0.0; dp_d5220 = 0.0; dp_d5232 = 0.0; dp_d5421 = 0.0; dp_d5433 = 0.0; dp_xlamo = 0.0; dp_del1 = 0.0; dp_del2 = 0.0; dp_del3 = 0.0; dp_fasx2 = 0.0; dp_fasx4 = 0.0; dp_fasx6 = 0.0; dp_xfact = 0.0; dp_xli = 0.0; dp_xni = 0.0; dp_atime = 0.0; dp_stepp = 0.0; dp_stepn = 0.0; dp_step2 = 0.0; dp_iresfl = false; dp_isynfl = false; } ///////////////////////////////////////////////////////////////////////////// bool cNoradSDP4::DeepInit(double *eosq, double *sinio, double *cosio, double *betao, double *aodp, double *theta2, double *sing, double *cosg, double *betao2, double *xmdot, double *omgdot, double *xnodott) { eqsq = *eosq; siniq = *sinio; cosiq = *cosio; rteqsq = *betao; ao = *aodp; cosq2 = *theta2; sinomo = *sing; cosomo = *cosg; bsq = *betao2; xlldot = *xmdot; omgdt = *omgdot; xnodot = *xnodott; // Deep space initialization cJulian jd = m_Orbit.Epoch(); dp_thgr = jd.toGMST(); double eq = m_Orbit.Eccentricity(); double aqnv = 1.0 / ao; dp_xqncl = m_Orbit.Inclination(); double xmao = m_Orbit.mnAnomaly(); double xpidot = omgdt + xnodot; double sinq = sin(m_Orbit.RAAN()); double cosq = cos(m_Orbit.RAAN()); dp_omegaq = m_Orbit.ArgPerigee(); // Initialize lunar solar terms double day = jd.FromJan1_12h_1900(); if (day != dpi_day) { dpi_day = day; dpi_xnodce = 4.5236020 - 9.2422029E-4 * day; dpi_stem = sin(dpi_xnodce); dpi_ctem = cos(dpi_xnodce); dpi_zcosil = 0.91375164 - 0.03568096 * dpi_ctem; dpi_zsinil = sqrt(1.0 - dpi_zcosil * dpi_zcosil); dpi_zsinhl = 0.089683511 *dpi_stem / dpi_zsinil; dpi_zcoshl = sqrt(1.0 - dpi_zsinhl * dpi_zsinhl); dpi_c = 4.7199672 + 0.22997150 * day; dpi_gam = 5.8351514 + 0.0019443680 * day; dp_zmol = Fmod2p(dpi_c - dpi_gam); dpi_zx = 0.39785416 * dpi_stem / dpi_zsinil; dpi_zy = dpi_zcoshl * dpi_ctem + 0.91744867 * dpi_zsinhl * dpi_stem; dpi_zx = AcTan(dpi_zx,dpi_zy) + dpi_gam - dpi_xnodce; dpi_zcosgl = cos(dpi_zx); dpi_zsingl = sin(dpi_zx); dp_zmos = 6.2565837 + 0.017201977 * day; dp_zmos = Fmod2p(dp_zmos); } dp_savtsn = 1.0e20; double zcosg = zcosgs; double zsing = zsings; double zcosi = zcosis; double zsini = zsinis; double zcosh = cosq; double zsinh = sinq; double cc = c1ss; double zn = zns; double ze = zes; double zmo = dp_zmos; double xnoi = 1.0 / m_xnodp; double a1; double a3; double a7; double a8; double a9; double a10; double a2; double a4; double a5; double a6; double x1; double x2; double x3; double x4; double x5; double x6; double x7; double x8; double z31; double z32; double z33; double z1; double z2; double z3; double z11; double z12; double z13; double z21; double z22; double z23; double s3; double s2; double s4; double s1; double s5; double s6; double s7; double se = 0.0; double si = 0.0; double sl = 0.0; double sgh = 0.0; double sh = 0.0; // Apply the solar and lunar terms on the first pass, then re-apply the // solar terms again on the second pass. for (int pass = 1; pass <= 2; pass++) { // Do solar terms a1 = zcosg * zcosh + zsing * zcosi * zsinh; a3 = -zsing * zcosh + zcosg * zcosi * zsinh; a7 = -zcosg * zsinh + zsing * zcosi * zcosh; a8 = zsing * zsini; a9 = zsing * zsinh + zcosg * zcosi * zcosh; a10 = zcosg * zsini; a2 = cosiq * a7 + siniq * a8; a4 = cosiq * a9 + siniq * a10; a5 = -siniq * a7 + cosiq * a8; a6 = -siniq * a9 + cosiq * a10; x1 = a1 * cosomo + a2 * sinomo; x2 = a3 * cosomo + a4 * sinomo; x3 = -a1 * sinomo + a2 * cosomo; x4 = -a3 * sinomo + a4 * cosomo; x5 = a5 * sinomo; x6 = a6 * sinomo; x7 = a5 * cosomo; x8 = a6 * cosomo; z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * eqsq; z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * eqsq; z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * eqsq; z11 = -6.0 * a1 * a5 + eqsq*(-24.0 * x1 * x7 - 6.0 * x3 * x5); z12 = -6.0 * (a1 * a6 + a3 * a5) + eqsq * (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); z13 = -6.0 * a3 * a6 + eqsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); z21 = 6.0 * a2 * a5 + eqsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); z22 = 6.0*(a4 * a5 + a2 * a6) + eqsq * (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); z23 = 6.0 * a4 * a6 + eqsq*(24.0 * x2 * x6 - 6.0 * x4 * x8); z1 = z1 + z1 + bsq * z31; z2 = z2 + z2 + bsq * z32; z3 = z3 + z3 + bsq * z33; s3 = cc * xnoi; s2 = -0.5 * s3/rteqsq; s4 = s3 * rteqsq; s1 = -15.0 * eq * s4; s5 = x1 * x3 + x2 * x4; s6 = x2 * x3 + x1 * x4; s7 = x2 * x4 - x1 * x3; se = s1 * zn * s5; si = s2 * zn * (z11 + z13); sl = -zn * s3 * (z1 + z3 - 14.0 - 6.0 * eqsq); sgh = s4 * zn * (z31 + z33 - 6.0); sh = -zn * s2 * (z21 + z23); if (dp_xqncl < 5.2359877E-2) sh = 0.0; dp_ee2 = 2.0 * s1 * s6; dp_e3 = 2.0 * s1 * s7; dp_xi2 = 2.0 * s2 * z12; dp_xi3 = 2.0 * s2 * (z13 - z11); dp_xl2 = -2.0 * s3 * z2; dp_xl3 = -2.0 * s3 * (z3 - z1); dp_xl4 = -2.0 * s3 * (-21.0 - 9.0 * eqsq) * ze; dp_xgh2 = 2.0 * s4 * z32; dp_xgh3 = 2.0 * s4 * (z33 - z31); dp_xgh4 = -18.0 * s4 * ze; dp_xh2 = -2.0 * s2 * z22; dp_xh3 = -2.0 * s2 * (z23 - z21); if (pass == 1) { // Do lunar terms dp_sse = se; dp_ssi = si; dp_ssl = sl; dp_ssh = sh / siniq; dp_ssg = sgh - cosiq * dp_ssh; dp_se2 = dp_ee2; dp_si2 = dp_xi2; dp_sl2 = dp_xl2; dp_sgh2 = dp_xgh2; dp_sh2 = dp_xh2; dp_se3 = dp_e3; dp_si3 = dp_xi3; dp_sl3 = dp_xl3; dp_sgh3 = dp_xgh3; dp_sh3 = dp_xh3; dp_sl4 = dp_xl4; dp_sgh4 = dp_xgh4; zcosg = dpi_zcosgl; zsing = dpi_zsingl; zcosi = dpi_zcosil; zsini = dpi_zsinil; zcosh = dpi_zcoshl * cosq + dpi_zsinhl * sinq; zsinh = sinq * dpi_zcoshl - cosq * dpi_zsinhl; zn = znl; cc = c1l; ze = zel; zmo = dp_zmol; } } dp_sse = dp_sse + se; dp_ssi = dp_ssi + si; dp_ssl = dp_ssl + sl; dp_ssg = dp_ssg + sgh - cosiq / siniq * sh; dp_ssh = dp_ssh + sh / siniq; // Geopotential resonance initialization for 12 hour orbits dp_iresfl = false; dp_isynfl = false; bool bInitOnExit = true; double g310; double f220; double bfact = 0.0; if ((m_xnodp >= 0.0052359877) || (m_xnodp <= 0.0034906585)) { if ((m_xnodp < 8.26E-3) || (m_xnodp > 9.24E-3) || (eq < 0.5)) { bInitOnExit = false; } else { dp_iresfl = true; double eoc = eq * eqsq; double g201 = -0.306 - (eq - 0.64) * 0.440; double g211; double g322; double g410; double g422; double g520; if (eq <= 0.65) { g211 = 3.616 - 13.247 * eq + 16.290 * eqsq; g310 = -19.302 + 117.390 * eq - 228.419 * eqsq + 156.591 * eoc; g322 = -18.9068 + 109.7927 * eq - 214.6334 * eqsq + 146.5816 * eoc; g410 = -41.122 + 242.694 * eq - 471.094 * eqsq + 313.953 * eoc; g422 = -146.407 + 841.880 * eq - 1629.014 * eqsq + 1083.435 * eoc; g520 = -532.114 + 3017.977 * eq - 5740.0 * eqsq + 3708.276 * eoc; } else { g211 = -72.099 + 331.819 * eq - 508.738 * eqsq + 266.724 * eoc; g310 = -346.844 + 1582.851 * eq - 2415.925 * eqsq + 1246.113 * eoc; g322 = -342.585 + 1554.908 * eq - 2366.899 * eqsq + 1215.972 * eoc; g410 = -1052.797 + 4758.686 * eq - 7193.992 * eqsq + 3651.957 * eoc; g422 = -3581.69 + 16178.11 * eq - 24462.77 * eqsq + 12422.52 * eoc; if (eq <= 0.715) g520 = 1464.74 - 4664.75 * eq + 3763.64 * eqsq; else g520 = -5149.66 + 29936.92 * eq - 54087.36 * eqsq + 31324.56 * eoc; } double g533; double g521; double g532; if (eq < 0.7) { g533 = -919.2277 + 4988.61 * eq - 9064.77 * eqsq + 5542.21 * eoc; g521 = -822.71072 + 4568.6173 * eq - 8491.4146 * eqsq + 5337.524 * eoc; g532 = -853.666 + 4690.25 * eq - 8624.77 * eqsq + 5341.4 * eoc; } else { g533 = -37995.78 + 161616.52 * eq - 229838.2 * eqsq + 109377.94 * eoc; g521 = -51752.104 + 218913.95 * eq - 309468.16 * eqsq + 146349.42 * eoc; g532 = -40023.88 + 170470.89 * eq - 242699.48 * eqsq + 115605.82 * eoc; } double sini2 = siniq * siniq; f220 = 0.75*(1.0 + 2.0 * cosiq + cosq2); double f221 = 1.5 * sini2; double f321 = 1.875 * siniq*(1.0 - 2.0 * cosiq - 3.0 * cosq2); double f322 = -1.875 * siniq*(1.0 + 2.0 * cosiq - 3.0 * cosq2); double f441 = 35.0 * sini2 * f220; double f442 = 39.3750 * sini2 * sini2; double f522 = 9.84375 * siniq*(sini2*(1.0 - 2.0 * cosiq - 5.0 * cosq2) + 0.33333333*(-2.0 + 4.0 * cosiq + 6.0 * cosq2)); double f523 = siniq*(4.92187512 * sini2*(-2.0 - 4.0 * cosiq + 10.0 * cosq2) + 6.56250012 * (1.0 + 2.0 * cosiq - 3.0 * cosq2)); double f542 = 29.53125 * siniq*(2.0 - 8.0 * cosiq + cosq2 * (-12.0 + 8.0 * cosiq + 10.0 * cosq2)); double f543 = 29.53125 * siniq*(-2.0 - 8.0 * cosiq + cosq2 * (12.0 + 8.0 * cosiq - 10.0 * cosq2)); double xno2 = m_xnodp * m_xnodp; double ainv2 = aqnv * aqnv; double temp1 = 3.0 * xno2 * ainv2; double temp = temp1 * root22; dp_d2201 = temp * f220 * g201; dp_d2211 = temp * f221 * g211; temp1 = temp1 * aqnv; temp = temp1 * root32; dp_d3210 = temp * f321 * g310; dp_d3222 = temp * f322 * g322; temp1 = temp1 * aqnv; temp = 2.0 * temp1 * root44; dp_d4410 = temp * f441 * g410; dp_d4422 = temp * f442 * g422; temp1 = temp1 * aqnv; temp = temp1 * root52; dp_d5220 = temp * f522 * g520; dp_d5232 = temp * f523 * g532; temp = 2.0 * temp1 * root54; dp_d5421 = temp * f542 * g521; dp_d5433 = temp * f543 * g533; dp_xlamo = xmao + m_Orbit.RAAN() + m_Orbit.RAAN() - dp_thgr - dp_thgr; bfact = xlldot + xnodot + xnodot - thdt - thdt; bfact = bfact + dp_ssl + dp_ssh + dp_ssh; } } else { // Synchronous resonance terms initialization dp_iresfl = true; dp_isynfl = true; double g200 = 1.0 + eqsq * (-2.5 + 0.8125 * eqsq); g310 = 1.0 + 2.0 * eqsq; double g300 = 1.0 + eqsq * (-6.0 + 6.60937 * eqsq); f220 = 0.75 * (1.0 + cosiq) * (1.0 + cosiq); double f311 = 0.9375 * siniq * siniq * (1.0 + 3 * cosiq) - 0.75 * (1.0 + cosiq); double f330 = 1.0 + cosiq; f330 = 1.875 * f330 * f330 * f330; dp_del1 = 3.0 * m_xnodp * m_xnodp * aqnv * aqnv; dp_del2 = 2.0 * dp_del1 * f220 * g200 * q22; dp_del3 = 3.0 * dp_del1 * f330 * g300 * q33 * aqnv; dp_del1 = dp_del1 * f311 * g310 * q31 * aqnv; dp_fasx2 = 0.13130908; dp_fasx4 = 2.8843198; dp_fasx6 = 0.37448087; dp_xlamo = xmao + m_Orbit.RAAN() + m_Orbit.ArgPerigee() - dp_thgr; bfact = xlldot + xpidot - thdt; bfact = bfact + dp_ssl + dp_ssg + dp_ssh; } if (bInitOnExit) { dp_xfact = bfact - m_xnodp; // Initialize integrator dp_xli = dp_xlamo; dp_xni = m_xnodp; dp_atime = 0.0; dp_stepp = 720.0; dp_stepn = -720.0; dp_step2 = 259200.0; } *eosq = eqsq; *sinio = siniq; *cosio = cosiq; *betao = rteqsq; *aodp = ao; *theta2 = cosq2; *sing = sinomo; *cosg = cosomo; *betao2 = bsq; *xmdot = xlldot; *omgdot = omgdt; *xnodott = xnodot; return true; } ////////////////////////////////////////////////////////////////////////////// bool cNoradSDP4::DeepCalcDotTerms(double *pxndot, double *pxnddt, double *pxldot) { // Dot terms calculated if (dp_isynfl) { *pxndot = dp_del1 * sin(dp_xli - dp_fasx2) + dp_del2 * sin(2.0 * (dp_xli - dp_fasx4)) + dp_del3 * sin(3.0 * (dp_xli - dp_fasx6)); *pxnddt = dp_del1 * cos(dp_xli - dp_fasx2) + 2.0 * dp_del2 * cos(2.0 * (dp_xli - dp_fasx4)) + 3.0 * dp_del3 * cos(3.0 * (dp_xli - dp_fasx6)); } else { double xomi = dp_omegaq + omgdt * dp_atime; double x2omi = xomi + xomi; double x2li = dp_xli + dp_xli; *pxndot = dp_d2201 * sin(x2omi + dp_xli - g22) + dp_d2211 * sin(dp_xli - g22) + dp_d3210 * sin(xomi + dp_xli - g32) + dp_d3222 * sin(-xomi + dp_xli - g32) + dp_d4410 * sin(x2omi + x2li - g44) + dp_d4422 * sin(x2li - g44) + dp_d5220 * sin(xomi + dp_xli - g52) + dp_d5232 * sin(-xomi + dp_xli - g52) + dp_d5421 * sin(xomi + x2li - g54) + dp_d5433 * sin(-xomi + x2li - g54); *pxnddt = dp_d2201 * cos(x2omi + dp_xli - g22) + dp_d2211 * cos(dp_xli - g22) + dp_d3210 * cos(xomi + dp_xli - g32) + dp_d3222 * cos(-xomi + dp_xli - g32) + dp_d5220 * cos(xomi + dp_xli - g52) + dp_d5232 * cos(-xomi + dp_xli - g52) + 2.0 * (dp_d4410 * cos(x2omi + x2li - g44) + dp_d4422 * cos(x2li - g44) + dp_d5421 * cos(xomi + x2li - g54) + dp_d5433 * cos(-xomi + x2li - g54)); } *pxldot = dp_xni + dp_xfact; *pxnddt = (*pxnddt) * (*pxldot); return true; } ////////////////////////////////////////////////////////////////////////////// void cNoradSDP4::DeepCalcIntegrator(double *pxndot, double *pxnddt, double *pxldot, const double &delt) { DeepCalcDotTerms(pxndot, pxnddt, pxldot); dp_xli = dp_xli + (*pxldot) * delt + (*pxndot) * dp_step2; dp_xni = dp_xni + (*pxndot) * delt + (*pxnddt) * dp_step2; dp_atime = dp_atime + delt; } ////////////////////////////////////////////////////////////////////////////// bool cNoradSDP4::DeepSecular(double *xmdf, double *omgadf, double *xnode, double *emm, double *xincc, double *xnn, double *tsince) { xll = *xmdf; omgasm = *omgadf; xnodes = *xnode; xn = *xnn; t = *tsince; // Deep space secular effects xll = xll + dp_ssl * t; omgasm = omgasm + dp_ssg * t; xnodes = xnodes + dp_ssh * t; _em = m_Orbit.Eccentricity() + dp_sse * t; xinc = m_Orbit.Inclination() + dp_ssi * t; if (xinc < 0.0) { xinc = -xinc; xnodes = xnodes + PI; omgasm = omgasm - PI; } double xnddt = 0.0; double xndot = 0.0; double xldot = 0.0; double ft = 0.0; double delt = 0.0; bool fDone = false; if (dp_iresfl) { while (!fDone) { if ((dp_atime == 0.0) || ((t >= 0.0) && (dp_atime < 0.0)) || ((t < 0.0) && (dp_atime >= 0.0))) { if (t < 0) delt = dp_stepn; else delt = dp_stepp; // Epoch restart dp_atime = 0.0; dp_xni = m_xnodp; dp_xli = dp_xlamo; fDone = true; } else { if (fabs(t) < fabs(dp_atime)) { delt = dp_stepp; if (t >= 0.0) delt = dp_stepn; DeepCalcIntegrator(&xndot, &xnddt, &xldot, delt); } else { delt = dp_stepn; delt = dp_stepp; fDone = true; } } } while (fabs(t - dp_atime) >= dp_stepp) { DeepCalcIntegrator(&xndot, &xnddt, &xldot, delt); } ft = t - dp_atime; DeepCalcDotTerms(&xndot, &xnddt, &xldot); xn = dp_xni + xndot * ft + xnddt * ft * ft * 0.5; double xl = dp_xli + xldot * ft + xndot * ft * ft * 0.5; double temp = -xnodes + dp_thgr + t * thdt; xll = xl - omgasm + temp; if (!dp_isynfl) xll = xl + temp + temp; } *xmdf = xll; *omgadf = omgasm; *xnode = xnodes; *emm = _em; *xincc = xinc; *xnn = xn; *tsince = t; return true; } ////////////////////////////////////////////////////////////////////////////// bool cNoradSDP4::DeepPeriodics(double *e, double *xincc, double *omgadf, double *xnode, double *xmam) { _em = *e; xinc = *xincc; omgasm = *omgadf; xnodes = *xnode; xll = *xmam; // Lunar-solar periodics double sinis = sin(xinc); double cosis = cos(xinc); double sghs = 0.0; double shs = 0.0; double sh1 = 0.0; double pe = 0.0; double pinc = 0.0; double pl = 0.0; double sghl = 0.0; if (fabs(dp_savtsn - t) >= 30.0) { dp_savtsn = t; double zm = dp_zmos + zns * t; double zf = zm + 2.0 * zes * sin(zm); double sinzf = sin(zf); double f2 = 0.5 * sinzf * sinzf - 0.25; double f3 = -0.5 * sinzf * cos(zf); double ses = dp_se2 * f2 + dp_se3 * f3; double sis = dp_si2 * f2 + dp_si3 * f3; double sls = dp_sl2 * f2 + dp_sl3 * f3 + dp_sl4 * sinzf; sghs = dp_sgh2 * f2 + dp_sgh3 * f3 + dp_sgh4 * sinzf; shs = dp_sh2 * f2 + dp_sh3 * f3; zm = dp_zmol + znl * t; zf = zm + 2.0 * zel * sin(zm); sinzf = sin(zf); f2 = 0.5 * sinzf * sinzf - 0.25; f3 = -0.5 * sinzf * cos(zf); double sel = dp_ee2 * f2 + dp_e3 * f3; double sil = dp_xi2 * f2 + dp_xi3 * f3; double sll = dp_xl2 * f2 + dp_xl3 * f3 + dp_xl4 * sinzf; sghl = dp_xgh2 * f2 + dp_xgh3 * f3 + dp_xgh4 * sinzf; sh1 = dp_xh2 * f2 + dp_xh3 * f3; pe = ses + sel; pinc = sis + sil; pl = sls + sll; } double pgh = sghs + sghl; double ph = shs + sh1; xinc = xinc + pinc; _em = _em + pe; if (dp_xqncl >= 0.2) { // Apply periodics directly ph = ph / siniq; pgh = pgh - cosiq * ph; omgasm = omgasm + pgh; xnodes = xnodes + ph; xll = xll + pl; } else { // Apply periodics with Lyddane modification double sinok = sin(xnodes); double cosok = cos(xnodes); double alfdp = sinis * sinok; double betdp = sinis * cosok; double dalf = ph * cosok + pinc * cosis * sinok; double dbet = -ph * sinok + pinc * cosis * cosok; alfdp = alfdp + dalf; betdp = betdp + dbet; double xls = xll + omgasm + cosis * xnodes; double dls = pl + pgh - pinc * xnodes * sinis; xls = xls + dls; xnodes = AcTan(alfdp, betdp); xll = xll + pl; omgasm = xls - xll - cos(xinc) * xnodes; } *e = _em; *xincc = xinc; *omgadf = omgasm; *xnode = xnodes; *xmam = xll; return true; } ////////////////////////////////////////////////////////////////////////////// // getPosition() // This procedure returns the ECI position and velocity for the satellite // in the orbit at the given number of minutes since the TLE epoch time // using the NORAD Simplified General Perturbation 4, "deep space" orbit // model. // // tsince - Time in minutes since the TLE epoch (GMT). // pECI - pointer to location to store the ECI data. // To convert the returned ECI position vector to km, // multiply each component by: // (XKMPER_WGS72 / AE). // To convert the returned ECI velocity vector to km/sec, // multiply each component by: // (XKMPER_WGS72 / AE) * (MIN_PER_DAY / 86400). bool cNoradSDP4::getPosition(double tsince, cEci &eci) { DeepInit(&m_eosq, &m_sinio, &m_cosio, &m_betao, &m_aodp, &m_theta2, &m_sing, &m_cosg, &m_betao2, &m_xmdot, &m_omgdot, &m_xnodot); // Update for secular gravity and atmospheric drag double xmdf = m_Orbit.mnAnomaly() + m_xmdot * tsince; double omgadf = m_Orbit.ArgPerigee() + m_omgdot * tsince; double xnoddf = m_Orbit.RAAN() + m_xnodot * tsince; double tsq = tsince * tsince; double xnode = xnoddf + m_xnodcf * tsq; double tempa = 1.0 - m_c1 * tsince; double tempe = m_Orbit.BStar() * m_c4 * tsince; double templ = m_t2cof * tsq; double xn = m_xnodp; double em; double xinc; DeepSecular(&xmdf, &omgadf, &xnode, &em, &xinc, &xn, &tsince); double a = pow(XKE / xn, TWOTHRD) * sqr(tempa); double e = em - tempe; double xmam = xmdf + m_xnodp * templ; DeepPeriodics(&e, &xinc, &omgadf, &xnode, &xmam); double xl = xmam + omgadf + xnode; xn = XKE / pow(a, 1.5); return FinalPosition(xinc, omgadf, e, a, xl, xnode, xn, tsince, eci); } // cOrbit.cpp // // Copyright (c) 2002-2003 Michael F. Henry // // mfh 11/15/2003 // ////////////////////////////////////////////////////////////////////// cOrbit::cOrbit(const cTle &tle) : m_tle(tle), m_pNoradModel(NULL) { m_tle.Initialize(); int epochYear = (int)m_tle.getField(cTle::FLD_EPOCHYEAR); double epochDay = m_tle.getField(cTle::FLD_EPOCHDAY ); if (epochYear < 57) epochYear += 2000; else epochYear += 1900; m_jdEpoch = cJulian(epochYear, epochDay); m_secPeriod = -1.0; // Recover the original mean motion and semimajor axis from the // input elements. double mm = mnMotion(); double rpmin = mm * 2 * PI / MIN_PER_DAY; // rads per minute double a1 = pow(XKE / rpmin, TWOTHRD); double e = Eccentricity(); double i = Inclination(); double temp = (1.5 * CK2 * (3.0 * sqr(cos(i)) - 1.0) / pow(1.0 - e * e, 1.5)); double delta1 = temp / (a1 * a1); double a0 = a1 * (1.0 - delta1 * ((1.0 / 3.0) + delta1 * (1.0 + 134.0 / 81.0 * delta1))); double delta0 = temp / (a0 * a0); m_mnMotionRec = rpmin / (1.0 + delta0); m_aeAxisSemiMinorRec = a0 / (1.0 - delta0); m_aeAxisSemiMajorRec = m_aeAxisSemiMinorRec / sqrt(1.0 - (e * e)); m_kmPerigeeRec = XKMPER_WGS72 * (m_aeAxisSemiMajorRec * (1.0 - e) - AE); m_kmApogeeRec = XKMPER_WGS72 * (m_aeAxisSemiMajorRec * (1.0 + e) - AE); if (2.0 * PI / m_mnMotionRec >= 225.0) { // SDP4 - period >= 225 minutes. m_pNoradModel = new cNoradSDP4(*this); } else { // SGP4 - period < 225 minutes m_pNoradModel = new cNoradSGP4(*this); } } ///////////////////////////////////////////////////////////////////////////// cOrbit::~cOrbit() { delete m_pNoradModel; } ////////////////////////////////////////////////////////////////////////////// // Return the period in seconds double cOrbit::Period() const { if (m_secPeriod < 0.0) { // Calculate the period using the recovered mean motion. if (m_mnMotionRec == 0) m_secPeriod = 0.0; else m_secPeriod = (2 * PI) / m_mnMotionRec * 60.0; } return m_secPeriod; } ////////////////////////////////////////////////////////////////////////////// // Returns elapsed number of seconds from epoch to given time. // Note: "Predicted" TLEs can have epochs in the future. double cOrbit::TPlusEpoch(const cJulian &gmt) const { return gmt.spanSec(Epoch()); } ////////////////////////////////////////////////////////////////////////////// // Returns the mean anomaly in radians at given GMT. // At epoch, the mean anomaly is given by the elements data. double cOrbit::mnAnomaly(cJulian gmt) const { double span = TPlusEpoch(gmt); double P = Period(); assert(P != 0.0); return fmod(mnAnomaly() + (TWOPI * (span / P)), TWOPI); } ////////////////////////////////////////////////////////////////////////////// // getPosition() // This procedure returns the ECI position and velocity for the satellite // at "tsince" minutes from the (GMT) TLE epoch. The vectors returned in // the ECI object are kilometer-based. // tsince - Time in minutes since the TLE epoch (GMT). bool cOrbit::getPosition(double tsince, cEci *pEci) const { bool rc; rc = m_pNoradModel->getPosition(tsince, *pEci); pEci->ae2km(); return rc; } ////////////////////////////////////////////////////////////////////////////// // SatName() // Return the name of the satellite. If requested, the NORAD number is // appended to the end of the name, i.e., "ISS (ZARYA) #25544". // The name of the satellite with the NORAD number appended is important // because many satellites, especially debris, have the same name and // would otherwise appear to be the same satellite in ouput data. string cOrbit::SatName(bool fAppendId /* = false */) const { string str = m_tle.getName(); if (fAppendId) { string strId; m_tle.getField(cTle::FLD_NORADNUM, cTle::U_NATIVE, &strId); str = str + " #" + strId; } return str; }