KSat/fsfw
15
4
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests 3 Releases Wiki Activity

WIP: somethings wrong.. #19

Closed
muellerr wants to merge 808 commits from source/master into master
pull from: source/master
merge into: KSat:master
Conversation 0 Commits 808 Files Changed 512 +713 -713
5 changed files with 713 additions and 713 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 5ce954672b - Show all commits

454
coordinates/CoordinateTransformations.cpp
View File

@ -1,227 +1,227 @@
#include "../coordinates/CoordinateTransformations.h" #include "CoordinateTransformations.h"
#include "../globalfunctions/constants.h" #include "../globalfunctions/constants.h"
#include "../globalfunctions/math/MatrixOperations.h" #include "../globalfunctions/math/MatrixOperations.h"
#include "../globalfunctions/math/VectorOperations.h" #include "../globalfunctions/math/VectorOperations.h"
#include <stddef.h> #include <stddef.h>
#include <cmath> #include <cmath>
void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, void CoordinateTransformations::positionEcfToEci(const double* ecfPosition,
double* eciPosition, timeval *timeUTC) { double* eciPosition, timeval *timeUTC) {
ecfToEci(ecfPosition, eciPosition, NULL, timeUTC); ecfToEci(ecfPosition, eciPosition, NULL, timeUTC);
} }
void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity, void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity,
const double* ecfPosition, double* eciVelocity, timeval *timeUTC) { const double* ecfPosition, double* eciVelocity, timeval *timeUTC) {
ecfToEci(ecfVelocity, eciVelocity, ecfPosition, timeUTC); ecfToEci(ecfVelocity, eciVelocity, ecfPosition, timeUTC);
} }
void CoordinateTransformations::positionEciToEcf(const double* eciCoordinates, double* ecfCoordinates,timeval *timeUTC){ void CoordinateTransformations::positionEciToEcf(const double* eciCoordinates, double* ecfCoordinates,timeval *timeUTC){
eciToEcf(eciCoordinates,ecfCoordinates,NULL,timeUTC); eciToEcf(eciCoordinates,ecfCoordinates,NULL,timeUTC);
}; };
void CoordinateTransformations::velocityEciToEcf(const double* eciVelocity,const double* eciPosition, double* ecfVelocity,timeval* timeUTC){ void CoordinateTransformations::velocityEciToEcf(const double* eciVelocity,const double* eciPosition, double* ecfVelocity,timeval* timeUTC){
eciToEcf(eciVelocity,ecfVelocity,eciPosition,timeUTC); eciToEcf(eciVelocity,ecfVelocity,eciPosition,timeUTC);
} }
double CoordinateTransformations::getEarthRotationAngle(timeval timeUTC) { double CoordinateTransformations::getEarthRotationAngle(timeval timeUTC) {
double jD2000UTC; double jD2000UTC;
Clock::convertTimevalToJD2000(timeUTC, &jD2000UTC); Clock::convertTimevalToJD2000(timeUTC, &jD2000UTC);
double TTt2000 = getJuleanCenturiesTT(timeUTC); double TTt2000 = getJuleanCenturiesTT(timeUTC);
double theta = 2 * Math::PI double theta = 2 * Math::PI
* (0.779057273264 + 1.00273781191135448 * jD2000UTC); * (0.779057273264 + 1.00273781191135448 * jD2000UTC);
//Correct theta according to IAU 2000 precession-nutation model //Correct theta according to IAU 2000 precession-nutation model
theta = theta + 7.03270725817493E-008 + 0.0223603701 * TTt2000 theta = theta + 7.03270725817493E-008 + 0.0223603701 * TTt2000
+ 6.77128219501896E-006 * TTt2000 * TTt2000 + 6.77128219501896E-006 * TTt2000 * TTt2000
+ 4.5300990362875E-010 * TTt2000 * TTt2000 * TTt2000 + 4.5300990362875E-010 * TTt2000 * TTt2000 * TTt2000
+ 9.12419347848147E-011 * TTt2000 * TTt2000 * TTt2000 * TTt2000; + 9.12419347848147E-011 * TTt2000 * TTt2000 * TTt2000 * TTt2000;
return theta; return theta;
} }
void CoordinateTransformations::getEarthRotationMatrix(timeval timeUTC, void CoordinateTransformations::getEarthRotationMatrix(timeval timeUTC,
double matrix[][3]) { double matrix[][3]) {
double theta = getEarthRotationAngle(timeUTC); double theta = getEarthRotationAngle(timeUTC);
matrix[0][0] = cos(theta); matrix[0][0] = cos(theta);
matrix[0][1] = sin(theta); matrix[0][1] = sin(theta);
matrix[0][2] = 0; matrix[0][2] = 0;
matrix[1][0] = -sin(theta); matrix[1][0] = -sin(theta);
matrix[1][1] = cos(theta); matrix[1][1] = cos(theta);
matrix[1][2] = 0; matrix[1][2] = 0;
matrix[2][0] = 0; matrix[2][0] = 0;
matrix[2][1] = 0; matrix[2][1] = 0;
matrix[2][2] = 1; matrix[2][2] = 1;
} }
void CoordinateTransformations::ecfToEci(const double* ecfCoordinates, void CoordinateTransformations::ecfToEci(const double* ecfCoordinates,
double* eciCoordinates, double* eciCoordinates,
const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin) { const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin) {
timeval timeUTC; timeval timeUTC;
if (timeUTCin != NULL) { if (timeUTCin != NULL) {
timeUTC = *timeUTCin; timeUTC = *timeUTCin;
} else { } else {
Clock::getClock_timeval(&timeUTC); Clock::getClock_timeval(&timeUTC);
} }
double Tfi[3][3]; double Tfi[3][3];
double Tif[3][3]; double Tif[3][3];
getTransMatrixECITOECF(timeUTC,Tfi); getTransMatrixECITOECF(timeUTC,Tfi);
MatrixOperations<double>::transpose(Tfi[0], Tif[0], 3); MatrixOperations<double>::transpose(Tfi[0], Tif[0], 3);
MatrixOperations<double>::multiply(Tif[0], ecfCoordinates, eciCoordinates, MatrixOperations<double>::multiply(Tif[0], ecfCoordinates, eciCoordinates,
3, 3, 1); 3, 3, 1);
if (ecfPositionIfCoordinatesAreVelocity != NULL) { if (ecfPositionIfCoordinatesAreVelocity != NULL) {
double Tdotfi[3][3]; double Tdotfi[3][3];
double Tdotif[3][3]; double Tdotif[3][3];
double Trot[3][3] = { { 0, Earth::OMEGA, 0 }, double Trot[3][3] = { { 0, Earth::OMEGA, 0 },
{ 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } }; { 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } };
MatrixOperations<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3, MatrixOperations<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3,
3); 3);
MatrixOperations<double>::transpose(Tdotfi[0], Tdotif[0], 3); MatrixOperations<double>::transpose(Tdotfi[0], Tdotif[0], 3);
double velocityCorrection[3]; double velocityCorrection[3];
MatrixOperations<double>::multiply(Tdotif[0], MatrixOperations<double>::multiply(Tdotif[0],
ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3, ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3,
1); 1);
VectorOperations<double>::add(velocityCorrection, eciCoordinates, VectorOperations<double>::add(velocityCorrection, eciCoordinates,
eciCoordinates, 3); eciCoordinates, 3);
} }
} }
double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) { double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) {
timeval timeTT; timeval timeTT;
Clock::convertUTCToTT(timeUTC, &timeTT); Clock::convertUTCToTT(timeUTC, &timeTT);
double jD2000TT; double jD2000TT;
Clock::convertTimevalToJD2000(timeTT, &jD2000TT); Clock::convertTimevalToJD2000(timeTT, &jD2000TT);
return jD2000TT / 36525.; return jD2000TT / 36525.;
} }
void CoordinateTransformations::eciToEcf(const double* eciCoordinates, void CoordinateTransformations::eciToEcf(const double* eciCoordinates,
double* ecfCoordinates, double* ecfCoordinates,
const double* eciPositionIfCoordinatesAreVelocity,timeval *timeUTCin){ const double* eciPositionIfCoordinatesAreVelocity,timeval *timeUTCin){
timeval timeUTC; timeval timeUTC;
if (timeUTCin != NULL) { if (timeUTCin != NULL) {
timeUTC = *timeUTCin; timeUTC = *timeUTCin;
}else{ }else{
Clock::getClock_timeval(&timeUTC); Clock::getClock_timeval(&timeUTC);
} }
double Tfi[3][3]; double Tfi[3][3];
getTransMatrixECITOECF(timeUTC,Tfi); getTransMatrixECITOECF(timeUTC,Tfi);
MatrixOperations<double>::multiply(Tfi[0],eciCoordinates,ecfCoordinates,3,3,1); MatrixOperations<double>::multiply(Tfi[0],eciCoordinates,ecfCoordinates,3,3,1);
if (eciPositionIfCoordinatesAreVelocity != NULL) { if (eciPositionIfCoordinatesAreVelocity != NULL) {
double Tdotfi[3][3]; double Tdotfi[3][3];
double Trot[3][3] = { { 0, Earth::OMEGA, 0 }, double Trot[3][3] = { { 0, Earth::OMEGA, 0 },
{ 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } }; { 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } };
MatrixOperations<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3, MatrixOperations<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3,
3); 3);
double velocityCorrection[3]; double velocityCorrection[3];
MatrixOperations<double>::multiply(Tdotfi[0], MatrixOperations<double>::multiply(Tdotfi[0],
eciPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3, eciPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3,
1); 1);
VectorOperations<double>::add(ecfCoordinates, velocityCorrection, VectorOperations<double>::add(ecfCoordinates, velocityCorrection,
ecfCoordinates, 3); ecfCoordinates, 3);
} }
}; };
void CoordinateTransformations::getTransMatrixECITOECF(timeval timeUTC,double Tfi[3][3]){ void CoordinateTransformations::getTransMatrixECITOECF(timeval timeUTC,double Tfi[3][3]){
double TTt2000 = getJuleanCenturiesTT(timeUTC); double TTt2000 = getJuleanCenturiesTT(timeUTC);
////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////
// Calculate Precession Matrix // Calculate Precession Matrix
double zeta = 0.0111808609 * TTt2000 double zeta = 0.0111808609 * TTt2000
+ 1.46355554053347E-006 * TTt2000 * TTt2000 + 1.46355554053347E-006 * TTt2000 * TTt2000
+ 8.72567663260943E-008 * TTt2000 * TTt2000 * TTt2000; + 8.72567663260943E-008 * TTt2000 * TTt2000 * TTt2000;
double theta_p = 0.0097171735 * TTt2000 double theta_p = 0.0097171735 * TTt2000
- 2.06845757045384E-006 * TTt2000 * TTt2000 - 2.06845757045384E-006 * TTt2000 * TTt2000
- 2.02812107218552E-007 * TTt2000 * TTt2000 * TTt2000; - 2.02812107218552E-007 * TTt2000 * TTt2000 * TTt2000;
double z = zeta + 3.8436028638364E-006 * TTt2000 * TTt2000 double z = zeta + 3.8436028638364E-006 * TTt2000 * TTt2000
+ 0.000000001 * TTt2000 * TTt2000 * TTt2000; + 0.000000001 * TTt2000 * TTt2000 * TTt2000;
double mPrecession[3][3]; double mPrecession[3][3];
mPrecession[0][0] = -sin(z) * sin(zeta) + cos(z) * cos(theta_p) * cos(zeta); mPrecession[0][0] = -sin(z) * sin(zeta) + cos(z) * cos(theta_p) * cos(zeta);
mPrecession[1][0] = cos(z) * sin(zeta) + sin(z) * cos(theta_p) * cos(zeta); mPrecession[1][0] = cos(z) * sin(zeta) + sin(z) * cos(theta_p) * cos(zeta);
mPrecession[2][0] = sin(theta_p) * cos(zeta); mPrecession[2][0] = sin(theta_p) * cos(zeta);
mPrecession[0][1] = -sin(z) * cos(zeta) - cos(z) * cos(theta_p) * sin(zeta); mPrecession[0][1] = -sin(z) * cos(zeta) - cos(z) * cos(theta_p) * sin(zeta);
mPrecession[1][1] = cos(z) * cos(zeta) - sin(z) * cos(theta_p) * sin(zeta); mPrecession[1][1] = cos(z) * cos(zeta) - sin(z) * cos(theta_p) * sin(zeta);
mPrecession[2][1] = -sin(theta_p) * sin(zeta); mPrecession[2][1] = -sin(theta_p) * sin(zeta);
mPrecession[0][2] = -cos(z) * sin(theta_p); mPrecession[0][2] = -cos(z) * sin(theta_p);
mPrecession[1][2] = -sin(z) * sin(theta_p); mPrecession[1][2] = -sin(z) * sin(theta_p);
mPrecession[2][2] = cos(theta_p); mPrecession[2][2] = cos(theta_p);
////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////
// Calculate Nutation Matrix // Calculate Nutation Matrix
double omega_moon = 2.1824386244 - 33.7570459338 * TTt2000 double omega_moon = 2.1824386244 - 33.7570459338 * TTt2000
+ 3.61428599267159E-005 * TTt2000 * TTt2000 + 3.61428599267159E-005 * TTt2000 * TTt2000
+ 3.87850944887629E-008 * TTt2000 * TTt2000 * TTt2000; + 3.87850944887629E-008 * TTt2000 * TTt2000 * TTt2000;
double deltaPsi = -0.000083388 * sin(omega_moon); double deltaPsi = -0.000083388 * sin(omega_moon);
double deltaEpsilon = 4.46174030725106E-005 * cos(omega_moon); double deltaEpsilon = 4.46174030725106E-005 * cos(omega_moon);
double epsilon = 0.4090928042 - 0.0002269655 * TTt2000 double epsilon = 0.4090928042 - 0.0002269655 * TTt2000
- 2.86040071854626E-009 * TTt2000 * TTt2000 - 2.86040071854626E-009 * TTt2000 * TTt2000
+ 8.78967203851589E-009 * TTt2000 * TTt2000 * TTt2000; + 8.78967203851589E-009 * TTt2000 * TTt2000 * TTt2000;
double mNutation[3][3]; double mNutation[3][3];
mNutation[0][0] = cos(deltaPsi); mNutation[0][0] = cos(deltaPsi);
mNutation[1][0] = cos(epsilon + deltaEpsilon) * sin(deltaPsi); mNutation[1][0] = cos(epsilon + deltaEpsilon) * sin(deltaPsi);
mNutation[2][0] = sin(epsilon + deltaEpsilon) * sin(deltaPsi); mNutation[2][0] = sin(epsilon + deltaEpsilon) * sin(deltaPsi);
mNutation[0][1] = -cos(epsilon) * sin(deltaPsi); mNutation[0][1] = -cos(epsilon) * sin(deltaPsi);
mNutation[1][1] = cos(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) mNutation[1][1] = cos(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi)
+ sin(epsilon) * sin(epsilon + deltaEpsilon); + sin(epsilon) * sin(epsilon + deltaEpsilon);
mNutation[2][1] = cos(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) mNutation[2][1] = cos(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi)
- sin(epsilon) * cos(epsilon + deltaEpsilon); - sin(epsilon) * cos(epsilon + deltaEpsilon);
mNutation[0][2] = -sin(epsilon) * sin(deltaPsi); mNutation[0][2] = -sin(epsilon) * sin(deltaPsi);
mNutation[1][2] = sin(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) mNutation[1][2] = sin(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi)
- cos(epsilon) * sin(epsilon + deltaEpsilon); - cos(epsilon) * sin(epsilon + deltaEpsilon);
mNutation[2][2] = sin(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) mNutation[2][2] = sin(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi)
+ cos(epsilon) * cos(epsilon + deltaEpsilon); + cos(epsilon) * cos(epsilon + deltaEpsilon);
////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////
// Calculate Earth rotation matrix // Calculate Earth rotation matrix
//calculate theta //calculate theta
double mTheta[3][3]; double mTheta[3][3];
double Ttemp[3][3]; double Ttemp[3][3];
getEarthRotationMatrix(timeUTC, mTheta); getEarthRotationMatrix(timeUTC, mTheta);
//polar motion is neglected //polar motion is neglected
MatrixOperations<double>::multiply(mNutation[0], mPrecession[0], Ttemp[0], MatrixOperations<double>::multiply(mNutation[0], mPrecession[0], Ttemp[0],
3, 3, 3); 3, 3, 3);
MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3); MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3);
}; };

68
coordinates/CoordinateTransformations.h
View File

@ -1,34 +1,34 @@
#ifndef COORDINATETRANSFORMATIONS_H_ #ifndef COORDINATETRANSFORMATIONS_H_
#define COORDINATETRANSFORMATIONS_H_ #define COORDINATETRANSFORMATIONS_H_
#include "../timemanager/Clock.h" #include "../timemanager/Clock.h"
#include <cstring> #include <cstring>
class CoordinateTransformations { class CoordinateTransformations {
public: public:
static void positionEcfToEci(const double* ecfCoordinates, double* eciCoordinates, timeval *timeUTC = NULL); static void positionEcfToEci(const double* ecfCoordinates, double* eciCoordinates, timeval *timeUTC = NULL);
static void velocityEcfToEci(const double* ecfVelocity, static void velocityEcfToEci(const double* ecfVelocity,
const double* ecfPosition, const double* ecfPosition,
double* eciVelocity, timeval *timeUTC = NULL); double* eciVelocity, timeval *timeUTC = NULL);
static void positionEciToEcf(const double* eciCoordinates, double* ecfCoordinates,timeval *timeUTC = NULL); static void positionEciToEcf(const double* eciCoordinates, double* ecfCoordinates,timeval *timeUTC = NULL);
static void velocityEciToEcf(const double* eciVelocity,const double* eciPosition, double* ecfVelocity,timeval* timeUTC = NULL); static void velocityEciToEcf(const double* eciVelocity,const double* eciPosition, double* ecfVelocity,timeval* timeUTC = NULL);
static double getEarthRotationAngle(timeval timeUTC); static double getEarthRotationAngle(timeval timeUTC);
static void getEarthRotationMatrix(timeval timeUTC, double matrix[][3]); static void getEarthRotationMatrix(timeval timeUTC, double matrix[][3]);
private: private:
CoordinateTransformations(); CoordinateTransformations();
static void ecfToEci(const double* ecfCoordinates, double* eciCoordinates, static void ecfToEci(const double* ecfCoordinates, double* eciCoordinates,
const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin); const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin);
static void eciToEcf(const double* eciCoordinates, static void eciToEcf(const double* eciCoordinates,
double* ecfCoordinates, double* ecfCoordinates,
const double* eciPositionIfCoordinatesAreVelocity,timeval *timeUTCin); const double* eciPositionIfCoordinatesAreVelocity,timeval *timeUTCin);
static double getJuleanCenturiesTT(timeval timeUTC); static double getJuleanCenturiesTT(timeval timeUTC);
static void getTransMatrixECITOECF(timeval time,double Tfi[3][3]); static void getTransMatrixECITOECF(timeval time,double Tfi[3][3]);
}; };
#endif /* COORDINATETRANSFORMATIONS_H_ */ #endif /* COORDINATETRANSFORMATIONS_H_ */

360
coordinates/Jgm3Model.h
View File

@ -1,180 +1,180 @@
#ifndef FRAMEWORK_COORDINATES_JGM3MODEL_H_ #ifndef FRAMEWORK_COORDINATES_JGM3MODEL_H_
#define FRAMEWORK_COORDINATES_JGM3MODEL_H_ #define FRAMEWORK_COORDINATES_JGM3MODEL_H_
#include <stdint.h> #include <stdint.h>
#include "../coordinates/CoordinateTransformations.h" #include "CoordinateTransformations.h"
#include "../globalfunctions/math/VectorOperations.h" #include "../globalfunctions/math/VectorOperations.h"
#include "../globalfunctions/timevalOperations.h" #include "../globalfunctions/timevalOperations.h"
#include "../globalfunctions/constants.h" #include "../globalfunctions/constants.h"
#include <memory.h> #include <memory.h>
template<uint8_t DEGREE,uint8_t ORDER> template<uint8_t DEGREE,uint8_t ORDER>
class Jgm3Model { class Jgm3Model {
public: public:
static const uint32_t factorialLookupTable[DEGREE+3]; //This table is used instead of factorial calculation, must be increased if order or degree is higher static const uint32_t factorialLookupTable[DEGREE+3]; //This table is used instead of factorial calculation, must be increased if order or degree is higher
Jgm3Model() { Jgm3Model() {
y0[0] = 0; y0[0] = 0;
y0[1] = 0; y0[1] = 0;
y0[2] = 0; y0[2] = 0;
y0[3] = 0; y0[3] = 0;
y0[4] = 0; y0[4] = 0;
y0[5] = 0; y0[5] = 0;
lastExecutionTime.tv_sec = 0; lastExecutionTime.tv_sec = 0;
lastExecutionTime.tv_usec = 0; lastExecutionTime.tv_usec = 0;
} }
virtual ~Jgm3Model(){}; virtual ~Jgm3Model(){};
//double acsNavOrbit(double posECF[3],double velECF[3],timeval gpsTime); //double acsNavOrbit(double posECF[3],double velECF[3],timeval gpsTime);
double y0[6]; //position and velocity at beginning of RK step in EC double y0[6]; //position and velocity at beginning of RK step in EC
timeval lastExecutionTime; //Time of last execution timeval lastExecutionTime; //Time of last execution
void accelDegOrd(const double pos[3],const double S[ORDER+1][DEGREE+1],const double C[ORDER+1][DEGREE+1],double* accel){ void accelDegOrd(const double pos[3],const double S[ORDER+1][DEGREE+1],const double C[ORDER+1][DEGREE+1],double* accel){
//Get radius of this position //Get radius of this position
double r = VectorOperations<double>::norm(pos,3); double r = VectorOperations<double>::norm(pos,3);
//Initialize the V and W matrix //Initialize the V and W matrix
double V[DEGREE+2][ORDER+2] = {{0}}; double V[DEGREE+2][ORDER+2] = {{0}};
double W[DEGREE+2][ORDER+2] = {{0}}; double W[DEGREE+2][ORDER+2] = {{0}};
for(uint8_t m=0;m<(ORDER+2);m++){ for(uint8_t m=0;m<(ORDER+2);m++){
for(uint8_t n=m;n<(DEGREE+2);n++){ for(uint8_t n=m;n<(DEGREE+2);n++){
if((n==0) && (m==0)){ if((n==0) && (m==0)){
//Montenbruck "Satellite Orbits Eq.3.31" //Montenbruck "Satellite Orbits Eq.3.31"
V[0][0] = Earth::MEAN_RADIUS / r; V[0][0] = Earth::MEAN_RADIUS / r;
W[0][0] = 0; W[0][0] = 0;
}else{ }else{
if(n==m){ if(n==m){
//Montenbruck "Satellite Orbits Eq.3.29" //Montenbruck "Satellite Orbits Eq.3.29"
V[m][m] = (2*m-1)* (pos[0]*Earth::MEAN_RADIUS/pow(r,2)*V[m-1][m-1] - pos[1]*Earth::MEAN_RADIUS/pow(r,2)*W[m-1][m-1]); V[m][m] = (2*m-1)* (pos[0]*Earth::MEAN_RADIUS/pow(r,2)*V[m-1][m-1] - pos[1]*Earth::MEAN_RADIUS/pow(r,2)*W[m-1][m-1]);
W[m][m] = (2*m-1)* (pos[0]*Earth::MEAN_RADIUS/pow(r,2)*W[m-1][m-1] + pos[1]*Earth::MEAN_RADIUS/pow(r,2)*V[m-1][m-1]); W[m][m] = (2*m-1)* (pos[0]*Earth::MEAN_RADIUS/pow(r,2)*W[m-1][m-1] + pos[1]*Earth::MEAN_RADIUS/pow(r,2)*V[m-1][m-1]);
}else{ }else{
//Montenbruck "Satellite Orbits Eq.3.30" //Montenbruck "Satellite Orbits Eq.3.30"
V[n][m] = ((2*n-1)/(double)(n-m))*pos[2]*Earth::MEAN_RADIUS / pow(r,2)*V[n-1][m]; V[n][m] = ((2*n-1)/(double)(n-m))*pos[2]*Earth::MEAN_RADIUS / pow(r,2)*V[n-1][m];
W[n][m] = ((2*n-1)/(double)(n-m))*pos[2]*Earth::MEAN_RADIUS / pow(r,2)*W[n-1][m]; W[n][m] = ((2*n-1)/(double)(n-m))*pos[2]*Earth::MEAN_RADIUS / pow(r,2)*W[n-1][m];
if(n!=(m+1)){ if(n!=(m+1)){
V[n][m] = V[n][m] - (((n+m-1)/(double)(n-m)) * (pow(Earth::MEAN_RADIUS,2) / pow(r,2)) * V[n-2][m]); V[n][m] = V[n][m] - (((n+m-1)/(double)(n-m)) * (pow(Earth::MEAN_RADIUS,2) / pow(r,2)) * V[n-2][m]);
W[n][m] = W[n][m] - (((n+m-1)/(double)(n-m)) * (pow(Earth::MEAN_RADIUS,2) / pow(r,2)) * W[n-2][m]); W[n][m] = W[n][m] - (((n+m-1)/(double)(n-m)) * (pow(Earth::MEAN_RADIUS,2) / pow(r,2)) * W[n-2][m]);
}//End of if(n!=(m+1)) }//End of if(n!=(m+1))
}//End of if(n==m){ }//End of if(n==m){
}//End of if(n==0 and m==0) }//End of if(n==0 and m==0)
}//End of for(uint8_t n=0;n<(DEGREE+1);n++) }//End of for(uint8_t n=0;n<(DEGREE+1);n++)
}//End of for(uint8_t m=0;m<(ORDER+1);m++) }//End of for(uint8_t m=0;m<(ORDER+1);m++)
//overwrite accel if not properly initialized //overwrite accel if not properly initialized
accel[0] = 0; accel[0] = 0;
accel[1] = 0; accel[1] = 0;
accel[2] = 0; accel[2] = 0;
for(uint8_t m=0;m<(ORDER+1);m++){ for(uint8_t m=0;m<(ORDER+1);m++){
for(uint8_t n=m;n<(DEGREE+1);n++){ for(uint8_t n=m;n<(DEGREE+1);n++){
//Use table lookup to get factorial //Use table lookup to get factorial
double partAccel[3] = {0}; double partAccel[3] = {0};
double factor = Earth::STANDARD_GRAVITATIONAL_PARAMETER/pow(Earth::MEAN_RADIUS,2); double factor = Earth::STANDARD_GRAVITATIONAL_PARAMETER/pow(Earth::MEAN_RADIUS,2);
if(m==0){ if(m==0){
//Montenbruck "Satellite Orbits Eq.3.33" //Montenbruck "Satellite Orbits Eq.3.33"
partAccel[0] = factor * (-C[n][0]*V[n+1][1]); partAccel[0] = factor * (-C[n][0]*V[n+1][1]);
partAccel[1] = factor * (-C[n][0]*W[n+1][1]); partAccel[1] = factor * (-C[n][0]*W[n+1][1]);
}else{ }else{
double factMN = static_cast<double>(factorialLookupTable[n-m+2]) / static_cast<double>(factorialLookupTable[n-m]); double factMN = static_cast<double>(factorialLookupTable[n-m+2]) / static_cast<double>(factorialLookupTable[n-m]);
partAccel[0] = factor * 0.5 * ((-C[n][m]*V[n+1][m+1]-S[n][m]*W[n+1][m+1])+factMN*(C[n][m]*V[n+1][m-1]+S[n][m]*W[n+1][m-1])); partAccel[0] = factor * 0.5 * ((-C[n][m]*V[n+1][m+1]-S[n][m]*W[n+1][m+1])+factMN*(C[n][m]*V[n+1][m-1]+S[n][m]*W[n+1][m-1]));
partAccel[1] = factor * 0.5 * ((-C[n][m]*W[n+1][m+1]+S[n][m]*V[n+1][m+1])+factMN*(-C[n][m]*W[n+1][m-1]+S[n][m]*V[n+1][m-1])); partAccel[1] = factor * 0.5 * ((-C[n][m]*W[n+1][m+1]+S[n][m]*V[n+1][m+1])+factMN*(-C[n][m]*W[n+1][m-1]+S[n][m]*V[n+1][m-1]));
} }
partAccel[2] = factor * ((n-m+1)*(-C[n][m]*V[n+1][m]-S[n][m]*W[n+1][m])); partAccel[2] = factor * ((n-m+1)*(-C[n][m]*V[n+1][m]-S[n][m]*W[n+1][m]));
accel[0] += partAccel[0]; accel[0] += partAccel[0];
accel[1] += partAccel[1]; accel[1] += partAccel[1];
accel[2] += partAccel[2]; accel[2] += partAccel[2];
}//End of for(uint8_t n=0;n<DEGREE;n++) }//End of for(uint8_t n=0;n<DEGREE;n++)
}//End of uint8_t m=0;m<ORDER;m++ }//End of uint8_t m=0;m<ORDER;m++
} }
void initializeNavOrbit(const double position[3],const double velocity[3], timeval timeUTC){ void initializeNavOrbit(const double position[3],const double velocity[3], timeval timeUTC){
CoordinateTransformations::positionEcfToEci(position,&y0[0],&timeUTC); CoordinateTransformations::positionEcfToEci(position,&y0[0],&timeUTC);
CoordinateTransformations::velocityEcfToEci(velocity,position,&y0[3],&timeUTC); CoordinateTransformations::velocityEcfToEci(velocity,position,&y0[3],&timeUTC);
lastExecutionTime = timeUTC; lastExecutionTime = timeUTC;
} }
void acsNavOrbit(timeval timeUTC, const double S[ORDER+1][DEGREE+1],const double C[ORDER+1][DEGREE+1], double outputPos[3],double outputVel[3]){ void acsNavOrbit(timeval timeUTC, const double S[ORDER+1][DEGREE+1],const double C[ORDER+1][DEGREE+1], double outputPos[3],double outputVel[3]){
//RK4 Integration for this timestamp //RK4 Integration for this timestamp
double deltaT = timevalOperations::toDouble(timeUTC-lastExecutionTime); double deltaT = timevalOperations::toDouble(timeUTC-lastExecutionTime);
double y0dot[6] = {0,0,0,0,0,0}; double y0dot[6] = {0,0,0,0,0,0};
double yA[6] = {0,0,0,0,0,0}; double yA[6] = {0,0,0,0,0,0};
double yAdot[6] = {0,0,0,0,0,0}; double yAdot[6] = {0,0,0,0,0,0};
double yB[6] = {0,0,0,0,0,0}; double yB[6] = {0,0,0,0,0,0};
double yBdot[6] = {0,0,0,0,0,0}; double yBdot[6] = {0,0,0,0,0,0};
double yC[6] = {0,0,0,0,0,0}; double yC[6] = {0,0,0,0,0,0};
double yCdot[6] = {0,0,0,0,0,0}; double yCdot[6] = {0,0,0,0,0,0};
//Step One //Step One
rungeKuttaStep(y0,y0dot,lastExecutionTime,S,C); rungeKuttaStep(y0,y0dot,lastExecutionTime,S,C);
//Step Two //Step Two
VectorOperations<double>::mulScalar(y0dot,deltaT/2,yA,6); VectorOperations<double>::mulScalar(y0dot,deltaT/2,yA,6);
VectorOperations<double>::add(y0,yA,yA,6); VectorOperations<double>::add(y0,yA,yA,6);
rungeKuttaStep(yA,yAdot,lastExecutionTime,S,C); rungeKuttaStep(yA,yAdot,lastExecutionTime,S,C);
//Step Three //Step Three
VectorOperations<double>::mulScalar(yAdot,deltaT/2,yB,6); VectorOperations<double>::mulScalar(yAdot,deltaT/2,yB,6);
VectorOperations<double>::add(y0,yB,yB,6); VectorOperations<double>::add(y0,yB,yB,6);
rungeKuttaStep(yB,yBdot,lastExecutionTime,S,C); rungeKuttaStep(yB,yBdot,lastExecutionTime,S,C);
//Step Four //Step Four
VectorOperations<double>::mulScalar(yBdot,deltaT,yC,6); VectorOperations<double>::mulScalar(yBdot,deltaT,yC,6);
VectorOperations<double>::add(y0,yC,yC,6); VectorOperations<double>::add(y0,yC,yC,6);
rungeKuttaStep(yC,yCdot,lastExecutionTime,S,C); rungeKuttaStep(yC,yCdot,lastExecutionTime,S,C);
//Calc new State //Calc new State
VectorOperations<double>::mulScalar(yAdot,2,yAdot,6); VectorOperations<double>::mulScalar(yAdot,2,yAdot,6);
VectorOperations<double>::mulScalar(yBdot,2,yBdot,6); VectorOperations<double>::mulScalar(yBdot,2,yBdot,6);
VectorOperations<double>::add(y0dot,yAdot,y0dot,6); VectorOperations<double>::add(y0dot,yAdot,y0dot,6);
VectorOperations<double>::add(y0dot,yBdot,y0dot,6); VectorOperations<double>::add(y0dot,yBdot,y0dot,6);
VectorOperations<double>::add(y0dot,yCdot,y0dot,6); VectorOperations<double>::add(y0dot,yCdot,y0dot,6);
VectorOperations<double>::mulScalar(y0dot,1./6.*deltaT,y0dot,6); VectorOperations<double>::mulScalar(y0dot,1./6.*deltaT,y0dot,6);
VectorOperations<double>::add(y0,y0dot,y0,6); VectorOperations<double>::add(y0,y0dot,y0,6);
CoordinateTransformations::positionEciToEcf(&y0[0],outputPos,&timeUTC); CoordinateTransformations::positionEciToEcf(&y0[0],outputPos,&timeUTC);
CoordinateTransformations::velocityEciToEcf(&y0[3],&y0[0],outputVel,&timeUTC); CoordinateTransformations::velocityEciToEcf(&y0[3],&y0[0],outputVel,&timeUTC);
lastExecutionTime = timeUTC; lastExecutionTime = timeUTC;
} }
void rungeKuttaStep(const double* yIn,double* yOut,timeval time, const double S[ORDER+1][DEGREE+1],const double C[ORDER+1][DEGREE+1]){ void rungeKuttaStep(const double* yIn,double* yOut,timeval time, const double S[ORDER+1][DEGREE+1],const double C[ORDER+1][DEGREE+1]){
double rECF[3] = {0,0,0}; double rECF[3] = {0,0,0};
double rDotECF[3] = {0,0,0}; double rDotECF[3] = {0,0,0};
double accelECF[3] = {0,0,0}; double accelECF[3] = {0,0,0};
double accelECI[3] = {0,0,0}; double accelECI[3] = {0,0,0};
CoordinateTransformations::positionEciToEcf(&yIn[0],rECF,&time); CoordinateTransformations::positionEciToEcf(&yIn[0],rECF,&time);
CoordinateTransformations::velocityEciToEcf(&yIn[3],&yIn[0],rDotECF,&time); CoordinateTransformations::velocityEciToEcf(&yIn[3],&yIn[0],rDotECF,&time);
accelDegOrd(rECF,S,C,accelECF); accelDegOrd(rECF,S,C,accelECF);
//This is not correct, as the acceleration would have derived terms but we don't know the velocity and position at that time //This is not correct, as the acceleration would have derived terms but we don't know the velocity and position at that time
//Tests showed that a wrong velocity does make the equation worse than neglecting it //Tests showed that a wrong velocity does make the equation worse than neglecting it
CoordinateTransformations::positionEcfToEci(accelECF,accelECI,&time); CoordinateTransformations::positionEcfToEci(accelECF,accelECI,&time);
memcpy(&yOut[0],&yIn[3],sizeof(yOut[0])*3); memcpy(&yOut[0],&yIn[3],sizeof(yOut[0])*3);
memcpy(&yOut[3],accelECI,sizeof(yOut[0])*3); memcpy(&yOut[3],accelECI,sizeof(yOut[0])*3);
} }
}; };
#endif /* FRAMEWORK_COORDINATES_JGM3MODEL_H_ */ #endif /* FRAMEWORK_COORDINATES_JGM3MODEL_H_ */

456
coordinates/Sgp4Propagator.cpp
View File

@ -1,228 +1,228 @@
#include "../coordinates/CoordinateTransformations.h" #include "CoordinateTransformations.h"
#include "../coordinates/Sgp4Propagator.h" #include "Sgp4Propagator.h"
#include "../globalfunctions/constants.h" #include "../globalfunctions/constants.h"
#include "../globalfunctions/math/MatrixOperations.h" #include "../globalfunctions/math/MatrixOperations.h"
#include "../globalfunctions/math/VectorOperations.h" #include "../globalfunctions/math/VectorOperations.h"
#include "../globalfunctions/timevalOperations.h" #include "../globalfunctions/timevalOperations.h"
#include <cstring> #include <cstring>
Sgp4Propagator::Sgp4Propagator() : Sgp4Propagator::Sgp4Propagator() :
initialized(false), epoch({0, 0}), whichconst(wgs84) { initialized(false), epoch({0, 0}), whichconst(wgs84) {
} }
Sgp4Propagator::~Sgp4Propagator() { Sgp4Propagator::~Sgp4Propagator() {
} }
void jday(int year, int mon, int day, int hr, int minute, double sec, void jday(int year, int mon, int day, int hr, int minute, double sec,
double& jd) { double& jd) {
jd = 367.0 * year - floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25) jd = 367.0 * year - floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25)
+ floor(275 * mon / 9.0) + day + 1721013.5 + floor(275 * mon / 9.0) + day + 1721013.5
+ ((sec / 60.0 + minute) / 60.0 + hr) / 24.0; // ut in days + ((sec / 60.0 + minute) / 60.0 + hr) / 24.0; // ut in days
// - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5; // - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5;
} }
void days2mdhms(int year, double days, int& mon, int& day, int& hr, int& minute, void days2mdhms(int year, double days, int& mon, int& day, int& hr, int& minute,
double& sec) { double& sec) {
int i, inttemp, dayofyr; int i, inttemp, dayofyr;
double temp; double temp;
int lmonth[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; int lmonth[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
dayofyr = (int) floor(days); dayofyr = (int) floor(days);
/* ----------------- find month and day of month ---------------- */ /* ----------------- find month and day of month ---------------- */
if ((year % 4) == 0) if ((year % 4) == 0)
lmonth[1] = 29; lmonth[1] = 29;
i = 1; i = 1;
inttemp = 0; inttemp = 0;
while ((dayofyr > inttemp + lmonth[i - 1]) && (i < 12)) { while ((dayofyr > inttemp + lmonth[i - 1]) && (i < 12)) {
inttemp = inttemp + lmonth[i - 1]; inttemp = inttemp + lmonth[i - 1];
i++; i++;
} }
mon = i; mon = i;
day = dayofyr - inttemp; day = dayofyr - inttemp;
/* ----------------- find hours minutes and seconds ------------- */ /* ----------------- find hours minutes and seconds ------------- */
temp = (days - dayofyr) * 24.0; temp = (days - dayofyr) * 24.0;
hr = (int) floor(temp); hr = (int) floor(temp);
temp = (temp - hr) * 60.0; temp = (temp - hr) * 60.0;
minute = (int) floor(temp); minute = (int) floor(temp);
sec = (temp - minute) * 60.0; sec = (temp - minute) * 60.0;
} }
ReturnValue_t Sgp4Propagator::initialize(const uint8_t* line1, ReturnValue_t Sgp4Propagator::initialize(const uint8_t* line1,
const uint8_t* line2) { const uint8_t* line2) {
char longstr1[130]; char longstr1[130];
char longstr2[130]; char longstr2[130];
//need some space for decimal points //need some space for decimal points
memcpy(longstr1, line1, 69); memcpy(longstr1, line1, 69);
memcpy(longstr2, line2, 69); memcpy(longstr2, line2, 69);
const double deg2rad = Math::PI / 180.0; // 0.0174532925199433 const double deg2rad = Math::PI / 180.0; // 0.0174532925199433
const double xpdotp = 1440.0 / (2.0 * Math::PI); // 229.1831180523293 const double xpdotp = 1440.0 / (2.0 * Math::PI); // 229.1831180523293
double sec, mu, radiusearthkm, tumin, xke, j2, j3, j4, j3oj2; double sec, mu, radiusearthkm, tumin, xke, j2, j3, j4, j3oj2;
int cardnumb, numb, j; int cardnumb, numb, j;
long revnum = 0, elnum = 0; long revnum = 0, elnum = 0;
char classification, intldesg[11]; char classification, intldesg[11];
int year = 0; int year = 0;
int mon, day, hr, minute, nexp, ibexp; int mon, day, hr, minute, nexp, ibexp;
getgravconst(whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2); getgravconst(whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2);
satrec.error = 0; satrec.error = 0;
// set the implied decimal points since doing a formated read // set the implied decimal points since doing a formated read
// fixes for bad input data values (missing, ...) // fixes for bad input data values (missing, ...)
for (j = 10; j <= 15; j++) for (j = 10; j <= 15; j++)
if (longstr1[j] == ' ') if (longstr1[j] == ' ')
longstr1[j] = '_'; longstr1[j] = '_';
if (longstr1[44] != ' ') if (longstr1[44] != ' ')
longstr1[43] = longstr1[44]; longstr1[43] = longstr1[44];
longstr1[44] = '.'; longstr1[44] = '.';
if (longstr1[7] == ' ') if (longstr1[7] == ' ')
longstr1[7] = 'U'; longstr1[7] = 'U';
if (longstr1[9] == ' ') if (longstr1[9] == ' ')
longstr1[9] = '.'; longstr1[9] = '.';
for (j = 45; j <= 49; j++) for (j = 45; j <= 49; j++)
if (longstr1[j] == ' ') if (longstr1[j] == ' ')
longstr1[j] = '0'; longstr1[j] = '0';
if (longstr1[51] == ' ') if (longstr1[51] == ' ')
longstr1[51] = '0'; longstr1[51] = '0';
if (longstr1[53] != ' ') if (longstr1[53] != ' ')
longstr1[52] = longstr1[53]; longstr1[52] = longstr1[53];
longstr1[53] = '.'; longstr1[53] = '.';
longstr2[25] = '.'; longstr2[25] = '.';
for (j = 26; j <= 32; j++) for (j = 26; j <= 32; j++)
if (longstr2[j] == ' ') if (longstr2[j] == ' ')
longstr2[j] = '0'; longstr2[j] = '0';
if (longstr1[62] == ' ') if (longstr1[62] == ' ')
longstr1[62] = '0'; longstr1[62] = '0';
if (longstr1[68] == ' ') if (longstr1[68] == ' ')
longstr1[68] = '0'; longstr1[68] = '0';
sscanf(longstr1, sscanf(longstr1,
"%2d %5ld %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ", "%2d %5ld %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ",
&cardnumb, &satrec.satnum, &classification, intldesg, &cardnumb, &satrec.satnum, &classification, intldesg,
&satrec.epochyr, &satrec.epochdays, &satrec.ndot, &satrec.nddot, &satrec.epochyr, &satrec.epochdays, &satrec.ndot, &satrec.nddot,
&nexp, &satrec.bstar, &ibexp, &numb, &elnum); &nexp, &satrec.bstar, &ibexp, &numb, &elnum);
if (longstr2[52] == ' ') { if (longstr2[52] == ' ') {
sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n", sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n",
&cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo, &cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo,
&satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum); &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum);
} else { } else {
sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n", sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n",
&cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo, &cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo,
&satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum); &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum);
} }
// ---- find no, ndot, nddot ---- // ---- find no, ndot, nddot ----
satrec.no = satrec.no / xpdotp; //* rad/min satrec.no = satrec.no / xpdotp; //* rad/min
satrec.nddot = satrec.nddot * pow(10.0, nexp); satrec.nddot = satrec.nddot * pow(10.0, nexp);
satrec.bstar = satrec.bstar * pow(10.0, ibexp); satrec.bstar = satrec.bstar * pow(10.0, ibexp);
// ---- convert to sgp4 units ---- // ---- convert to sgp4 units ----
satrec.a = pow(satrec.no * tumin, (-2.0 / 3.0)); satrec.a = pow(satrec.no * tumin, (-2.0 / 3.0));
satrec.ndot = satrec.ndot / (xpdotp * 1440.0); //* ? * minperday satrec.ndot = satrec.ndot / (xpdotp * 1440.0); //* ? * minperday
satrec.nddot = satrec.nddot / (xpdotp * 1440.0 * 1440); satrec.nddot = satrec.nddot / (xpdotp * 1440.0 * 1440);
// ---- find standard orbital elements ---- // ---- find standard orbital elements ----
satrec.inclo = satrec.inclo * deg2rad; satrec.inclo = satrec.inclo * deg2rad;
satrec.nodeo = satrec.nodeo * deg2rad; satrec.nodeo = satrec.nodeo * deg2rad;
satrec.argpo = satrec.argpo * deg2rad; satrec.argpo = satrec.argpo * deg2rad;
satrec.mo = satrec.mo * deg2rad; satrec.mo = satrec.mo * deg2rad;
satrec.alta = satrec.a * (1.0 + satrec.ecco) - 1.0; satrec.alta = satrec.a * (1.0 + satrec.ecco) - 1.0;
satrec.altp = satrec.a * (1.0 - satrec.ecco) - 1.0; satrec.altp = satrec.a * (1.0 - satrec.ecco) - 1.0;
// ---------------------------------------------------------------- // ----------------------------------------------------------------
// find sgp4epoch time of element set // find sgp4epoch time of element set
// remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch) // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch)
// and minutes from the epoch (time) // and minutes from the epoch (time)
// ---------------------------------------------------------------- // ----------------------------------------------------------------
// ---------------- temp fix for years from 1957-2056 ------------------- // ---------------- temp fix for years from 1957-2056 -------------------
// --------- correct fix will occur when year is 4-digit in tle --------- // --------- correct fix will occur when year is 4-digit in tle ---------
if (satrec.epochyr < 57) { if (satrec.epochyr < 57) {
year = satrec.epochyr + 2000; year = satrec.epochyr + 2000;
} else { } else {
year = satrec.epochyr + 1900; year = satrec.epochyr + 1900;
} }
days2mdhms(year, satrec.epochdays, mon, day, hr, minute, sec); days2mdhms(year, satrec.epochdays, mon, day, hr, minute, sec);
jday(year, mon, day, hr, minute, sec, satrec.jdsatepoch); jday(year, mon, day, hr, minute, sec, satrec.jdsatepoch);
double unixSeconds = (satrec.jdsatepoch - 2451544.5) * 24 * 3600 double unixSeconds = (satrec.jdsatepoch - 2451544.5) * 24 * 3600
+ 946684800; + 946684800;
epoch.tv_sec = unixSeconds; epoch.tv_sec = unixSeconds;
double subseconds = unixSeconds - epoch.tv_sec; double subseconds = unixSeconds - epoch.tv_sec;
epoch.tv_usec = subseconds * 1000000; epoch.tv_usec = subseconds * 1000000;
// ---------------- initialize the orbit at sgp4epoch ------------------- // ---------------- initialize the orbit at sgp4epoch -------------------
uint8_t result = sgp4init(whichconst, satrec.satnum, uint8_t result = sgp4init(whichconst, satrec.satnum,
satrec.jdsatepoch - 2433281.5, satrec.bstar, satrec.ecco, satrec.jdsatepoch - 2433281.5, satrec.bstar, satrec.ecco,
satrec.argpo, satrec.inclo, satrec.mo, satrec.no, satrec.nodeo, satrec.argpo, satrec.inclo, satrec.mo, satrec.no, satrec.nodeo,
satrec); satrec);
if (result != 00) { if (result != 00) {
return MAKE_RETURN_CODE(result); return MAKE_RETURN_CODE(result);
} else { } else {
initialized = true; initialized = true;
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
} }
ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity, ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity,
timeval time, uint8_t gpsUtcOffset) { timeval time, uint8_t gpsUtcOffset) {
if (!initialized) { if (!initialized) {
return TLE_NOT_INITIALIZED; return TLE_NOT_INITIALIZED;
} }
//Time since epoch in minutes //Time since epoch in minutes
timeval timeSinceEpoch = time - epoch; timeval timeSinceEpoch = time - epoch;
double minutesSinceEpoch = timeSinceEpoch.tv_sec / 60. double minutesSinceEpoch = timeSinceEpoch.tv_sec / 60.
+ timeSinceEpoch.tv_usec / 60000000.; + timeSinceEpoch.tv_usec / 60000000.;
double yearsSinceEpoch = minutesSinceEpoch / 60 / 24 / 365; double yearsSinceEpoch = minutesSinceEpoch / 60 / 24 / 365;
if ((yearsSinceEpoch > 1) || (yearsSinceEpoch < -1)) { if ((yearsSinceEpoch > 1) || (yearsSinceEpoch < -1)) {
return TLE_TOO_OLD; return TLE_TOO_OLD;
} }
double positionTEME[3]; double positionTEME[3];
double velocityTEME[3]; double velocityTEME[3];
uint8_t result = sgp4(whichconst, satrec, minutesSinceEpoch, positionTEME, uint8_t result = sgp4(whichconst, satrec, minutesSinceEpoch, positionTEME,
velocityTEME); velocityTEME);
VectorOperations<double>::mulScalar(positionTEME, 1000, positionTEME, 3); VectorOperations<double>::mulScalar(positionTEME, 1000, positionTEME, 3);
VectorOperations<double>::mulScalar(velocityTEME, 1000, velocityTEME, 3); VectorOperations<double>::mulScalar(velocityTEME, 1000, velocityTEME, 3);
//Transform to ECF //Transform to ECF
double earthRotationMatrix[3][3]; double earthRotationMatrix[3][3];
CoordinateTransformations::getEarthRotationMatrix(time, CoordinateTransformations::getEarthRotationMatrix(time,
earthRotationMatrix); earthRotationMatrix);
MatrixOperations<double>::multiply(earthRotationMatrix[0], positionTEME, MatrixOperations<double>::multiply(earthRotationMatrix[0], positionTEME,
position, 3, 3, 1); position, 3, 3, 1);
MatrixOperations<double>::multiply(earthRotationMatrix[0], velocityTEME, MatrixOperations<double>::multiply(earthRotationMatrix[0], velocityTEME,
velocity, 3, 3, 1); velocity, 3, 3, 1);
double omegaEarth[3] = { 0, 0, Earth::OMEGA }; double omegaEarth[3] = { 0, 0, Earth::OMEGA };
double velocityCorrection[3]; double velocityCorrection[3];
VectorOperations<double>::cross(omegaEarth, position, velocityCorrection); VectorOperations<double>::cross(omegaEarth, position, velocityCorrection);
VectorOperations<double>::subtract(velocity, velocityCorrection, velocity); VectorOperations<double>::subtract(velocity, velocityCorrection, velocity);
if (result != 0) { if (result != 0) {
return MAKE_RETURN_CODE(result || 0xB0); return MAKE_RETURN_CODE(result || 0xB0);
} else { } else {
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
} }

88
coordinates/Sgp4Propagator.h
View File

@ -1,44 +1,44 @@
#ifndef SGP4PROPAGATOR_H_ #ifndef SGP4PROPAGATOR_H_
#define SGP4PROPAGATOR_H_ #define SGP4PROPAGATOR_H_
#include <sys/time.h> #include <sys/time.h>
#include "../contrib/sgp4/sgp4unit.h" #include "../contrib/sgp4/sgp4unit.h"
#include "../returnvalues/HasReturnvaluesIF.h" #include "../returnvalues/HasReturnvaluesIF.h"
class Sgp4Propagator { class Sgp4Propagator {
public: public:
static const uint8_t INTERFACE_ID = CLASS_ID::SGP4PROPAGATOR_CLASS; static const uint8_t INTERFACE_ID = CLASS_ID::SGP4PROPAGATOR_CLASS;
static const ReturnValue_t INVALID_ECCENTRICITY = MAKE_RETURN_CODE(0xA1); static const ReturnValue_t INVALID_ECCENTRICITY = MAKE_RETURN_CODE(0xA1);
static const ReturnValue_t INVALID_MEAN_MOTION = MAKE_RETURN_CODE(0xA2); static const ReturnValue_t INVALID_MEAN_MOTION = MAKE_RETURN_CODE(0xA2);
static const ReturnValue_t INVALID_PERTURBATION_ELEMENTS = MAKE_RETURN_CODE(0xA3); static const ReturnValue_t INVALID_PERTURBATION_ELEMENTS = MAKE_RETURN_CODE(0xA3);
static const ReturnValue_t INVALID_SEMI_LATUS_RECTUM = MAKE_RETURN_CODE(0xA4); static const ReturnValue_t INVALID_SEMI_LATUS_RECTUM = MAKE_RETURN_CODE(0xA4);
static const ReturnValue_t INVALID_EPOCH_ELEMENTS = MAKE_RETURN_CODE(0xA5); static const ReturnValue_t INVALID_EPOCH_ELEMENTS = MAKE_RETURN_CODE(0xA5);
static const ReturnValue_t SATELLITE_HAS_DECAYED = MAKE_RETURN_CODE(0xA6); static const ReturnValue_t SATELLITE_HAS_DECAYED = MAKE_RETURN_CODE(0xA6);
static const ReturnValue_t TLE_TOO_OLD = MAKE_RETURN_CODE(0xB1); static const ReturnValue_t TLE_TOO_OLD = MAKE_RETURN_CODE(0xB1);
static const ReturnValue_t TLE_NOT_INITIALIZED = MAKE_RETURN_CODE(0xB2); static const ReturnValue_t TLE_NOT_INITIALIZED = MAKE_RETURN_CODE(0xB2);
Sgp4Propagator(); Sgp4Propagator();
virtual ~Sgp4Propagator(); virtual ~Sgp4Propagator();
ReturnValue_t initialize(const uint8_t *line1, const uint8_t *line2); ReturnValue_t initialize(const uint8_t *line1, const uint8_t *line2);
/** /**
* *
* @param[out] position in ECF * @param[out] position in ECF
* @param[out] velocity in ECF * @param[out] velocity in ECF
* @param time to which to propagate * @param time to which to propagate
* @return * @return
*/ */
ReturnValue_t propagate(double *position, double *velocity, timeval time, uint8_t gpsUtcOffset); ReturnValue_t propagate(double *position, double *velocity, timeval time, uint8_t gpsUtcOffset);
private: private:
bool initialized; bool initialized;
timeval epoch; timeval epoch;
elsetrec satrec; elsetrec satrec;
gravconsttype whichconst; gravconsttype whichconst;
}; };
#endif /* SGP4PROPAGATOR_H_ */ #endif /* SGP4PROPAGATOR_H_ */