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

Improve DeepSpacePeriodics.

feature/19
Daniel Warner 2018-08-27 16:40:00 +01:00 committed by Daniel Warner
parent a7d72d70e4
commit 6982421275
2 changed files with 207 additions and 344 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

468
libsgp4/SGP4.cc
View File

@ -257,7 +257,17 @@ Eci SGP4::FindPositionSDP4(double tsince) const
double em = elements_.Eccentricity(); double em = elements_.Eccentricity();
xinc = elements_.Inclination(); xinc = elements_.Inclination();
DeepSpaceSecular(tsince, xmdf, omgadf, xnode, em, xinc, xn); DeepSpaceSecular(tsince,
elements_,
common_consts_,
deepspace_consts_,
integrator_params_,
xmdf,
omgadf,
xnode,
em,
xinc,
xn);
if (xn <= 0.0) if (xn <= 0.0)
{ {
@ -268,7 +278,13 @@ Eci SGP4::FindPositionSDP4(double tsince) const
e = em - tempe; e = em - tempe;
double xmam = xmdf + elements_.RecoveredMeanMotion() * templ; double xmam = xmdf + elements_.RecoveredMeanMotion() * templ;
DeepSpacePeriodics(tsince, e, xinc, omgadf, xnode, xmam); DeepSpacePeriodics(tsince,
deepspace_consts_,
e,
xinc,
omgadf,
xnode,
xmam);
/* /*
* keeping xinc positive important unless you need to display xinc * keeping xinc positive important unless you need to display xinc
@ -651,7 +667,7 @@ static inline double EvaluateCubicPolynomial(
const double squared, const double squared,
const double cubed) const double cubed)
{ {
return constant + x * (linear + x * (squared + x * cubed)); return constant + x * linear + x * x * squared + x * x * x * cubed;
} }
void SGP4::DeepSpaceInitialise( void SGP4::DeepSpaceInitialise(
@ -867,9 +883,7 @@ void SGP4::DeepSpaceInitialise(
deepspace_consts_.ssg += sgh - cosio * shdq; deepspace_consts_.ssg += sgh - cosio * shdq;
deepspace_consts_.ssh += shdq; deepspace_consts_.ssh += shdq;
deepspace_consts_.resonance_flag = false; deepspace_consts_.shape = DeepSpaceConstants::NONE;
deepspace_consts_.synchronous_flag = false;
bool initialise_integrator = true;
if (elements_.RecoveredMeanMotion() < 0.0052359877 if (elements_.RecoveredMeanMotion() < 0.0052359877
&& elements_.RecoveredMeanMotion() > 0.0034906585) && elements_.RecoveredMeanMotion() > 0.0034906585)
@ -877,8 +891,7 @@ void SGP4::DeepSpaceInitialise(
/* /*
* 24h synchronous resonance terms initialisation * 24h synchronous resonance terms initialisation
*/ */
deepspace_consts_.resonance_flag = true; deepspace_consts_.shape = DeepSpaceConstants::SYNCHRONOUS;
deepspace_consts_.synchronous_flag = true;
const double g200 = 1.0 + eosq * (-2.5 + 0.8125 * eosq); const double g200 = 1.0 + eosq * (-2.5 + 0.8125 * eosq);
const double g310 = 1.0 + 2.0 * eosq; const double g310 = 1.0 + 2.0 * eosq;
@ -898,7 +911,7 @@ void SGP4::DeepSpaceInitialise(
deepspace_consts_.del1 = deepspace_consts_.del1 deepspace_consts_.del1 = deepspace_consts_.del1
* f311 * g310 * Q31 * aqnv; * f311 * g310 * Q31 * aqnv;
integrator_consts_.xlamo = Util::WrapTwoPI(elements_.MeanAnomoly() deepspace_consts_.xlamo = Util::WrapTwoPI(elements_.MeanAnomoly()
+ elements_.AscendingNode() + elements_.AscendingNode()
+ elements_.ArgumentPerigee() + elements_.ArgumentPerigee()
- deepspace_consts_.gsto); - deepspace_consts_.gsto);
@ -911,14 +924,14 @@ void SGP4::DeepSpaceInitialise(
|| elements_.RecoveredMeanMotion() > 9.24e-3 || elements_.RecoveredMeanMotion() > 9.24e-3
|| elements_.Eccentricity() < 0.5) || elements_.Eccentricity() < 0.5)
{ {
initialise_integrator = false; // do nothing
} }
else else
{ {
/* /*
* geopotential resonance initialisation for 12 hour orbits * geopotential resonance initialisation for 12 hour orbits
*/ */
deepspace_consts_.resonance_flag = true; deepspace_consts_.shape = DeepSpaceConstants::RESONANCE;
double g211; double g211;
double g310; double g310;
@ -932,7 +945,7 @@ void SGP4::DeepSpaceInitialise(
if (elements_.Eccentricity() <= 0.65) if (elements_.Eccentricity() <= 0.65)
{ {
g211 = EvaluateCubicPolynomial(elements_.Eccentricity(), g211 = EvaluateCubicPolynomial(elements_.Eccentricity(),
3.616, -13.247, +16.290, 0.0); 3.616, -13.247, 16.290, 0.0);
g310 = EvaluateCubicPolynomial(elements_.Eccentricity(), g310 = EvaluateCubicPolynomial(elements_.Eccentricity(),
-19.302, 117.390, -228.419, 156.591); -19.302, 117.390, -228.419, 156.591);
g322 = EvaluateCubicPolynomial(elements_.Eccentricity(), g322 = EvaluateCubicPolynomial(elements_.Eccentricity(),
@ -1038,7 +1051,7 @@ void SGP4::DeepSpaceInitialise(
deepspace_consts_.d5421 = temp * f542 * g521; deepspace_consts_.d5421 = temp * f542 * g521;
deepspace_consts_.d5433 = temp * f543 * g533; deepspace_consts_.d5433 = temp * f543 * g533;
integrator_consts_.xlamo = Util::WrapTwoPI( deepspace_consts_.xlamo = Util::WrapTwoPI(
elements_.MeanAnomoly() elements_.MeanAnomoly()
+ elements_.AscendingNode() + elements_.AscendingNode()
+ elements_.AscendingNode() + elements_.AscendingNode()
@ -1052,111 +1065,83 @@ void SGP4::DeepSpaceInitialise(
+ deepspace_consts_.ssh; + deepspace_consts_.ssh;
} }
if (initialise_integrator) if (deepspace_consts_.shape != DeepSpaceConstants::NONE)
{ {
/* /*
* initialise integrator * initialise integrator
*/ */
integrator_consts_.xfact = bfact - elements_.RecoveredMeanMotion(); deepspace_consts_.xfact = bfact - elements_.RecoveredMeanMotion();
integrator_params_.atime = 0.0; integrator_params_.atime = 0.0;
integrator_params_.xni = elements_.RecoveredMeanMotion(); integrator_params_.xni = elements_.RecoveredMeanMotion();
integrator_params_.xli = integrator_consts_.xlamo; integrator_params_.xli = deepspace_consts_.xlamo;
/*
* precompute dot terms for epoch
*/
DeepSpaceCalcDotTerms(integrator_consts_.values_0);
} }
} }
void SGP4::DeepSpaceCalculateLunarSolarTerms( /**
* From DeepSpaceConstants, this uses:
* zmos, se2, se3, si2, si3, sl2, sl3, sl4, sgh2, sgh3, sgh4, sh2, sh3
* zmol, ee2, e3, xi2, xi3, xl2, xl3, xl4, xgh2, xgh3, xgh4, xh2, xh3
*/
void SGP4::DeepSpacePeriodics(
const double tsince, const double tsince,
double& pe, const DeepSpaceConstants& ds_constants,
double& pinc, double& em,
double& pl, double& xinc,
double& pgh, double& omgasm,
double& ph) const double& xnodes,
double& xll)
{ {
static const double ZES = 0.01675; static const double ZES = 0.01675;
static const double ZNS = 1.19459E-5; static const double ZNS = 1.19459E-5;
static const double ZNL = 1.5835218E-4; static const double ZNL = 1.5835218E-4;
static const double ZEL = 0.05490; static const double ZEL = 0.05490;
/* // calculate solar terms for time tsince
* calculate solar terms for time tsince double zm = ds_constants.zmos + ZNS * tsince;
*/
double zm = deepspace_consts_.zmos + ZNS * tsince;
double zf = zm + 2.0 * ZES * sin(zm); double zf = zm + 2.0 * ZES * sin(zm);
double sinzf = sin(zf); double sinzf = sin(zf);
double f2 = 0.5 * sinzf * sinzf - 0.25; double f2 = 0.5 * sinzf * sinzf - 0.25;
double f3 = -0.5 * sinzf * cos(zf); double f3 = -0.5 * sinzf * cos(zf);
const double ses = deepspace_consts_.se2 * f2 const double ses = ds_constants.se2 * f2
+ deepspace_consts_.se3 * f3; + ds_constants.se3 * f3;
const double sis = deepspace_consts_.si2 * f2 const double sis = ds_constants.si2 * f2
+ deepspace_consts_.si3 * f3; + ds_constants.si3 * f3;
const double sls = deepspace_consts_.sl2 * f2 const double sls = ds_constants.sl2 * f2
+ deepspace_consts_.sl3 * f3 + ds_constants.sl3 * f3
+ deepspace_consts_.sl4 * sinzf; + ds_constants.sl4 * sinzf;
const double sghs = deepspace_consts_.sgh2 * f2 const double sghs = ds_constants.sgh2 * f2
+ deepspace_consts_.sgh3 * f3 + ds_constants.sgh3 * f3
+ deepspace_consts_.sgh4 * sinzf; + ds_constants.sgh4 * sinzf;
const double shs = deepspace_consts_.sh2 * f2 const double shs = ds_constants.sh2 * f2
+ deepspace_consts_.sh3 * f3; + ds_constants.sh3 * f3;
/* // calculate lunar terms for time tsince
* calculate lunar terms for time tsince zm = ds_constants.zmol + ZNL * tsince;
*/
zm = deepspace_consts_.zmol + ZNL * tsince;
zf = zm + 2.0 * ZEL * sin(zm); zf = zm + 2.0 * ZEL * sin(zm);
sinzf = sin(zf); sinzf = sin(zf);
f2 = 0.5 * sinzf * sinzf - 0.25; f2 = 0.5 * sinzf * sinzf - 0.25;
f3 = -0.5 * sinzf * cos(zf); f3 = -0.5 * sinzf * cos(zf);
const double sel = deepspace_consts_.ee2 * f2 const double sel = ds_constants.ee2 * f2
+ deepspace_consts_.e3 * f3; + ds_constants.e3 * f3;
const double sil = deepspace_consts_.xi2 * f2 const double sil = ds_constants.xi2 * f2
+ deepspace_consts_.xi3 * f3; + ds_constants.xi3 * f3;
const double sll = deepspace_consts_.xl2 * f2 const double sll = ds_constants.xl2 * f2
+ deepspace_consts_.xl3 * f3 + ds_constants.xl3 * f3
+ deepspace_consts_.xl4 * sinzf; + ds_constants.xl4 * sinzf;
const double sghl = deepspace_consts_.xgh2 * f2 const double sghl = ds_constants.xgh2 * f2
+ deepspace_consts_.xgh3 * f3 + ds_constants.xgh3 * f3
+ deepspace_consts_.xgh4 * sinzf; + ds_constants.xgh4 * sinzf;
const double shl = deepspace_consts_.xh2 * f2 const double shl = ds_constants.xh2 * f2
+ deepspace_consts_.xh3 * f3; + ds_constants.xh3 * f3;
/* // merge calculated values
* merge calculated values const double pe = ses + sel;
*/ const double pinc = sis + sil;
pe = ses + sel; const double pl = sls + sll;
pinc = sis + sil; const double pgh = sghs + sghl;
pl = sls + sll; const double ph = shs + shl;
pgh = sghs + sghl;
ph = shs + shl;
}
void SGP4::DeepSpacePeriodics(
const double tsince,
double& em,
double& xinc,
double& omgasm,
double& xnodes,
double& xll) const
{
/*
* storage for lunar / solar terms
* set by DeepSpaceCalculateLunarSolarTerms()
*/
double pe = 0.0;
double pinc = 0.0;
double pl = 0.0;
double pgh = 0.0;
double ph = 0.0;
/*
* calculate lunar / solar terms for current time
*/
DeepSpaceCalculateLunarSolarTerms(tsince, pe, pinc, pl, pgh, ph);
xinc += pinc; xinc += pinc;
em += pe; em += pe;
@ -1175,48 +1160,29 @@ void SGP4::DeepSpacePeriodics(
if (xinc >= 0.2) if (xinc >= 0.2)
{ {
/* // apply periodics directly
* apply periodics directly omgasm += pgh - cosis * ph / sinis;
*/ xnodes += ph / sinis;
ph /= sinis;
omgasm += pgh - cosis * ph;
xnodes += ph;
xll += pl; xll += pl;
} }
else else
{ {
/* // apply periodics with lyddane modification
* apply periodics with lyddane modification
*/
const double sinok = sin(xnodes); const double sinok = sin(xnodes);
const double cosok = cos(xnodes); const double cosok = cos(xnodes);
double alfdp = sinis * sinok; double alfdp = sinis * sinok;
double betdp = sinis * cosok; double betdp = sinis * cosok;
const double dalf = ph * cosok + pinc * cosis * sinok; const double dalf = ph * cosok + pinc * cosis * sinok;
const double dbet = -ph * sinok + pinc * cosis * cosok; const double dbet = -ph * sinok + pinc * cosis * cosok;
alfdp += dalf; alfdp += dalf;
betdp += dbet; betdp += dbet;
xnodes = Util::WrapTwoPI(xnodes); xnodes = Util::WrapTwoPI(xnodes);
double xls = xll + omgasm + cosis * xnodes; double xls = xll + omgasm + cosis * xnodes;
double dls = pl + pgh - pinc * xnodes * sinis; double dls = pl + pgh - pinc * xnodes * sinis;
xls += dls; xls += dls;
/*
* save old xnodes value
*/
const double oldxnodes = xnodes; const double oldxnodes = xnodes;
xnodes = atan2(alfdp, betdp); xnodes = atan2(alfdp, betdp);
if (xnodes < 0.0) /**
{
xnodes += kTWOPI;
}
/*
* Get perturbed xnodes in to same quadrant as original. * Get perturbed xnodes in to same quadrant as original.
* RAAN is in the range of 0 to 360 degrees * RAAN is in the range of 0 to 360 degrees
* atan2 is in the range of -180 to 180 degrees * atan2 is in the range of -180 to 180 degrees
@ -1240,106 +1206,16 @@ void SGP4::DeepSpacePeriodics(
void SGP4::DeepSpaceSecular( void SGP4::DeepSpaceSecular(
const double tsince, const double tsince,
const OrbitalElements& elements,
const CommonConstants& c_constants,
const DeepSpaceConstants& ds_constants,
IntegratorParams& integ_params,
double& xll, double& xll,
double& omgasm, double& omgasm,
double& xnodes, double& xnodes,
double& em, double& em,
double& xinc, double& xinc,
double& xn) const double& xn)
{
static const double STEP = 720.0;
static const double STEP2 = 259200.0;
xll += deepspace_consts_.ssl * tsince;
omgasm += deepspace_consts_.ssg * tsince;
xnodes += deepspace_consts_.ssh * tsince;
em += deepspace_consts_.sse * tsince;
xinc += deepspace_consts_.ssi * tsince;
if (deepspace_consts_.resonance_flag)
{
/*
* 1st condition (if tsince is less than one time step from epoch)
* 2nd condition (if integrator_params_.atime and
* tsince are of opposite signs, so zero crossing required)
* 3rd condition (if tsince is closer to zero than
* integrator_params_.atime, only integrate away from zero)
*/
if (fabs(tsince) < STEP ||
tsince * integrator_params_.atime <= 0.0 ||
fabs(tsince) < fabs(integrator_params_.atime))
{
/*
* restart from epoch
*/
integrator_params_.atime = 0.0;
integrator_params_.xni = elements_.RecoveredMeanMotion();
integrator_params_.xli = integrator_consts_.xlamo;
/*
* restore precomputed values for epoch
*/
integrator_params_.values_t = integrator_consts_.values_0;
}
double ft = tsince - integrator_params_.atime;
/*
* if time difference (ft) is greater than the time step (720.0)
* loop around until integrator_params_.atime is within one time step of
* tsince
*/
if (fabs(ft) >= STEP)
{
/*
* calculate step direction to allow integrator_params_.atime
* to catch up with tsince
*/
double delt = -STEP;
if (ft >= 0.0)
{
delt = STEP;
}
do
{
/*
* integrate using current dot terms
*/
DeepSpaceIntegrator(delt, STEP2, integrator_params_.values_t);
/*
* calculate dot terms for next integration
*/
DeepSpaceCalcDotTerms(integrator_params_.values_t);
ft = tsince - integrator_params_.atime;
} while (fabs(ft) >= STEP);
}
/*
* integrator
*/
xn = integrator_params_.xni
+ integrator_params_.values_t.xndot * ft
+ integrator_params_.values_t.xnddt * ft * ft * 0.5;
const double xl = integrator_params_.xli
+ integrator_params_.values_t.xldot * ft
+ integrator_params_.values_t.xndot * ft * ft * 0.5;
const double temp = -xnodes + deepspace_consts_.gsto + tsince * kTHDT;
if (deepspace_consts_.synchronous_flag)
{
xll = xl + temp - omgasm;
}
else
{
xll = xl + temp + temp;
}
}
}
void SGP4::DeepSpaceCalcDotTerms(struct IntegratorValues& values) const
{ {
static const double G22 = 5.7686396; static const double G22 = 5.7686396;
static const double G32 = 0.95240898; static const double G32 = 0.95240898;
@ -1350,89 +1226,114 @@ void SGP4::DeepSpaceCalcDotTerms(struct IntegratorValues& values) const
static const double FASX4 = 2.8843198; static const double FASX4 = 2.8843198;
static const double FASX6 = 0.37448087; static const double FASX6 = 0.37448087;
if (deepspace_consts_.synchronous_flag) static const double STEP = 720.0;
{ static const double STEP2 = 259200.0;
values.xndot = deepspace_consts_.del1 xll += ds_constants.ssl * tsince;
* sin(integrator_params_.xli - FASX2) omgasm += ds_constants.ssg * tsince;
+ deepspace_consts_.del2 xnodes += ds_constants.ssh * tsince;
* sin(2.0 * (integrator_params_.xli - FASX4)) em += ds_constants.sse * tsince;
+ deepspace_consts_.del3 xinc += ds_constants.ssi * tsince;
* sin(3.0 * (integrator_params_.xli - FASX6));
values.xnddt = deepspace_consts_.del1 if (ds_constants.shape != DeepSpaceConstants::NONE)
* cos(integrator_params_.xli - FASX2) {
+ 2.0 * deepspace_consts_.del2 double xndot = 0.0;
* cos(2.0 * (integrator_params_.xli - FASX4)) double xnddt = 0.0;
+ 3.0 * deepspace_consts_.del3 double xldot = 0.0;
* cos(3.0 * (integrator_params_.xli - FASX6)); /*
* 1st condition (if tsince is less than one time step from epoch)
* 2nd condition (if atime and
* tsince are of opposite signs, so zero crossing required)
* 3rd condition (if tsince is closer to zero than
* atime, only integrate away from zero)
*/
if (fabs(tsince) < STEP ||
tsince * integ_params.atime <= 0.0 ||
fabs(tsince) < fabs(integ_params.atime))
{
// restart back at the epoch
integ_params.atime = 0.0;
// TODO: check
integ_params.xni = elements.RecoveredMeanMotion();
// TODO: check
integ_params.xli = ds_constants.xlamo;
}
bool running = true;
while (running)
{
// always calculate dot terms ready for integration beginning
// from the start of the range which is 'atime'
if (ds_constants.shape == DeepSpaceConstants::SYNCHRONOUS)
{
xndot = ds_constants.del1 * sin(integ_params.xli - FASX2)
+ ds_constants.del2 * sin(2.0 * (integ_params.xli - FASX4))
+ ds_constants.del3 * sin(3.0 * (integ_params.xli - FASX6));
xnddt = ds_constants.del1 * cos(integ_params.xli - FASX2)
+ 2.0 * ds_constants.del2 * cos(2.0 * (integ_params.xli - FASX4))
+ 3.0 * ds_constants.del3 * cos(3.0 * (integ_params.xli - FASX6));
} }
else else
{ {
const double xomi = elements_.ArgumentPerigee() // TODO: check
+ common_consts_.omgdot * integrator_params_.atime; const double xomi = elements.ArgumentPerigee() + c_constants.omgdot * integ_params.atime;
const double x2omi = xomi + xomi; const double x2omi = xomi + xomi;
const double x2li = integrator_params_.xli + integrator_params_.xli; const double x2li = integ_params.xli + integ_params.xli;
xndot = ds_constants.d2201 * sin(x2omi + integ_params.xli - G22)
values.xndot = deepspace_consts_.d2201 + ds_constants.d2211 * sin(integ_params.xli - G22)
* sin(x2omi + integrator_params_.xli - G22) + ds_constants.d3210 * sin(xomi + integ_params.xli - G32)
* + deepspace_consts_.d2211 + ds_constants.d3222 * sin(-xomi + integ_params.xli - G32)
* sin(integrator_params_.xli - G22) + ds_constants.d4410 * sin(x2omi + x2li - G44)
+ deepspace_consts_.d3210 + ds_constants.d4422 * sin(x2li - G44)
* sin(xomi + integrator_params_.xli - G32) + ds_constants.d5220 * sin(xomi + integ_params.xli - G52)
+ deepspace_consts_.d3222 + ds_constants.d5232 * sin(-xomi + integ_params.xli - G52)
* sin(-xomi + integrator_params_.xli - G32) + ds_constants.d5421 * sin(xomi + x2li - G54)
+ deepspace_consts_.d4410 + ds_constants.d5433 * sin(-xomi + x2li - G54);
* sin(x2omi + x2li - G44) xnddt = ds_constants.d2201 * cos(x2omi + integ_params.xli - G22)
+ deepspace_consts_.d4422 + ds_constants.d2211 * cos(integ_params.xli - G22)
* sin(x2li - G44) + ds_constants.d3210 * cos(xomi + integ_params.xli - G32)
+ deepspace_consts_.d5220 + ds_constants.d3222 * cos(-xomi + integ_params.xli - G32)
* sin(xomi + integrator_params_.xli - G52) + ds_constants.d5220 * cos(xomi + integ_params.xli - G52)
+ deepspace_consts_.d5232 + ds_constants.d5232 * cos(-xomi + integ_params.xli - G52)
* sin(-xomi + integrator_params_.xli - G52) + 2.0 * (ds_constants.d4410 * cos(x2omi + x2li - G44)
+ deepspace_consts_.d5421 + ds_constants.d4422 * cos(x2li - G44)
* sin(xomi + x2li - G54) + ds_constants.d5421 * cos(xomi + x2li - G54)
+ deepspace_consts_.d5433 + ds_constants.d5433 * cos(-xomi + x2li - G54));
* sin(-xomi + x2li - G54);
values.xnddt = deepspace_consts_.d2201
* cos(x2omi + integrator_params_.xli - G22)
+ deepspace_consts_.d2211
* cos(integrator_params_.xli - G22)
+ deepspace_consts_.d3210
* cos(xomi + integrator_params_.xli - G32)
+ deepspace_consts_.d3222
* cos(-xomi + integrator_params_.xli - G32)
+ deepspace_consts_.d5220
* cos(xomi + integrator_params_.xli - G52)
+ deepspace_consts_.d5232
* cos(-xomi + integrator_params_.xli - G52)
+ 2.0 * (deepspace_consts_.d4410 * cos(x2omi + x2li - G44)
+ deepspace_consts_.d4422
* cos(x2li - G44)
+ deepspace_consts_.d5421
* cos(xomi + x2li - G54)
+ deepspace_consts_.d5433
* cos(-xomi + x2li - G54));
} }
xldot = integ_params.xni + ds_constants.xfact;
xnddt *= xldot;
values.xldot = integrator_params_.xni + integrator_consts_.xfact; double ft = tsince - integ_params.atime;
values.xnddt *= values.xldot; if (fabs(ft) >= STEP)
}
void SGP4::DeepSpaceIntegrator(
const double delt,
const double step2,
const struct IntegratorValues &values) const
{ {
/* const double delt = (ft >= 0.0 ? STEP : -STEP);
* integrator // integrate by a full step ('delt'), updating the cached
*/ // values for the new 'atime'
integrator_params_.xli += values.xldot * delt + values.xndot * step2; integ_params.xli = integ_params.xli + xldot * delt + xndot * STEP2;
integrator_params_.xni += values.xndot * delt + values.xnddt * step2; integ_params.xni = integ_params.xni + xndot * delt + xnddt * STEP2;
integ_params.atime += delt;
}
else
{
// integrate by the difference 'ft' remaining
xn = integ_params.xni + xndot * ft
+ xnddt * ft * ft * 0.5;
const double xl_temp = integ_params.xli + xldot * ft
+ xndot * ft * ft * 0.5;
/* const double theta = Util::WrapTwoPI(ds_constants.gsto + tsince * kTHDT);
* increment integrator time if (ds_constants.shape == DeepSpaceConstants::SYNCHRONOUS)
*/ {
integrator_params_.atime += delt; xll = xl_temp + theta - xnodes - omgasm;
}
else
{
xll = xl_temp + 2.0 * (theta - xnodes);
}
running = false;
}
}
}
} }
void SGP4::Reset() void SGP4::Reset()
@ -1443,6 +1344,5 @@ void SGP4::Reset()
std::memset(&common_consts_, 0, sizeof(common_consts_)); std::memset(&common_consts_, 0, sizeof(common_consts_));
std::memset(&nearspace_consts_, 0, sizeof(nearspace_consts_)); std::memset(&nearspace_consts_, 0, sizeof(nearspace_consts_));
std::memset(&deepspace_consts_, 0, sizeof(deepspace_consts_)); std::memset(&deepspace_consts_, 0, sizeof(deepspace_consts_));
std::memset(&integrator_consts_, 0, sizeof(integrator_consts_));
std::memset(&integrator_params_, 0, sizeof(integrator_params_)); std::memset(&integrator_params_, 0, sizeof(integrator_params_));
} }

71
libsgp4/SGP4.h
View File

@ -140,37 +140,18 @@ private:
double del1; double del1;
double del2; double del2;
double del3; double del3;
/*
* whether the deep space orbit is
* geopotential resonance for 12 hour orbits
*/
bool resonance_flag;
/*
* whether the deep space orbit is
* 24h synchronous resonance
*/
bool synchronous_flag;
};
struct IntegratorValues
{
double xndot;
double xnddt;
double xldot;
};
struct IntegratorConstants
{
/* /*
* integrator constants * integrator constants
*/ */
double xfact; double xfact;
double xlamo; double xlamo;
/* enum TOrbitShape
* integrator values for epoch {
*/ NONE,
struct IntegratorValues values_0; RESONANCE,
SYNCHRONOUS
} shape;
}; };
struct IntegratorParams struct IntegratorParams
@ -181,10 +162,6 @@ private:
double xli; double xli;
double xni; double xni;
double atime; double atime;
/*
* itegrator values for current d_atime_
*/
struct IntegratorValues values_t;
}; };
void Initialise(); void Initialise();
@ -226,49 +203,36 @@ private:
const double xmdot, const double xmdot,
const double omgdot, const double omgdot,
const double xnodot); const double xnodot);
/*
* Calculate lunar / solar terms
*/
void DeepSpaceCalculateLunarSolarTerms(
const double tsince,
double& pe,
double& pinc,
double& pl,
double& pgh,
double& ph) const;
/** /**
* Calculate lunar / solar periodics and apply * Calculate lunar / solar periodics and apply
*/ */
void DeepSpacePeriodics( static void DeepSpacePeriodics(
const double tsince, const double tsince,
const DeepSpaceConstants& ds_constants,
double& em, double& em,
double& xinc, double& xinc,
double& omgasm, double& omgasm,
double& xnodes, double& xnodes,
double& xll) const; double& xll);
/** /**
* Deep space secular effects * Deep space secular effects
*/ */
void DeepSpaceSecular( static void DeepSpaceSecular(
const double tsince, const double tsince,
const OrbitalElements& elements,
const CommonConstants& c_constants,
const DeepSpaceConstants& ds_constants,
IntegratorParams& integ_params,
double& xll, double& xll,
double& omgasm, double& omgasm,
double& xnodes, double& xnodes,
double& em, double& em,
double& xinc, double& xinc,
double& xn) const; double& xn);
/** /**
* Calculate dot terms * Reset
* @param[in,out] values the integrator values
*/ */
void DeepSpaceCalcDotTerms(struct IntegratorValues& values) const;
/**
* Deep space integrator for time period of delt
*/
void DeepSpaceIntegrator(
const double delt,
const double step2,
const struct IntegratorValues& values) const;
void Reset(); void Reset();
/* /*
@ -277,7 +241,6 @@ private:
struct CommonConstants common_consts_; struct CommonConstants common_consts_;
struct NearSpaceConstants nearspace_consts_; struct NearSpaceConstants nearspace_consts_;
struct DeepSpaceConstants deepspace_consts_; struct DeepSpaceConstants deepspace_consts_;
struct IntegratorConstants integrator_consts_;
mutable struct IntegratorParams integrator_params_; mutable struct IntegratorParams integrator_params_;
/* /*