| 1 |
kusanagi |
1.1 |
//
|
| 2 |
|
|
// cNoradBase.cpp
|
| 3 |
|
|
//
|
| 4 |
|
|
// Historical Note:
|
| 5 |
|
|
// The equations used here (and in derived classes) to determine satellite
|
| 6 |
|
|
// ECI coordinates/velocity come from the December, 1980 NORAD document
|
| 7 |
|
|
// "Space Track Report No. 3". The report details 6 orbital models and
|
| 8 |
|
|
// provides FORTRAN IV implementations of each. The classes here
|
| 9 |
|
|
// implement only two of the orbital models: SGP4 and SDP4. These two models,
|
| 10 |
|
|
// one for "near-earth" objects and one for "deep space" objects, are widely
|
| 11 |
|
|
// used in satellite tracking software and can produce very accurate results
|
| 12 |
|
|
// when used with current NORAD two-line element datum.
|
| 13 |
|
|
//
|
| 14 |
|
|
// The NORAD FORTRAN IV SGP4/SDP4 implementations were converted to Pascal by
|
| 15 |
|
|
// Dr. TS Kelso in 1995. In 1996 these routines were ported in a straight-
|
| 16 |
|
|
// forward manner to C++ by Varol Okan. The SGP4/SDP4 classes here were
|
| 17 |
|
|
// written by Michael F. Henry in 2002-03 and are a modern C++ re-write of
|
| 18 |
|
|
// the work done by Okan. In addition to introducing an object-oriented
|
| 19 |
|
|
// architecture, the last residues of the original FORTRAN code (such as
|
| 20 |
|
|
// labels and gotos) were eradicated.
|
| 21 |
|
|
//
|
| 22 |
|
|
// For excellent information on the underlying physics of orbits, visible
|
| 23 |
|
|
// satellite observations, current NORAD TLE data, and other related material,
|
| 24 |
|
|
// see http://www.celestrak.com which is maintained by Dr. TS Kelso.
|
| 25 |
|
|
//
|
| 26 |
|
|
// Copyright (c) 2003 Michael F. Henry
|
| 27 |
|
|
//
|
| 28 |
|
|
// mfh 12/07/2003
|
| 29 |
|
|
//
|
| 30 |
|
|
#include "stdafx.h"
|
| 31 |
|
|
#include "cNoradBase.h"
|
| 32 |
|
|
#include "cOrbit.h"
|
| 33 |
|
|
#include "coord.h"
|
| 34 |
|
|
#include "cEci.h"
|
| 35 |
|
|
#include "cVector.h"
|
| 36 |
|
|
#include "cJulian.h"
|
| 37 |
|
|
|
| 38 |
|
|
//////////////////////////////////////////////////////////////////////////////
|
| 39 |
|
|
cNoradBase::cNoradBase(const cOrbit &orbit) :
|
| 40 |
|
|
m_Orbit(orbit)
|
| 41 |
|
|
{
|
| 42 |
|
|
Initialize();
|
| 43 |
|
|
}
|
| 44 |
|
|
|
| 45 |
|
|
cNoradBase::~cNoradBase(void)
|
| 46 |
|
|
{
|
| 47 |
|
|
}
|
| 48 |
|
|
|
| 49 |
|
|
cNoradBase& cNoradBase::operator=(const cNoradBase &b)
|
| 50 |
|
|
{
|
| 51 |
|
|
// m_Orbit is a "const" member var, so cast away its
|
| 52 |
|
|
// "const-ness" in order to complete the assigment.
|
| 53 |
|
|
*(const_cast<cOrbit*>(&m_Orbit)) = b.m_Orbit;
|
| 54 |
|
|
|
| 55 |
|
|
return *this;
|
| 56 |
|
|
}
|
| 57 |
|
|
|
| 58 |
|
|
//////////////////////////////////////////////////////////////////////////////
|
| 59 |
|
|
// Initialize()
|
| 60 |
|
|
// Perform the initialization of member variables, specifically the variables
|
| 61 |
|
|
// used by derived-class objects to calculate ECI coordinates.
|
| 62 |
|
|
void cNoradBase::Initialize()
|
| 63 |
|
|
{
|
| 64 |
|
|
// Initialize any variables which are time-independent when
|
| 65 |
|
|
// calculating the ECI coordinates of the satellite.
|
| 66 |
|
|
m_satInc = m_Orbit.Inclination();
|
| 67 |
|
|
m_satEcc = m_Orbit.Eccentricity();
|
| 68 |
|
|
|
| 69 |
|
|
m_cosio = cos(m_satInc);
|
| 70 |
|
|
m_theta2 = m_cosio * m_cosio;
|
| 71 |
|
|
m_x3thm1 = 3.0 * m_theta2 - 1.0;
|
| 72 |
|
|
m_eosq = m_satEcc * m_satEcc;
|
| 73 |
|
|
m_betao2 = 1.0 - m_eosq;
|
| 74 |
|
|
m_betao = sqrt(m_betao2);
|
| 75 |
|
|
|
| 76 |
|
|
// The "recovered" semi-minor axis and mean motion.
|
| 77 |
|
|
m_aodp = m_Orbit.SemiMinor();
|
| 78 |
|
|
m_xnodp = m_Orbit.mnMotionRec();
|
| 79 |
|
|
|
| 80 |
|
|
// For perigee below 156 km, the values of S and QOMS2T are altered.
|
| 81 |
|
|
m_perigee = XKMPER_WGS72 * (m_aodp * (1.0 - m_satEcc) - AE);
|
| 82 |
|
|
|
| 83 |
|
|
m_s4 = S;
|
| 84 |
|
|
m_qoms24 = QOMS2T;
|
| 85 |
|
|
|
| 86 |
|
|
if (m_perigee < 156.0)
|
| 87 |
|
|
{
|
| 88 |
|
|
m_s4 = m_perigee - 78.0;
|
| 89 |
|
|
|
| 90 |
|
|
if (m_perigee <= 98.0)
|
| 91 |
|
|
{
|
| 92 |
|
|
m_s4 = 20.0;
|
| 93 |
|
|
}
|
| 94 |
|
|
|
| 95 |
|
|
m_qoms24 = pow((120.0 - m_s4) * AE / XKMPER_WGS72, 4.0);
|
| 96 |
|
|
m_s4 = m_s4 / XKMPER_WGS72 + AE;
|
| 97 |
|
|
}
|
| 98 |
|
|
|
| 99 |
|
|
const double pinvsq = 1.0 / (m_aodp * m_aodp * m_betao2 * m_betao2);
|
| 100 |
|
|
|
| 101 |
|
|
m_tsi = 1.0 / (m_aodp - m_s4);
|
| 102 |
|
|
m_eta = m_aodp * m_satEcc * m_tsi;
|
| 103 |
|
|
m_etasq = m_eta * m_eta;
|
| 104 |
|
|
m_eeta = m_satEcc * m_eta;
|
| 105 |
|
|
|
| 106 |
|
|
const double psisq = fabs(1.0 - m_etasq);
|
| 107 |
|
|
|
| 108 |
|
|
m_coef = m_qoms24 * pow(m_tsi,4.0);
|
| 109 |
|
|
m_coef1 = m_coef / pow(psisq,3.5);
|
| 110 |
|
|
|
| 111 |
|
|
const double c2 = m_coef1 * m_xnodp *
|
| 112 |
|
|
(m_aodp * (1.0 + 1.5 * m_etasq + m_eeta * (4.0 + m_etasq)) +
|
| 113 |
|
|
0.75 * CK2 * m_tsi / psisq * m_x3thm1 *
|
| 114 |
|
|
(8.0 + 3.0 * m_etasq * (8.0 + m_etasq)));
|
| 115 |
|
|
|
| 116 |
|
|
m_c1 = m_Orbit.BStar() * c2;
|
| 117 |
|
|
m_sinio = sin(m_satInc);
|
| 118 |
|
|
|
| 119 |
|
|
const double a3ovk2 = -XJ3 / CK2 * pow(AE,3.0);
|
| 120 |
|
|
|
| 121 |
|
|
m_c3 = m_coef * m_tsi * a3ovk2 * m_xnodp * AE * m_sinio / m_satEcc;
|
| 122 |
|
|
m_x1mth2 = 1.0 - m_theta2;
|
| 123 |
|
|
m_c4 = 2.0 * m_xnodp * m_coef1 * m_aodp * m_betao2 *
|
| 124 |
|
|
(m_eta * (2.0 + 0.5 * m_etasq) +
|
| 125 |
|
|
m_satEcc * (0.5 + 2.0 * m_etasq) -
|
| 126 |
|
|
2.0 * CK2 * m_tsi / (m_aodp * psisq) *
|
| 127 |
|
|
(-3.0 * m_x3thm1 * (1.0 - 2.0 * m_eeta + m_etasq * (1.5 - 0.5 * m_eeta)) +
|
| 128 |
|
|
0.75 * m_x1mth2 *
|
| 129 |
|
|
(2.0 * m_etasq - m_eeta * (1.0 + m_etasq)) *
|
| 130 |
|
|
cos(2.0 * m_Orbit.ArgPerigee())));
|
| 131 |
|
|
|
| 132 |
|
|
const double theta4 = m_theta2 * m_theta2;
|
| 133 |
|
|
const double temp1 = 3.0 * CK2 * pinvsq * m_xnodp;
|
| 134 |
|
|
const double temp2 = temp1 * CK2 * pinvsq;
|
| 135 |
|
|
const double temp3 = 1.25 * CK4 * pinvsq * pinvsq * m_xnodp;
|
| 136 |
|
|
|
| 137 |
|
|
m_xmdot = m_xnodp + 0.5 * temp1 * m_betao * m_x3thm1 +
|
| 138 |
|
|
0.0625 * temp2 * m_betao *
|
| 139 |
|
|
(13.0 - 78.0 * m_theta2 + 137.0 * theta4);
|
| 140 |
|
|
|
| 141 |
|
|
const double x1m5th = 1.0 - 5.0 * m_theta2;
|
| 142 |
|
|
|
| 143 |
|
|
m_omgdot = -0.5 * temp1 * x1m5th + 0.0625 * temp2 *
|
| 144 |
|
|
(7.0 - 114.0 * m_theta2 + 395.0 * theta4) +
|
| 145 |
|
|
temp3 * (3.0 - 36.0 * m_theta2 + 49.0 * theta4);
|
| 146 |
|
|
|
| 147 |
|
|
const double xhdot1 = -temp1 * m_cosio;
|
| 148 |
|
|
|
| 149 |
|
|
m_xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * m_theta2) +
|
| 150 |
|
|
2.0 * temp3 * (3.0 - 7.0 * m_theta2)) * m_cosio;
|
| 151 |
|
|
m_xnodcf = 3.5 * m_betao2 * xhdot1 * m_c1;
|
| 152 |
|
|
m_t2cof = 1.5 * m_c1;
|
| 153 |
|
|
m_xlcof = 0.125 * a3ovk2 * m_sinio *
|
| 154 |
|
|
(3.0 + 5.0 * m_cosio) / (1.0 + m_cosio);
|
| 155 |
|
|
m_aycof = 0.25 * a3ovk2 * m_sinio;
|
| 156 |
|
|
m_x7thm1 = 7.0 * m_theta2 - 1.0;
|
| 157 |
|
|
}
|
| 158 |
|
|
|
| 159 |
|
|
//////////////////////////////////////////////////////////////////////////////
|
| 160 |
|
|
bool cNoradBase::FinalPosition(double incl, double omega,
|
| 161 |
|
|
double e, double a,
|
| 162 |
|
|
double xl, double xnode,
|
| 163 |
|
|
double xn, double tsince,
|
| 164 |
|
|
cEci &eci)
|
| 165 |
|
|
{
|
| 166 |
|
|
if ((e * e) > 1.0)
|
| 167 |
|
|
{
|
| 168 |
|
|
// error in satellite data
|
| 169 |
|
|
return false;
|
| 170 |
|
|
}
|
| 171 |
|
|
|
| 172 |
|
|
double beta = sqrt(1.0 - e * e);
|
| 173 |
|
|
|
| 174 |
|
|
// Long period periodics
|
| 175 |
|
|
double axn = e * cos(omega);
|
| 176 |
|
|
double temp = 1.0 / (a * beta * beta);
|
| 177 |
|
|
double xll = temp * m_xlcof * axn;
|
| 178 |
|
|
double aynl = temp * m_aycof;
|
| 179 |
|
|
double xlt = xl + xll;
|
| 180 |
|
|
double ayn = e * sin(omega) + aynl;
|
| 181 |
|
|
|
| 182 |
|
|
// Solve Kepler's Equation
|
| 183 |
|
|
|
| 184 |
|
|
double capu = Fmod2p(xlt - xnode);
|
| 185 |
|
|
double temp2 = capu;
|
| 186 |
|
|
double temp3 = 0.0;
|
| 187 |
|
|
double temp4 = 0.0;
|
| 188 |
|
|
double temp5 = 0.0;
|
| 189 |
|
|
double temp6 = 0.0;
|
| 190 |
|
|
double sinepw = 0.0;
|
| 191 |
|
|
double cosepw = 0.0;
|
| 192 |
|
|
bool fDone = false;
|
| 193 |
|
|
|
| 194 |
|
|
for (int i = 1; (i <= 10) && !fDone; i++)
|
| 195 |
|
|
{
|
| 196 |
|
|
sinepw = sin(temp2);
|
| 197 |
|
|
cosepw = cos(temp2);
|
| 198 |
|
|
temp3 = axn * sinepw;
|
| 199 |
|
|
temp4 = ayn * cosepw;
|
| 200 |
|
|
temp5 = axn * cosepw;
|
| 201 |
|
|
temp6 = ayn * sinepw;
|
| 202 |
|
|
|
| 203 |
|
|
double epw = (capu - temp4 + temp3 - temp2) /
|
| 204 |
|
|
(1.0 - temp5 - temp6) + temp2;
|
| 205 |
|
|
|
| 206 |
|
|
if (fabs(epw - temp2) <= E6A)
|
| 207 |
|
|
fDone = true;
|
| 208 |
|
|
else
|
| 209 |
|
|
temp2 = epw;
|
| 210 |
|
|
}
|
| 211 |
|
|
|
| 212 |
|
|
// Short period preliminary quantities
|
| 213 |
|
|
double ecose = temp5 + temp6;
|
| 214 |
|
|
double esine = temp3 - temp4;
|
| 215 |
|
|
double elsq = axn * axn + ayn * ayn;
|
| 216 |
|
|
temp = 1.0 - elsq;
|
| 217 |
|
|
double pl = a * temp;
|
| 218 |
|
|
double r = a * (1.0 - ecose);
|
| 219 |
|
|
double temp1 = 1.0 / r;
|
| 220 |
|
|
double rdot = XKE * sqrt(a) * esine * temp1;
|
| 221 |
|
|
double rfdot = XKE * sqrt(pl) * temp1;
|
| 222 |
|
|
temp2 = a * temp1;
|
| 223 |
|
|
double betal = sqrt(temp);
|
| 224 |
|
|
temp3 = 1.0 / (1.0 + betal);
|
| 225 |
|
|
double cosu = temp2 * (cosepw - axn + ayn * esine * temp3);
|
| 226 |
|
|
double sinu = temp2 * (sinepw - ayn - axn * esine * temp3);
|
| 227 |
|
|
double u = AcTan(sinu, cosu);
|
| 228 |
|
|
double sin2u = 2.0 * sinu * cosu;
|
| 229 |
|
|
double cos2u = 2.0 * cosu * cosu - 1.0;
|
| 230 |
|
|
|
| 231 |
|
|
temp = 1.0 / pl;
|
| 232 |
|
|
temp1 = CK2 * temp;
|
| 233 |
|
|
temp2 = temp1 * temp;
|
| 234 |
|
|
|
| 235 |
|
|
// Update for short periodics
|
| 236 |
|
|
double rk = r * (1.0 - 1.5 * temp2 * betal * m_x3thm1) +
|
| 237 |
|
|
0.5 * temp1 * m_x1mth2 * cos2u;
|
| 238 |
|
|
double uk = u - 0.25 * temp2 * m_x7thm1 * sin2u;
|
| 239 |
|
|
double xnodek = xnode + 1.5 * temp2 * m_cosio * sin2u;
|
| 240 |
|
|
double xinck = incl + 1.5 * temp2 * m_cosio * m_sinio * cos2u;
|
| 241 |
|
|
double rdotk = rdot - xn * temp1 * m_x1mth2 * sin2u;
|
| 242 |
|
|
double rfdotk = rfdot + xn * temp1 * (m_x1mth2 * cos2u + 1.5 * m_x3thm1);
|
| 243 |
|
|
|
| 244 |
|
|
// Orientation vectors
|
| 245 |
|
|
double sinuk = sin(uk);
|
| 246 |
|
|
double cosuk = cos(uk);
|
| 247 |
|
|
double sinik = sin(xinck);
|
| 248 |
|
|
double cosik = cos(xinck);
|
| 249 |
|
|
double sinnok = sin(xnodek);
|
| 250 |
|
|
double cosnok = cos(xnodek);
|
| 251 |
|
|
double xmx = -sinnok * cosik;
|
| 252 |
|
|
double xmy = cosnok * cosik;
|
| 253 |
|
|
double ux = xmx * sinuk + cosnok * cosuk;
|
| 254 |
|
|
double uy = xmy * sinuk + sinnok * cosuk;
|
| 255 |
|
|
double uz = sinik * sinuk;
|
| 256 |
|
|
double vx = xmx * cosuk - cosnok * sinuk;
|
| 257 |
|
|
double vy = xmy * cosuk - sinnok * sinuk;
|
| 258 |
|
|
double vz = sinik * cosuk;
|
| 259 |
|
|
|
| 260 |
|
|
// Position
|
| 261 |
|
|
double x = rk * ux;
|
| 262 |
|
|
double y = rk * uy;
|
| 263 |
|
|
double z = rk * uz;
|
| 264 |
|
|
|
| 265 |
|
|
cVector vecPos(x, y, z);
|
| 266 |
|
|
|
| 267 |
|
|
// Validate on altitude
|
| 268 |
|
|
double altKm = (vecPos.Magnitude() * (XKMPER_WGS72 / AE));
|
| 269 |
|
|
|
| 270 |
|
|
if ((altKm < XKMPER_WGS72) || (altKm > (2 * GEOSYNC_ALT)))
|
| 271 |
|
|
return false;
|
| 272 |
|
|
|
| 273 |
|
|
// Velocity
|
| 274 |
|
|
double xdot = rdotk * ux + rfdotk * vx;
|
| 275 |
|
|
double ydot = rdotk * uy + rfdotk * vy;
|
| 276 |
|
|
double zdot = rdotk * uz + rfdotk * vz;
|
| 277 |
|
|
|
| 278 |
|
|
cVector vecVel(xdot, ydot, zdot);
|
| 279 |
|
|
|
| 280 |
|
|
cJulian gmt = m_Orbit.Epoch();
|
| 281 |
|
|
gmt.addMin(tsince);
|
| 282 |
|
|
|
| 283 |
|
|
eci = cEci(vecPos, vecVel, gmt);
|
| 284 |
|
|
|
| 285 |
|
|
return true;
|
| 286 |
|
|
}
|
| 287 |
|
|
|
| 288 |
|
|
|
| 289 |
|
|
|
Parent Directory
| 
Revision Log