KJ135
/
sgp4
mirror of http://172.16.0.12:5000/LAMPSARToolSoftware/sgp4.git
4
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sorting out global variables

feature/19
Daniel Warner 2011-03-27 02:06:49 +01:00
parent 26d5a6c1de
commit 93d60e91e8
2 changed files with 133 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

216
SGDP4.cpp
View File

@ -34,15 +34,16 @@ void SGDP4::SetTle(const Tle& tle) {
* from input elements * from input elements
*/ */
double a1 = pow(Globals::XKE() / MeanMotion(), Globals::TOTHRD()); double a1 = pow(Globals::XKE() / MeanMotion(), Globals::TOTHRD());
cosio_ = cos(Inclination()); i_cosio_ = cos(Inclination());
double theta2 = cosio_ * cosio_; i_sinio_ = sin(Inclination());
x3thm1_ = 3.0 * theta2 - 1.0; double theta2 = i_cosio_ * i_cosio_;
i_x3thm1_ = 3.0 * theta2 - 1.0;
double eosq = Eccentricity() * Eccentricity(); double eosq = Eccentricity() * Eccentricity();
double betao2 = 1.0 - eosq; double betao2 = 1.0 - eosq;
double betao = sqrt(betao2); double betao = sqrt(betao2);
double del1 = 1.5 * Globals::CK2() * x3thm1_ / (a1 * a1 * betao * betao2); double del1 = 1.5 * Globals::CK2() * i_x3thm1_ / (a1 * a1 * betao * betao2);
double ao = a1 * (1.0 - del1 * (0.5 * Globals::TOTHRD() + del1 * (1.0 + 134.0 / 81.0 * del1))); double ao = a1 * (1.0 - del1 * (0.5 * Globals::TOTHRD() + del1 * (1.0 + 134.0 / 81.0 * del1)));
double delo = 1.5 * Globals::CK2() * x3thm1_ / (ao * ao * betao * betao2); double delo = 1.5 * Globals::CK2() * i_x3thm1_ / (ao * ao * betao * betao2);
recovered_mean_motion_ = MeanMotion() / (1.0 + delo); recovered_mean_motion_ = MeanMotion() / (1.0 + delo);
recovered_semi_major_axis_ = ao / (1.0 - delo); recovered_semi_major_axis_ = ao / (1.0 - delo);
@ -58,45 +59,12 @@ void SGDP4::SetTle(const Tle& tle) {
void SGDP4::Initialize(const double& theta2, const double& betao2, const double& betao, const double& eosq) { void SGDP4::Initialize(const double& theta2, const double& betao2, const double& betao, const double& eosq) {
cosio_ = 0.0;
sinio_ = 0.0;
eta_ = 0.0;
coef1_ = 0.0;
c1_ = 0.0;
a3ovk2_ = 0.0;
x1mth2_ = 0.0;
c4_ = 0.0;
c5_ = 0.0;
xmdot_ = 0.0;
omgdot_ = 0.0;
xnodot_ = 0.0;
xnodcf_ = 0.0;
t2cof_ = 0.0;
xlcof_ = 0.0;
aycof_ = 0.0;
x7thm1_ = 0.0;
omgcof_ = 0.0;
xmcof_ = 0.0;
delmo_ = 0.0;
sinmo_ = 0.0;
d2_ = 0.0;
d3_ = 0.0;
d4_ = 0.0;
t3cof_ = 0.0;
t4cof_ = 0.0;
t5cof_ = 0.0;
gsto_ = 0.0;
if (Period() >= 225.0) { if (Period() >= 225.0) {
use_deep_space_ = true; i_use_deep_space_ = true;
} else { } else {
use_deep_space_ = false; i_use_deep_space_ = false;
use_simple_model_ = false; i_use_simple_model_ = false;
/* /*
* for perigee less than 220 kilometers, the simple_model flag is set and * for perigee less than 220 kilometers, the simple_model flag is set and
* the equations are truncated to linear variation in sqrt a and * the equations are truncated to linear variation in sqrt a and
@ -104,106 +72,111 @@ void SGDP4::Initialize(const double& theta2, const double& betao2, const double&
* delta omega term and the delta m term are dropped * delta omega term and the delta m term are dropped
*/ */
if (Perigee() < 220.0) { if (Perigee() < 220.0) {
use_simple_model_ = true; i_use_simple_model_ = true;
} }
} }
double s4_ = Globals::S(); double s4 = Globals::S();
double qoms24_ = Globals::QOMS2T(); double qoms24 = Globals::QOMS2T();
/* /*
* for perigee below 156km, the values of * for perigee below 156km, the values of
* s4 and qoms2t are altered * s4 and qoms2t are altered
*/ */
if (Perigee() < 156.0) { if (Perigee() < 156.0) {
s4_ = Perigee() - 78.0; s4 = Perigee() - 78.0;
if (Perigee() <= 98.0) { if (Perigee() <= 98.0) {
s4_ = 20.0; s4 = 20.0;
} }
qoms24_ = pow((120.0 - s4_) * Globals::AE() / Globals::XKMPER(), 4.0); qoms24 = pow((120.0 - s4) * Globals::AE() / Globals::XKMPER(), 4.0);
s4_ = s4_ / Globals::XKMPER() + Globals::AE(); s4 = s4 / Globals::XKMPER() + Globals::AE();
} }
double pinvsq = 1.0 / (RecoveredSemiMajorAxis() * RecoveredSemiMajorAxis() * betao2 * betao2); double pinvsq = 1.0 / (RecoveredSemiMajorAxis() * RecoveredSemiMajorAxis() * betao2 * betao2);
double tsi = 1.0 / (RecoveredSemiMajorAxis() - s4);
double tsi = 1.0 / (RecoveredSemiMajorAxis() - s4_); i_eta_ = RecoveredSemiMajorAxis() * Eccentricity() * tsi;
eta_ = RecoveredSemiMajorAxis() * Eccentricity() * tsi; double etasq = i_eta_ * i_eta_;
double etasq = eta_ * eta_; double eeta = Eccentricity() * i_eta_;
double eeta = Eccentricity() * eta_;
double psisq = fabs(1.0 - etasq); double psisq = fabs(1.0 - etasq);
double coef = qoms24_ * pow(tsi, 4.0); double coef = qoms24 * pow(tsi, 4.0);
coef1_ = coef / pow(psisq, 3.5); double coef1 = coef / pow(psisq, 3.5);
double c2 = coef1_ * RecoveredMeanMotion() * (RecoveredSemiMajorAxis() * (1.0 + 1.5 * etasq + eeta * double c2 = coef1 * RecoveredMeanMotion() * (RecoveredSemiMajorAxis() *
(4.0 + etasq)) + 0.75 * Globals::CK2() * tsi / psisq * (1.0 + 1.5 * etasq + eeta * (4.0 + etasq)) +
x3thm1_ * (8.0 + 3.0 * etasq * (8.0 + etasq))); 0.75 * Globals::CK2() * tsi / psisq *
c1_ = BStar() * c2; i_x3thm1_ * (8.0 + 3.0 * etasq *
a3ovk2_ = -Globals::XJ3() / Globals::CK2() * pow(Globals::AE(), 3.0); (8.0 + etasq)));
i_c1_ = BStar() * c2;
x1mth2_ = 1.0 - theta2; i_a3ovk2_ = -Globals::XJ3() / Globals::CK2() * pow(Globals::AE(), 3.0);
c4_ = 2.0 * RecoveredMeanMotion() * coef1_ * RecoveredSemiMajorAxis() * betao2 * i_x1mth2_ = 1.0 - theta2;
(eta_ * (2.0 + 0.5 * etasq) + Eccentricity() * (0.5 + 2.0 * etasq) - i_c4_ = 2.0 * RecoveredMeanMotion() * coef1 * RecoveredSemiMajorAxis() * betao2 *
(i_eta_ * (2.0 + 0.5 * etasq) + Eccentricity() * (0.5 + 2.0 * etasq) -
2.0 * Globals::CK2() * tsi / (RecoveredSemiMajorAxis() * psisq) * 2.0 * Globals::CK2() * tsi / (RecoveredSemiMajorAxis() * psisq) *
(-3.0 * x3thm1_ * (1.0 - 2.0 * eeta + etasq * (-3.0 * i_x3thm1_ * (1.0 - 2.0 * eeta + etasq *
(1.5 - 0.5 * eeta)) + 0.75 * x1mth2_ * (2.0 * etasq - eeta * (1.5 - 0.5 * eeta)) + 0.75 * i_x1mth2_ * (2.0 * etasq - eeta *
(1.0 + etasq)) * cos(2.0 * ArgumentPerigee()))); (1.0 + etasq)) * cos(2.0 * ArgumentPerigee())));
double theta4 = theta2 * theta2; double theta4 = theta2 * theta2;
double temp1 = 3.0 * Globals::CK2() * pinvsq * RecoveredMeanMotion(); double temp1 = 3.0 * Globals::CK2() * pinvsq * RecoveredMeanMotion();
double temp2 = temp1 * Globals::CK2() * pinvsq; double temp2 = temp1 * Globals::CK2() * pinvsq;
double temp3 = 1.25 * Globals::CK4() * pinvsq * pinvsq * RecoveredMeanMotion(); double temp3 = 1.25 * Globals::CK4() * pinvsq * pinvsq * RecoveredMeanMotion();
xmdot_ = RecoveredMeanMotion() + 0.5 * temp1 * betao * x3thm1_ + 0.0625 * temp2 * betao * i_xmdot_ = RecoveredMeanMotion() + 0.5 * temp1 * betao *
i_x3thm1_ + 0.0625 * temp2 * betao *
(13.0 - 78.0 * theta2 + 137.0 * theta4); (13.0 - 78.0 * theta2 + 137.0 * theta4);
double x1m5th = 1.0 - 5.0 * theta2; double x1m5th = 1.0 - 5.0 * theta2;
omgdot_ = -0.5 * temp1 * x1m5th + 0.0625 * temp2 * (7.0 - 114.0 * theta2 + i_omgdot_ = -0.5 * temp1 * x1m5th +
395.0 * theta4) + temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4); 0.0625 * temp2 * (7.0 - 114.0 * theta2 + 395.0 * theta4) +
double xhdot1_ = -temp1 * cosio_; temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4);
xnodot_ = xhdot1_ + (0.5 * temp2 * (4.0 - 19.0 * theta2) + 2.0 * temp3 * double xhdot1_ = -temp1 * i_cosio_;
(3.0 - 7.0 * theta2)) * cosio_; i_xnodot_ = xhdot1_ + (0.5 * temp2 * (4.0 - 19.0 * theta2) + 2.0 * temp3 *
xnodcf_ = 3.5 * betao2 * xhdot1_ * c1_; (3.0 - 7.0 * theta2)) * i_cosio_;
t2cof_ = 1.5 * c1_; i_xnodcf_ = 3.5 * betao2 * xhdot1_ * i_c1_;
i_t2cof_ = 1.5 * i_c1_;
if (fabs(cosio_ + 1.0) > 1.5e-12) if (fabs(i_cosio_ + 1.0) > 1.5e-12)
xlcof_ = 0.125 * a3ovk2_ * sinio_ * (3.0 + 5.0 * cosio_) / (1.0 + cosio_); i_xlcof_ = 0.125 * i_a3ovk2_ * i_sinio_ * (3.0 + 5.0 * i_cosio_) / (1.0 + i_cosio_);
else else
xlcof_ = 0.125 * a3ovk2_ * sinio_ * (3.0 + 5.0 * cosio_) / 1.5e-12; i_xlcof_ = 0.125 * i_a3ovk2_ * i_sinio_ * (3.0 + 5.0 * i_cosio_) / 1.5e-12;
aycof_ = 0.25 * a3ovk2_ * sinio_; i_aycof_ = 0.25 * i_a3ovk2_ * i_sinio_;
x7thm1_ = 7.0 * theta2 - 1.0; i_x7thm1_ = 7.0 * theta2 - 1.0;
if (!use_deep_space_) { if (!i_use_deep_space_) {
double c3 = 0.0; double c3 = 0.0;
if (Eccentricity() > 1.0e-4) { if (Eccentricity() > 1.0e-4) {
c3 = coef * tsi * a3ovk2_ * RecoveredMeanMotion() * Globals::AE() * c3 = coef * tsi * i_a3ovk2_ * RecoveredMeanMotion() * Globals::AE() *
sinio_ / Eccentricity(); i_a3ovk2_ / Eccentricity();
} }
c5_ = 2.0 * coef1_ * RecoveredSemiMajorAxis() * betao2 * (1.0 + 2.75 * (etasq + eeta) + eeta * etasq); i_c5_ = 2.0 * coef1 * RecoveredSemiMajorAxis() * betao2 * (1.0 + 2.75 * (etasq + eeta) + eeta * etasq);
omgcof_ = BStar() * c3 * cos(ArgumentPerigee()); i_omgcof_ = BStar() * c3 * cos(ArgumentPerigee());
xmcof_ = 0.0; i_xmcof_ = 0.0;
if (Eccentricity() > 1.0e-4) if (Eccentricity() > 1.0e-4)
xmcof_ = -Globals::TOTHRD() * coef * BStar() * Globals::AE() / eeta; i_xmcof_ = -Globals::TOTHRD() * coef * BStar() * Globals::AE() / eeta;
delmo_ = pow(1.0 + eta_ * (cos(MeanAnomoly())), 3.0); i_delmo_ = pow(1.0 + i_eta_ * (cos(MeanAnomoly())), 3.0);
sinmo_ = sin(MeanAnomoly()); i_sinmo_ = sin(MeanAnomoly());
} }
if (!use_simple_model_) { if (!i_use_simple_model_) {
double c1sq = c1_ * c1_; double c1sq = i_c1_ * i_c1_;
d2_ = 4.0 * RecoveredSemiMajorAxis() * tsi * c1sq; i_d2_ = 4.0 * RecoveredSemiMajorAxis() * tsi * c1sq;
double temp = d2_ * tsi * c1_ / 3.0; double temp = i_d2_ * tsi * i_c1_ / 3.0;
d3_ = (17.0 * RecoveredSemiMajorAxis() + s4_) * temp; i_d3_ = (17.0 * RecoveredSemiMajorAxis() + s4) * temp;
d4_ = 0.5 * temp * RecoveredSemiMajorAxis() * tsi * (221.0 * RecoveredSemiMajorAxis() + 31.0 * s4_) * c1_; i_d4_ = 0.5 * temp * RecoveredSemiMajorAxis() *
t3cof_ = d2_ + 2.0 * c1sq; tsi * (221.0 * RecoveredSemiMajorAxis() + 31.0 * s4) * i_c1_;
t4cof_ = 0.25 * (3.0 * d3_ + c1_ * (12.0 * d2_ + 10.0 * c1sq)); i_t3cof_ = i_d2_ + 2.0 * c1sq;
t5cof_ = 0.2 * (3.0 * d4_ + 12.0 * c1_ * d3_ + 6.0 * d2_ * d2_ + 15.0 * i_t4cof_ = 0.25 * (3.0 * i_d3_ + i_c1_ *
c1sq * (2.0 * d2_ + c1sq)); (12.0 * i_d2_ + 10.0 * c1sq));
} else if (use_deep_space_) { i_t5cof_ = 0.2 * (3.0 * i_d4_ + 12.0 * i_c1_ *
gsto_ = Epoch().ToGMST(); i_d3_ + 6.0 * i_d2_ * i_d2_ + 15.0 *
c1sq * (2.0 * i_d2_ + c1sq));
} else if (i_use_deep_space_) {
i_gsto_ = Epoch().ToGMST();
double sing = sin(ArgumentPerigee()); double sing = sin(ArgumentPerigee());
double cosg = cos(ArgumentPerigee()); double cosg = cos(ArgumentPerigee());
DeepSpaceInitialize(eosq, sinio_, cosio_, betao, DeepSpaceInitialize(eosq, i_sinio_, i_cosio_, betao,
theta2, sing, cosg, betao2, theta2, sing, cosg, betao2,
xmdot_, omgdot_, xnodot_); i_xmdot_, i_omgdot_, i_xnodot_);
} }
first_run_ = false; first_run_ = false;
@ -211,6 +184,9 @@ void SGDP4::Initialize(const double& theta2, const double& betao2, const double&
void SGDP4::FindPosition(double tsince) { void SGDP4::FindPosition(double tsince) {
/*
* local copies which we can safely modify
*/
double tsince_eccentricity = Eccentricity(); double tsince_eccentricity = Eccentricity();
double tsince_arg_perigee = ArgumentPerigee(); double tsince_arg_perigee = ArgumentPerigee();
double tsince_inclination = Inclination(); double tsince_inclination = Inclination();
@ -222,19 +198,19 @@ void SGDP4::FindPosition(double tsince) {
/* /*
* update for secular gravity and atmospheric drag * update for secular gravity and atmospheric drag
*/ */
double xmdf = MeanAnomoly() + xmdot_ * tsince; double xmdf = MeanAnomoly() + i_xmdot_ * tsince;
double omgadf = ArgumentPerigee() + omgdot_ * tsince; double omgadf = ArgumentPerigee() + i_omgdot_ * tsince;
double xnoddf = tsince_ascending_node + xnodot_ * tsince; double xnoddf = tsince_ascending_node + i_xnodot_ * tsince;
double tsq = tsince * tsince; double tsq = tsince * tsince;
double xnode = xnoddf + xnodcf_ * tsq; double xnode = xnoddf + i_xnodcf_ * tsq;
double tempa = 1.0 - c1_ * tsince; double tempa = 1.0 - i_c1_ * tsince;
double tempe = BStar() * c4_ * tsince; double tempe = BStar() * i_c4_ * tsince;
double templ = t2cof_ * tsq; double templ = i_t2cof_ * tsq;
tsince_arg_perigee = omgadf; tsince_arg_perigee = omgadf;
if (use_deep_space_) { if (i_use_deep_space_) {
double xn = RecoveredMeanMotion(); double xn = RecoveredMeanMotion();
#if 0 #if 0
CALL DPSEC(xmdf, tsince_arg_perigee, XNODE, tle_data_tsince_.eo, tsince_inclination, xn, tsince); CALL DPSEC(xmdf, tsince_arg_perigee, XNODE, tle_data_tsince_.eo, tsince_inclination, xn, tsince);
@ -261,25 +237,25 @@ void SGDP4::FindPosition(double tsince) {
x7thm1_ = 7.0 * theta2 - 1.0; x7thm1_ = 7.0 * theta2 - 1.0;
if (fabs(cosio_ + 1.0) > 1.5e-12) if (fabs(cosio_ + 1.0) > 1.5e-12)
xlcof_ = 0.125 * a3ovk2_ * sinio_ * (3.0 + 5.0 * cosio_) / (1.0 + cosio_); xlcof_ = 0.125 * i_a3ovk2_ * sinio_ * (3.0 + 5.0 * cosio_) / (1.0 + cosio_);
else else
xlcof_ = 0.125 * a3ovk2_ * sinio_ * (3.0 + 5.0 * cosio_) / 1.5e-12; xlcof_ = 0.125 * i_a3ovk2_ * sinio_ * (3.0 + 5.0 * cosio_) / 1.5e-12;
aycof_ = 0.25 * a3ovk2_ * sinio_; aycof_ = 0.25 * i_a3ovk2_ * sinio_;
} else { } else {
double xmp = xmdf; double xmp = xmdf;
if (!use_simple_model_) { if (!i_use_simple_model_) {
double delomg = omgcof_ * tsince; double delomg = omgcof_ * tsince;
double delm = xmcof_ * (pow(1.0 + eta_ * cos(xmdf), 3.0) - delmo_); double delm = xmcof_ * (pow(1.0 + i_eta_ * cos(xmdf), 3.0) - delmo_);
double temp1 = delomg + delm; double temp1 = delomg + delm;
xmp = xmdf + temp1; xmp = xmdf + temp1;
tsince_arg_perigee -= temp1; tsince_arg_perigee -= temp1;
double tcube = tsq * tsince; double tcube = tsq * tsince;
double tfour = tsince * tcube; double tfour = tsince * tcube;
tempa -= d2_ * tsq - d3_ * tcube - d4_ * tfour; tempa -= i_d2_ * tsq - i_d3_ * tcube - i_d4_ * tfour;
tempe += BStar() * c5_ * (sin(xmp) - sinmo_); tempe += BStar() * i_c5_ * (sin(xmp) - sinmo_);
templ += t3cof_ * tcube + tfour * (t4cof_ + tsince * t5cof_); templ += i_t3cof_ * tcube + tfour * (i_t4cof_ + tsince * i_t5cof_);
} }
a = RecoveredSemiMajorAxis() * pow(tempa, 2.0); a = RecoveredSemiMajorAxis() * pow(tempa, 2.0);
tsince_eccentricity = Eccentricity() - tempe; tsince_eccentricity = Eccentricity() - tempe;
@ -718,7 +694,7 @@ void SGDP4::DeepSpaceInitialize(const double& eosq, const double& sinio, const d
d_d5421_ = temp * f542 * g521; d_d5421_ = temp * f542 * g521;
d_d5433_ = temp * f543 * g533; d_d5433_ = temp * f543 * g533;
d_xlamo_ = MeanAnomoly() + AscendingNode() + AscendingNode() - gsto_ - gsto_; d_xlamo_ = MeanAnomoly() + AscendingNode() + AscendingNode() - i_gsto_ - i_gsto_;
bfact = xmdot + xnodot + xnodot - THDT - THDT; bfact = xmdot + xnodot + xnodot - THDT - THDT;
bfact = bfact + d_ssl_ + d_ssh_ + d_ssh_; bfact = bfact + d_ssl_ + d_ssh_ + d_ssh_;
} }
@ -744,7 +720,7 @@ void SGDP4::DeepSpaceInitialize(const double& eosq, const double& sinio, const d
d_fasx4_ = 2.8843198; d_fasx4_ = 2.8843198;
d_fasx6_ = 0.37448087; d_fasx6_ = 0.37448087;
d_xlamo_ = MeanAnomoly() + AscendingNode() + ArgumentPerigee() - gsto_; d_xlamo_ = MeanAnomoly() + AscendingNode() + ArgumentPerigee() - i_gsto_;
bfact = xmdot + xpidot - THDT; bfact = xmdot + xpidot - THDT;
bfact = bfact + d_ssl_ + d_ssg_ + d_ssh_; bfact = bfact + d_ssl_ + d_ssg_ + d_ssh_;
} }

72
SGDP4.h
View File

@ -21,42 +21,40 @@ private:
bool first_run_; bool first_run_;
double cosio_; /*
double sinio_; * i_ variables are constants that wont be modified outside Init
double x3thm1_; */
double i_cosio_;
double i_sinio_;
double i_x3thm1_;
double i_eta_;
double i_c1_;
double i_a3ovk2_;
double i_x1mth2_;
double i_c4_;
double i_c5_;
double i_xmdot_;
double i_omgdot_;
double i_xnodot_;
double i_xnodcf_;
double i_t2cof_;
double i_xlcof_;
double i_aycof_;
double i_x7thm1_;
double i_omgcof_;
double i_xmcof_;
double i_delmo_;
double i_sinmo_;
double i_d2_;
double i_d3_;
double i_d4_;
double i_t3cof_;
double i_t4cof_;
double i_t5cof_;
double i_gsto_;
double eta_; bool i_use_simple_model_;
double coef1_; bool i_use_deep_space_;
double c1_;
double a3ovk2_;
double x1mth2_;
double c4_;
double c5_;
double xmdot_;
double omgdot_;
double xnodot_;
double xnodcf_;
double t2cof_;
double xlcof_;
double aycof_;
double x7thm1_;
double omgcof_;
double xmcof_;
double delmo_;
double sinmo_;
double d2_;
double d3_;
double d4_;
double t3cof_;
double t4cof_;
double t5cof_;
double gsto_;
bool use_simple_model_;
bool use_deep_space_;
/* /*
* XMO * XMO
@ -139,6 +137,10 @@ private:
return epoch_; return epoch_;
} }
/*
* these variables are set at the very start
* and should not be changed after that
*/
double mean_anomoly_; double mean_anomoly_;
double ascending_node_; double ascending_node_;
double argument_perigee_; double argument_perigee_;