Daniel Warner
parent
3fa30998bc
commit
90d6800594
|
|
@ -78,6 +78,7 @@
|
|||
<ClCompile Include="Globals.cpp" />
|
||||
<ClCompile Include="Julian.cpp" />
|
||||
<ClCompile Include="main.cpp" />
|
||||
<ClCompile Include="SGDP4.cpp" />
|
||||
<ClCompile Include="Tle.cpp" />
|
||||
<ClCompile Include="Vector.cpp" />
|
||||
</ItemGroup>
|
||||
|
|
@ -85,6 +86,8 @@
|
|||
<ClInclude Include="Coord.h" />
|
||||
<ClInclude Include="Globals.h" />
|
||||
<ClInclude Include="Julian.h" />
|
||||
<ClInclude Include="SatelliteException.h" />
|
||||
<ClInclude Include="SGDP4.h" />
|
||||
<ClInclude Include="Tle.h" />
|
||||
<ClInclude Include="Vector.h" />
|
||||
</ItemGroup>
|
||||
|
|
|
|||
|
|
@ -33,6 +33,9 @@
|
|||
<ClCompile Include="Tle.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="SGDP4.cpp">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="Coord.h">
|
||||
|
|
@ -50,5 +53,11 @@
|
|||
<ClInclude Include="Tle.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="SGDP4.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="SatelliteException.h">
|
||||
<Filter>Header Files</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
|
|
@ -0,0 +1,225 @@
|
|||
#include "SGDP4.h"
|
||||
|
||||
#include "SatelliteException.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
SGDP4::SGDP4(void) {
|
||||
first_run_ = true;
|
||||
}
|
||||
|
||||
SGDP4::~SGDP4(void) {
|
||||
}
|
||||
|
||||
void SGDP4::Initialize(const Tle& tle) {
|
||||
/*
|
||||
* extract and format tle data
|
||||
*/
|
||||
tle_data_.bstar = tle.GetField(Tle::FLD_BSTAR);
|
||||
tle_data_.eo = tle.GetField(Tle::FLD_E);
|
||||
tle_data_.omega = tle.GetField(Tle::FLD_ARGPER, Tle::U_RAD);
|
||||
tle_data_.xincl = tle.GetField(Tle::FLD_I, Tle::U_RAD);
|
||||
tle_data_.xmo = tle.GetField(Tle::FLD_M, Tle::U_RAD);
|
||||
tle_data_.xno = tle.GetField(Tle::FLD_MMOTION) / (1440.0 / Globals::TWOPI());
|
||||
tle_data_.xnodeo = tle.GetField(Tle::FLD_RAAN, Tle::U_RAD);
|
||||
|
||||
/*
|
||||
* generate julian date for tle epoch
|
||||
*/
|
||||
int year = static_cast<int> (tle.GetField(Tle::FLD_EPOCHYEAR));
|
||||
if (year < 57)
|
||||
year += 2000;
|
||||
else
|
||||
year += 1900;
|
||||
double day = tle.GetField(Tle::FLD_EPOCHDAY);
|
||||
Julian jul(year, day);
|
||||
tle_data_.epoch = jul;
|
||||
|
||||
double cosio_ = 0.0;
|
||||
double sinio_ = 0.0;
|
||||
double betao2_ = 0.0;
|
||||
double betao_ = 0.0;
|
||||
double xnodp_ = 0.0;
|
||||
double aodp_ = 0.0;
|
||||
|
||||
double gsto = 0.0;
|
||||
|
||||
bool use_simple_model_ = false;
|
||||
bool use_deep_space_ = false;
|
||||
|
||||
/*
|
||||
* recover original mean motion (xnodp) and semimajor axis (aodp)
|
||||
* from input elements
|
||||
*/
|
||||
double a1 = pow(Globals::XKE() / tle_data_.xno, Globals::TOTHRD());
|
||||
cosio_ = cos(tle_data_.xincl);
|
||||
sinio_ = sin(tle_data_.xincl);
|
||||
double theta2 = cosio_ * cosio_;
|
||||
double x3thm1 = 3.0 * theta2 - 1.0;
|
||||
double eosq = tle_data_.eo * tle_data_.eo;
|
||||
betao2_ = 1.0 - eosq;
|
||||
betao_ = sqrt(betao2_);
|
||||
double del1 = 1.5 * Globals::CK2() * x3thm1 / (a1 * a1 * betao_ * betao2_);
|
||||
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_);
|
||||
/*
|
||||
* recovered mean motion
|
||||
*/
|
||||
xnodp_ = tle_data_.xno / (1.0 + delo);
|
||||
/*
|
||||
* recovered semimajor axis
|
||||
*/
|
||||
aodp_ = ao / (1.0 - delo);
|
||||
|
||||
gsto = tle_data_.epoch.ToGMST();
|
||||
|
||||
double rp = aodp_ * (1.0 - tle_data_.eo);
|
||||
double perigee = (aodp_ * (1.0 - tle_data_.eo) - Globals::AE()) * Globals::XKMPER();
|
||||
double period = Globals::TWOPI() / xnodp_;
|
||||
|
||||
/*
|
||||
* for perigee less than 220 kilometers, the simple_model flag is set and
|
||||
* the equations are truncated to linear variation in sqrt a and
|
||||
* quadratic variation in mean anomly. also, the c3 term, the
|
||||
* delta omega term and the delta m term are dropped
|
||||
*/
|
||||
use_simple_model_ = false;
|
||||
if (rp < (220.0 / Globals::XKMPER() + Globals::AE()))
|
||||
use_simple_model_ = true;
|
||||
|
||||
double s4_ = 0.0;
|
||||
double qoms24_ = 0.0;
|
||||
|
||||
s4_ = Globals::S();
|
||||
qoms24_ = Globals::QOMS2T();
|
||||
/*
|
||||
* for perigee below 156km, the values of
|
||||
* s4 and qoms2t are altered
|
||||
*/
|
||||
if (perigee < 156.0) {
|
||||
s4_ = perigee - 78.0;
|
||||
if (perigee <= 98.0) {
|
||||
s4_ = 20.0;
|
||||
}
|
||||
|
||||
qoms24_ = pow((120.0 - s4_) * Globals::AE() / Globals::XKMPER(), 4.0);
|
||||
s4_ = s4_ / Globals::XKMPER() + Globals::AE();
|
||||
}
|
||||
|
||||
if (period >= 225.0) {
|
||||
use_deep_space_ = true;
|
||||
}
|
||||
|
||||
double pinvsq = 1.0 / (aodp_ * aodp_ * betao2_ * betao2_);
|
||||
double sing = sin(tle_data_.omega);
|
||||
double cosg = cos(tle_data_.omega);
|
||||
|
||||
double tsi_ = 0.0;
|
||||
double eta_ = 0.0;
|
||||
double eeta_ = 0.0;
|
||||
double coef_ = 0.0;
|
||||
double coef1_ = 0.0;
|
||||
double c1_ = 0.0;
|
||||
double a3ovk2_ = 0.0;
|
||||
|
||||
tsi_ = 1.0 / (aodp_ - s4_);
|
||||
eta_ = aodp_ * tle_data_.eo * tsi_;
|
||||
double etasq = eta_ * eta_;
|
||||
eeta_ = tle_data_.eo * eta_;
|
||||
double psisq = fabs(1.0 - etasq);
|
||||
coef_ = qoms24_ * pow(tsi_, 4.0);
|
||||
coef1_ = coef_ / pow(psisq, 3.5);
|
||||
double c2 = coef1_ * xnodp_ * (aodp_ * (1.0 + 1.5 * etasq + eeta_ *
|
||||
(4.0 + etasq)) + 0.75 * Globals::CK2() * tsi_ / psisq *
|
||||
x3thm1 * (8.0 + 3.0 * etasq * (8.0 + etasq)));
|
||||
c1_ = tle_data_.bstar * c2;
|
||||
a3ovk2_ = -Globals::XJ3() / Globals::CK2() * pow(Globals::AE(), 3.0);
|
||||
|
||||
double c3_ = 0.0;
|
||||
double c4_ = 0.0;
|
||||
double c5_ = 0.0;
|
||||
double xmdot_ = 0.0;
|
||||
double omgdot_ = 0.0;
|
||||
double xnodot_ = 0.0;
|
||||
double xnodcf_ = 0.0;
|
||||
double t2cof_ = 0.0;
|
||||
double xlcof_ = 0.0; // move to end?
|
||||
double aycof_ = 0.0; // move to end?
|
||||
double x7thm1_ = 0.0; // move to end?
|
||||
double omgcof_ = 0.0;
|
||||
double xmcof_ = 0.0;
|
||||
double delmo_ = 0.0;
|
||||
double sinmo_ = 0.0;
|
||||
|
||||
double x1mth2 = 1.0 - theta2;
|
||||
c4_ = 2.0 * xnodp_ * coef1_ * aodp_ * betao2_ *
|
||||
(eta_ * (2.0 + 0.5 * etasq) + tle_data_.eo * (0.5 + 2.0 * etasq) -
|
||||
2.0 * Globals::CK2() * tsi_ / (aodp_ * psisq) *
|
||||
(-3.0 * x3thm1 * (1.0 - 2.0 * eeta_ + etasq *
|
||||
(1.5 - 0.5 * eeta_)) + 0.75 * x1mth2 * (2.0 * etasq - eeta_ *
|
||||
(1.0 + etasq)) * cos(2.0 * tle_data_.omega)));
|
||||
double theta4 = theta2 * theta2;
|
||||
double temp1 = 3.0 * Globals::CK2() * pinvsq * xnodp_;
|
||||
double temp2 = temp1 * Globals::CK2() * pinvsq;
|
||||
double temp3 = 1.25 * Globals::CK4() * pinvsq * pinvsq * xnodp_;
|
||||
xmdot_ = xnodp_ + 0.5 * temp1 * betao_ * x3thm1 + 0.0625 * temp2 * betao_ *
|
||||
(13.0 - 78.0 * theta2 + 137.0 * theta4);
|
||||
double x1m5th = 1.0 - 5.0 * theta2;
|
||||
omgdot_ = -0.5 * temp1 * x1m5th + 0.0625 * temp2 * (7.0 - 114.0 * theta2 +
|
||||
395.0 * theta4) + temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4);
|
||||
double xhdot1_ = -temp1 * cosio_;
|
||||
xnodot_ = xhdot1_ + (0.5 * temp2 * (4.0 - 19.0 * theta2) + 2.0 * temp3 *
|
||||
(3.0 - 7.0 * theta2)) * cosio_;
|
||||
xnodcf_ = 3.5 * betao2_ * xhdot1_ * c1_;
|
||||
t2cof_ = 1.5 * c1_;
|
||||
|
||||
if (fabs(cosio_ + 1.0) > 1.5e-12)
|
||||
xlcof_ = 0.125 * a3ovk2_ * sinio_ * (3.0 + 5.0 * cosio_) / (1.0 + cosio_);
|
||||
else
|
||||
xlcof_ = 0.125 * a3ovk2_ * sinio_ * (3.0 + 5.0 * cosio_) / 1.5e-12;
|
||||
|
||||
aycof_ = 0.25 * a3ovk2_ * sinio_;
|
||||
x7thm1_ = 7.0 * theta2 - 1.0;
|
||||
|
||||
if (!use_deep_space_) {
|
||||
|
||||
// check result (different to telsko)
|
||||
c3_ = 0.0;
|
||||
if (tle_data_.eo > 1.0e-4)
|
||||
c3_ = coef_ * tsi_ * a3ovk2_ * xnodp_ * Globals::AE() *
|
||||
sinio_ / tle_data_.eo;
|
||||
|
||||
c5_ = 2.0 * coef1_ * aodp_ * betao2_ * (1.0 + 2.75 * (etasq + eeta_) + eeta_ * etasq);
|
||||
omgcof_ = tle_data_.bstar * c3_ * cos(tle_data_.omega);
|
||||
|
||||
xmcof_ = 0.0;
|
||||
if (tle_data_.eo > 1.0e-4)
|
||||
xmcof_ = -Globals::TOTHRD() * coef_ * tle_data_.bstar * Globals::AE() / eeta_;
|
||||
|
||||
delmo_ = pow(1.0 + eta_ * (cos(tle_data_.xmo)), 3.0);
|
||||
sinmo_ = sin(tle_data_.xmo);
|
||||
}
|
||||
|
||||
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 (!use_simple_model_) {
|
||||
double c1sq = c1_ * c1_;
|
||||
d2_ = 4.0 * aodp_ * tsi_ * c1sq;
|
||||
double temp = d2_ * tsi_ * c1_ / 3.0;
|
||||
d3_ = (17.0 * aodp_ + s4_) * temp;
|
||||
d4_ = 0.5 * temp * aodp_ * tsi_ * (221.0 * aodp_ + 31.0 * s4_) * c1_;
|
||||
t3cof_ = d2_ + 2.0 * c1sq;
|
||||
t4cof_ = 0.25 * (3.0 * d3_ + c1_ * (12.0 * d2_ + 10.0 * c1sq));
|
||||
t5cof_ = 0.2 * (3.0 * d4_ + 12.0 * c1_ * d3_ + 6.0 * d2_ * d2_ + 15.0 *
|
||||
c1sq * (2.0 * d2_ + c1sq));
|
||||
} else if (use_deep_space_) {
|
||||
|
||||
}
|
||||
|
||||
first_run_ = false;
|
||||
}
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
#ifndef SGDP4_H_
|
||||
#define SGDP4_H_
|
||||
|
||||
#include "Tle.h"
|
||||
|
||||
class SGDP4
|
||||
{
|
||||
public:
|
||||
SGDP4(void);
|
||||
virtual ~SGDP4(void);
|
||||
|
||||
void Initialize(const Tle& tle);
|
||||
|
||||
struct TleData {
|
||||
double bstar;
|
||||
double eo;
|
||||
double omega;
|
||||
double xincl;
|
||||
double xmo;
|
||||
double xno;
|
||||
double xnodeo;
|
||||
Julian epoch;
|
||||
};
|
||||
|
||||
private:
|
||||
bool first_run_;
|
||||
|
||||
struct TleData tle_data_;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
#ifndef SATELLITEEXCEPTION_H_
|
||||
#define SATELLITEEXCEPTION_H_
|
||||
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
|
||||
class SatelliteException : public std::exception
|
||||
{
|
||||
public:
|
||||
SatelliteException(const char* message)
|
||||
: message_(message) {}
|
||||
virtual ~SatelliteException(void) throw() {}
|
||||
|
||||
virtual const char* what() const throw()
|
||||
{
|
||||
return message_.c_str();
|
||||
}
|
||||
|
||||
private:
|
||||
std::string message_;
|
||||
};
|
||||
|
||||
#endif
|
||||
15
main.cpp
15
main.cpp
|
|
@ -1,5 +1,6 @@
|
|||
#include "Julian.h"
|
||||
#include "Tle.h"
|
||||
#include "SGDP4.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <list>
|
||||
|
|
@ -40,12 +41,14 @@ int main() {
|
|||
"1 35683U 09041C 11083.19129526 .00000128 00000-0 29890-4 0 8539",
|
||||
"2 35683 98.0774 342.2478 0001262 104.0560 256.0776 14.69410764 88426"));
|
||||
|
||||
Julian julian_now;
|
||||
int year = 0;
|
||||
int month = 0;
|
||||
double day = 0.0;
|
||||
julian_now.GetComponent(year, month, day);
|
||||
printf("year: %i\nmonth: %i\nday: %lf\n", year, month, day);
|
||||
|
||||
std::list<Tle>::iterator itr;
|
||||
Julian jul;
|
||||
|
||||
for (itr = tles.begin(); itr != tles.end(); itr++) {
|
||||
SGDP4 model;
|
||||
model.Initialize(*itr);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue