diff --git a/coordinates/CoordinateTransformations.cpp b/coordinates/CoordinateTransformations.cpp index df9b3cac..4e2debbe 100644 --- a/coordinates/CoordinateTransformations.cpp +++ b/coordinates/CoordinateTransformations.cpp @@ -1,227 +1,227 @@ -#include "../coordinates/CoordinateTransformations.h" -#include "../globalfunctions/constants.h" -#include "../globalfunctions/math/MatrixOperations.h" -#include "../globalfunctions/math/VectorOperations.h" -#include -#include - - -void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, - double* eciPosition, timeval *timeUTC) { - ecfToEci(ecfPosition, eciPosition, NULL, timeUTC); - -} - -void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity, - const double* ecfPosition, double* eciVelocity, timeval *timeUTC) { - ecfToEci(ecfVelocity, eciVelocity, ecfPosition, timeUTC); -} - -void CoordinateTransformations::positionEciToEcf(const double* eciCoordinates, double* ecfCoordinates,timeval *timeUTC){ - eciToEcf(eciCoordinates,ecfCoordinates,NULL,timeUTC); -}; - -void CoordinateTransformations::velocityEciToEcf(const double* eciVelocity,const double* eciPosition, double* ecfVelocity,timeval* timeUTC){ - eciToEcf(eciVelocity,ecfVelocity,eciPosition,timeUTC); -} - -double CoordinateTransformations::getEarthRotationAngle(timeval timeUTC) { - - double jD2000UTC; - Clock::convertTimevalToJD2000(timeUTC, &jD2000UTC); - - double TTt2000 = getJuleanCenturiesTT(timeUTC); - - double theta = 2 * Math::PI - * (0.779057273264 + 1.00273781191135448 * jD2000UTC); - - //Correct theta according to IAU 2000 precession-nutation model - theta = theta + 7.03270725817493E-008 + 0.0223603701 * TTt2000 - + 6.77128219501896E-006 * TTt2000 * TTt2000 - + 4.5300990362875E-010 * TTt2000 * TTt2000 * TTt2000 - + 9.12419347848147E-011 * TTt2000 * TTt2000 * TTt2000 * TTt2000; - return theta; -} - -void CoordinateTransformations::getEarthRotationMatrix(timeval timeUTC, - double matrix[][3]) { - double theta = getEarthRotationAngle(timeUTC); - - matrix[0][0] = cos(theta); - matrix[0][1] = sin(theta); - matrix[0][2] = 0; - matrix[1][0] = -sin(theta); - matrix[1][1] = cos(theta); - matrix[1][2] = 0; - matrix[2][0] = 0; - matrix[2][1] = 0; - matrix[2][2] = 1; -} - -void CoordinateTransformations::ecfToEci(const double* ecfCoordinates, - double* eciCoordinates, - const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin) { - - timeval timeUTC; - if (timeUTCin != NULL) { - timeUTC = *timeUTCin; - } else { - Clock::getClock_timeval(&timeUTC); - } - - double Tfi[3][3]; - double Tif[3][3]; - getTransMatrixECITOECF(timeUTC,Tfi); - - MatrixOperations::transpose(Tfi[0], Tif[0], 3); - - MatrixOperations::multiply(Tif[0], ecfCoordinates, eciCoordinates, - 3, 3, 1); - - - - - if (ecfPositionIfCoordinatesAreVelocity != NULL) { - - double Tdotfi[3][3]; - double Tdotif[3][3]; - double Trot[3][3] = { { 0, Earth::OMEGA, 0 }, - { 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } }; - - MatrixOperations::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3, - 3); - - MatrixOperations::transpose(Tdotfi[0], Tdotif[0], 3); - - double velocityCorrection[3]; - - MatrixOperations::multiply(Tdotif[0], - ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3, - 1); - - VectorOperations::add(velocityCorrection, eciCoordinates, - eciCoordinates, 3); - } -} - -double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) { - timeval timeTT; - Clock::convertUTCToTT(timeUTC, &timeTT); - double jD2000TT; - Clock::convertTimevalToJD2000(timeTT, &jD2000TT); - - return jD2000TT / 36525.; -} - -void CoordinateTransformations::eciToEcf(const double* eciCoordinates, - double* ecfCoordinates, - const double* eciPositionIfCoordinatesAreVelocity,timeval *timeUTCin){ - timeval timeUTC; - if (timeUTCin != NULL) { - timeUTC = *timeUTCin; - }else{ - Clock::getClock_timeval(&timeUTC); - } - - double Tfi[3][3]; - - getTransMatrixECITOECF(timeUTC,Tfi); - - MatrixOperations::multiply(Tfi[0],eciCoordinates,ecfCoordinates,3,3,1); - - if (eciPositionIfCoordinatesAreVelocity != NULL) { - - double Tdotfi[3][3]; - double Trot[3][3] = { { 0, Earth::OMEGA, 0 }, - { 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } }; - - MatrixOperations::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3, - 3); - - double velocityCorrection[3]; - - MatrixOperations::multiply(Tdotfi[0], - eciPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3, - 1); - - VectorOperations::add(ecfCoordinates, velocityCorrection, - ecfCoordinates, 3); - } -}; - -void CoordinateTransformations::getTransMatrixECITOECF(timeval timeUTC,double Tfi[3][3]){ - double TTt2000 = getJuleanCenturiesTT(timeUTC); - - ////////////////////////////////////////////////////////// - // Calculate Precession Matrix - - double zeta = 0.0111808609 * TTt2000 - + 1.46355554053347E-006 * TTt2000 * TTt2000 - + 8.72567663260943E-008 * TTt2000 * TTt2000 * TTt2000; - double theta_p = 0.0097171735 * TTt2000 - - 2.06845757045384E-006 * TTt2000 * TTt2000 - - 2.02812107218552E-007 * TTt2000 * TTt2000 * TTt2000; - double z = zeta + 3.8436028638364E-006 * TTt2000 * TTt2000 - + 0.000000001 * TTt2000 * TTt2000 * TTt2000; - - double mPrecession[3][3]; - - 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[2][0] = sin(theta_p) * cos(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[2][1] = -sin(theta_p) * sin(zeta); - - mPrecession[0][2] = -cos(z) * sin(theta_p); - mPrecession[1][2] = -sin(z) * sin(theta_p); - mPrecession[2][2] = cos(theta_p); - - ////////////////////////////////////////////////////////// - // Calculate Nutation Matrix - - double omega_moon = 2.1824386244 - 33.7570459338 * TTt2000 - + 3.61428599267159E-005 * TTt2000 * TTt2000 - + 3.87850944887629E-008 * TTt2000 * TTt2000 * TTt2000; - - double deltaPsi = -0.000083388 * sin(omega_moon); - double deltaEpsilon = 4.46174030725106E-005 * cos(omega_moon); - - double epsilon = 0.4090928042 - 0.0002269655 * TTt2000 - - 2.86040071854626E-009 * TTt2000 * TTt2000 - + 8.78967203851589E-009 * TTt2000 * TTt2000 * TTt2000; - - - double mNutation[3][3]; - - mNutation[0][0] = cos(deltaPsi); - mNutation[1][0] = cos(epsilon + deltaEpsilon) * sin(deltaPsi); - mNutation[2][0] = sin(epsilon + deltaEpsilon) * sin(deltaPsi); - - mNutation[0][1] = -cos(epsilon) * sin(deltaPsi); - mNutation[1][1] = cos(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) - + sin(epsilon) * sin(epsilon + deltaEpsilon); - mNutation[2][1] = cos(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) - - sin(epsilon) * cos(epsilon + deltaEpsilon); - - mNutation[0][2] = -sin(epsilon) * sin(deltaPsi); - mNutation[1][2] = sin(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) - - cos(epsilon) * sin(epsilon + deltaEpsilon); - mNutation[2][2] = sin(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) - + cos(epsilon) * cos(epsilon + deltaEpsilon); - - ////////////////////////////////////////////////////////// - // Calculate Earth rotation matrix - //calculate theta - - double mTheta[3][3]; - double Ttemp[3][3]; - getEarthRotationMatrix(timeUTC, mTheta); - - //polar motion is neglected - MatrixOperations::multiply(mNutation[0], mPrecession[0], Ttemp[0], - 3, 3, 3); - - MatrixOperations::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3); -}; +#include "CoordinateTransformations.h" +#include "../globalfunctions/constants.h" +#include "../globalfunctions/math/MatrixOperations.h" +#include "../globalfunctions/math/VectorOperations.h" +#include +#include + + +void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, + double* eciPosition, timeval *timeUTC) { + ecfToEci(ecfPosition, eciPosition, NULL, timeUTC); + +} + +void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity, + const double* ecfPosition, double* eciVelocity, timeval *timeUTC) { + ecfToEci(ecfVelocity, eciVelocity, ecfPosition, timeUTC); +} + +void CoordinateTransformations::positionEciToEcf(const double* eciCoordinates, double* ecfCoordinates,timeval *timeUTC){ + eciToEcf(eciCoordinates,ecfCoordinates,NULL,timeUTC); +}; + +void CoordinateTransformations::velocityEciToEcf(const double* eciVelocity,const double* eciPosition, double* ecfVelocity,timeval* timeUTC){ + eciToEcf(eciVelocity,ecfVelocity,eciPosition,timeUTC); +} + +double CoordinateTransformations::getEarthRotationAngle(timeval timeUTC) { + + double jD2000UTC; + Clock::convertTimevalToJD2000(timeUTC, &jD2000UTC); + + double TTt2000 = getJuleanCenturiesTT(timeUTC); + + double theta = 2 * Math::PI + * (0.779057273264 + 1.00273781191135448 * jD2000UTC); + + //Correct theta according to IAU 2000 precession-nutation model + theta = theta + 7.03270725817493E-008 + 0.0223603701 * TTt2000 + + 6.77128219501896E-006 * TTt2000 * TTt2000 + + 4.5300990362875E-010 * TTt2000 * TTt2000 * TTt2000 + + 9.12419347848147E-011 * TTt2000 * TTt2000 * TTt2000 * TTt2000; + return theta; +} + +void CoordinateTransformations::getEarthRotationMatrix(timeval timeUTC, + double matrix[][3]) { + double theta = getEarthRotationAngle(timeUTC); + + matrix[0][0] = cos(theta); + matrix[0][1] = sin(theta); + matrix[0][2] = 0; + matrix[1][0] = -sin(theta); + matrix[1][1] = cos(theta); + matrix[1][2] = 0; + matrix[2][0] = 0; + matrix[2][1] = 0; + matrix[2][2] = 1; +} + +void CoordinateTransformations::ecfToEci(const double* ecfCoordinates, + double* eciCoordinates, + const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin) { + + timeval timeUTC; + if (timeUTCin != NULL) { + timeUTC = *timeUTCin; + } else { + Clock::getClock_timeval(&timeUTC); + } + + double Tfi[3][3]; + double Tif[3][3]; + getTransMatrixECITOECF(timeUTC,Tfi); + + MatrixOperations::transpose(Tfi[0], Tif[0], 3); + + MatrixOperations::multiply(Tif[0], ecfCoordinates, eciCoordinates, + 3, 3, 1); + + + + + if (ecfPositionIfCoordinatesAreVelocity != NULL) { + + double Tdotfi[3][3]; + double Tdotif[3][3]; + double Trot[3][3] = { { 0, Earth::OMEGA, 0 }, + { 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } }; + + MatrixOperations::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3, + 3); + + MatrixOperations::transpose(Tdotfi[0], Tdotif[0], 3); + + double velocityCorrection[3]; + + MatrixOperations::multiply(Tdotif[0], + ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3, + 1); + + VectorOperations::add(velocityCorrection, eciCoordinates, + eciCoordinates, 3); + } +} + +double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) { + timeval timeTT; + Clock::convertUTCToTT(timeUTC, &timeTT); + double jD2000TT; + Clock::convertTimevalToJD2000(timeTT, &jD2000TT); + + return jD2000TT / 36525.; +} + +void CoordinateTransformations::eciToEcf(const double* eciCoordinates, + double* ecfCoordinates, + const double* eciPositionIfCoordinatesAreVelocity,timeval *timeUTCin){ + timeval timeUTC; + if (timeUTCin != NULL) { + timeUTC = *timeUTCin; + }else{ + Clock::getClock_timeval(&timeUTC); + } + + double Tfi[3][3]; + + getTransMatrixECITOECF(timeUTC,Tfi); + + MatrixOperations::multiply(Tfi[0],eciCoordinates,ecfCoordinates,3,3,1); + + if (eciPositionIfCoordinatesAreVelocity != NULL) { + + double Tdotfi[3][3]; + double Trot[3][3] = { { 0, Earth::OMEGA, 0 }, + { 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } }; + + MatrixOperations::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3, + 3); + + double velocityCorrection[3]; + + MatrixOperations::multiply(Tdotfi[0], + eciPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3, + 1); + + VectorOperations::add(ecfCoordinates, velocityCorrection, + ecfCoordinates, 3); + } +}; + +void CoordinateTransformations::getTransMatrixECITOECF(timeval timeUTC,double Tfi[3][3]){ + double TTt2000 = getJuleanCenturiesTT(timeUTC); + + ////////////////////////////////////////////////////////// + // Calculate Precession Matrix + + double zeta = 0.0111808609 * TTt2000 + + 1.46355554053347E-006 * TTt2000 * TTt2000 + + 8.72567663260943E-008 * TTt2000 * TTt2000 * TTt2000; + double theta_p = 0.0097171735 * TTt2000 + - 2.06845757045384E-006 * TTt2000 * TTt2000 + - 2.02812107218552E-007 * TTt2000 * TTt2000 * TTt2000; + double z = zeta + 3.8436028638364E-006 * TTt2000 * TTt2000 + + 0.000000001 * TTt2000 * TTt2000 * TTt2000; + + double mPrecession[3][3]; + + 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[2][0] = sin(theta_p) * cos(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[2][1] = -sin(theta_p) * sin(zeta); + + mPrecession[0][2] = -cos(z) * sin(theta_p); + mPrecession[1][2] = -sin(z) * sin(theta_p); + mPrecession[2][2] = cos(theta_p); + + ////////////////////////////////////////////////////////// + // Calculate Nutation Matrix + + double omega_moon = 2.1824386244 - 33.7570459338 * TTt2000 + + 3.61428599267159E-005 * TTt2000 * TTt2000 + + 3.87850944887629E-008 * TTt2000 * TTt2000 * TTt2000; + + double deltaPsi = -0.000083388 * sin(omega_moon); + double deltaEpsilon = 4.46174030725106E-005 * cos(omega_moon); + + double epsilon = 0.4090928042 - 0.0002269655 * TTt2000 + - 2.86040071854626E-009 * TTt2000 * TTt2000 + + 8.78967203851589E-009 * TTt2000 * TTt2000 * TTt2000; + + + double mNutation[3][3]; + + mNutation[0][0] = cos(deltaPsi); + mNutation[1][0] = cos(epsilon + deltaEpsilon) * sin(deltaPsi); + mNutation[2][0] = sin(epsilon + deltaEpsilon) * sin(deltaPsi); + + mNutation[0][1] = -cos(epsilon) * sin(deltaPsi); + mNutation[1][1] = cos(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) + + sin(epsilon) * sin(epsilon + deltaEpsilon); + mNutation[2][1] = cos(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) + - sin(epsilon) * cos(epsilon + deltaEpsilon); + + mNutation[0][2] = -sin(epsilon) * sin(deltaPsi); + mNutation[1][2] = sin(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) + - cos(epsilon) * sin(epsilon + deltaEpsilon); + mNutation[2][2] = sin(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) + + cos(epsilon) * cos(epsilon + deltaEpsilon); + + ////////////////////////////////////////////////////////// + // Calculate Earth rotation matrix + //calculate theta + + double mTheta[3][3]; + double Ttemp[3][3]; + getEarthRotationMatrix(timeUTC, mTheta); + + //polar motion is neglected + MatrixOperations::multiply(mNutation[0], mPrecession[0], Ttemp[0], + 3, 3, 3); + + MatrixOperations::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3); +}; diff --git a/coordinates/CoordinateTransformations.h b/coordinates/CoordinateTransformations.h index 0bfb203d..09ea2c48 100644 --- a/coordinates/CoordinateTransformations.h +++ b/coordinates/CoordinateTransformations.h @@ -1,34 +1,34 @@ -#ifndef COORDINATETRANSFORMATIONS_H_ -#define COORDINATETRANSFORMATIONS_H_ - -#include "../timemanager/Clock.h" -#include - -class CoordinateTransformations { -public: - static void positionEcfToEci(const double* ecfCoordinates, double* eciCoordinates, timeval *timeUTC = NULL); - - static void velocityEcfToEci(const double* ecfVelocity, - const double* ecfPosition, - double* eciVelocity, 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 double getEarthRotationAngle(timeval timeUTC); - - static void getEarthRotationMatrix(timeval timeUTC, double matrix[][3]); -private: - CoordinateTransformations(); - static void ecfToEci(const double* ecfCoordinates, double* eciCoordinates, - const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin); - static void eciToEcf(const double* eciCoordinates, - double* ecfCoordinates, - const double* eciPositionIfCoordinatesAreVelocity,timeval *timeUTCin); - - static double getJuleanCenturiesTT(timeval timeUTC); - static void getTransMatrixECITOECF(timeval time,double Tfi[3][3]); - -}; - -#endif /* COORDINATETRANSFORMATIONS_H_ */ +#ifndef COORDINATETRANSFORMATIONS_H_ +#define COORDINATETRANSFORMATIONS_H_ + +#include "../timemanager/Clock.h" +#include + +class CoordinateTransformations { +public: + static void positionEcfToEci(const double* ecfCoordinates, double* eciCoordinates, timeval *timeUTC = NULL); + + static void velocityEcfToEci(const double* ecfVelocity, + const double* ecfPosition, + double* eciVelocity, 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 double getEarthRotationAngle(timeval timeUTC); + + static void getEarthRotationMatrix(timeval timeUTC, double matrix[][3]); +private: + CoordinateTransformations(); + static void ecfToEci(const double* ecfCoordinates, double* eciCoordinates, + const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin); + static void eciToEcf(const double* eciCoordinates, + double* ecfCoordinates, + const double* eciPositionIfCoordinatesAreVelocity,timeval *timeUTCin); + + static double getJuleanCenturiesTT(timeval timeUTC); + static void getTransMatrixECITOECF(timeval time,double Tfi[3][3]); + +}; + +#endif /* COORDINATETRANSFORMATIONS_H_ */ diff --git a/coordinates/Jgm3Model.h b/coordinates/Jgm3Model.h index fd74741d..884ed141 100644 --- a/coordinates/Jgm3Model.h +++ b/coordinates/Jgm3Model.h @@ -1,180 +1,180 @@ -#ifndef FRAMEWORK_COORDINATES_JGM3MODEL_H_ -#define FRAMEWORK_COORDINATES_JGM3MODEL_H_ - -#include -#include "../coordinates/CoordinateTransformations.h" -#include "../globalfunctions/math/VectorOperations.h" -#include "../globalfunctions/timevalOperations.h" -#include "../globalfunctions/constants.h" -#include - - -template -class Jgm3Model { -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 - - Jgm3Model() { - y0[0] = 0; - y0[1] = 0; - y0[2] = 0; - y0[3] = 0; - y0[4] = 0; - y0[5] = 0; - - lastExecutionTime.tv_sec = 0; - lastExecutionTime.tv_usec = 0; - } - virtual ~Jgm3Model(){}; - - //double acsNavOrbit(double posECF[3],double velECF[3],timeval gpsTime); - - double y0[6]; //position and velocity at beginning of RK step in EC - 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){ - //Get radius of this position - double r = VectorOperations::norm(pos,3); - - - - //Initialize the V and W matrix - double V[DEGREE+2][ORDER+2] = {{0}}; - double W[DEGREE+2][ORDER+2] = {{0}}; - - for(uint8_t m=0;m<(ORDER+2);m++){ - for(uint8_t n=m;n<(DEGREE+2);n++){ - if((n==0) && (m==0)){ - //Montenbruck "Satellite Orbits Eq.3.31" - V[0][0] = Earth::MEAN_RADIUS / r; - W[0][0] = 0; - }else{ - if(n==m){ - //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]); - 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{ - //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]; - 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)){ - 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]); - }//End of if(n!=(m+1)) - }//End of if(n==m){ - }//End of if(n==0 and m==0) - }//End of for(uint8_t n=0;n<(DEGREE+1);n++) - }//End of for(uint8_t m=0;m<(ORDER+1);m++) - - - //overwrite accel if not properly initialized - accel[0] = 0; - accel[1] = 0; - accel[2] = 0; - - for(uint8_t m=0;m<(ORDER+1);m++){ - for(uint8_t n=m;n<(DEGREE+1);n++){ - //Use table lookup to get factorial - double partAccel[3] = {0}; - double factor = Earth::STANDARD_GRAVITATIONAL_PARAMETER/pow(Earth::MEAN_RADIUS,2); - if(m==0){ - //Montenbruck "Satellite Orbits Eq.3.33" - partAccel[0] = factor * (-C[n][0]*V[n+1][1]); - partAccel[1] = factor * (-C[n][0]*W[n+1][1]); - }else{ - double factMN = static_cast(factorialLookupTable[n-m+2]) / static_cast(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[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])); - - - accel[0] += partAccel[0]; - accel[1] += partAccel[1]; - accel[2] += partAccel[2]; - }//End of for(uint8_t n=0;n::mulScalar(y0dot,deltaT/2,yA,6); - VectorOperations::add(y0,yA,yA,6); - rungeKuttaStep(yA,yAdot,lastExecutionTime,S,C); - - //Step Three - VectorOperations::mulScalar(yAdot,deltaT/2,yB,6); - VectorOperations::add(y0,yB,yB,6); - rungeKuttaStep(yB,yBdot,lastExecutionTime,S,C); - - //Step Four - VectorOperations::mulScalar(yBdot,deltaT,yC,6); - VectorOperations::add(y0,yC,yC,6); - rungeKuttaStep(yC,yCdot,lastExecutionTime,S,C); - - //Calc new State - VectorOperations::mulScalar(yAdot,2,yAdot,6); - VectorOperations::mulScalar(yBdot,2,yBdot,6); - VectorOperations::add(y0dot,yAdot,y0dot,6); - VectorOperations::add(y0dot,yBdot,y0dot,6); - VectorOperations::add(y0dot,yCdot,y0dot,6); - VectorOperations::mulScalar(y0dot,1./6.*deltaT,y0dot,6); - VectorOperations::add(y0,y0dot,y0,6); - - CoordinateTransformations::positionEciToEcf(&y0[0],outputPos,&timeUTC); - CoordinateTransformations::velocityEciToEcf(&y0[3],&y0[0],outputVel,&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]){ - double rECF[3] = {0,0,0}; - double rDotECF[3] = {0,0,0}; - double accelECF[3] = {0,0,0}; - double accelECI[3] = {0,0,0}; - - - CoordinateTransformations::positionEciToEcf(&yIn[0],rECF,&time); - CoordinateTransformations::velocityEciToEcf(&yIn[3],&yIn[0],rDotECF,&time); - 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 - //Tests showed that a wrong velocity does make the equation worse than neglecting it - CoordinateTransformations::positionEcfToEci(accelECF,accelECI,&time); - memcpy(&yOut[0],&yIn[3],sizeof(yOut[0])*3); - memcpy(&yOut[3],accelECI,sizeof(yOut[0])*3); - } -}; - - - - - - -#endif /* FRAMEWORK_COORDINATES_JGM3MODEL_H_ */ +#ifndef FRAMEWORK_COORDINATES_JGM3MODEL_H_ +#define FRAMEWORK_COORDINATES_JGM3MODEL_H_ + +#include +#include "CoordinateTransformations.h" +#include "../globalfunctions/math/VectorOperations.h" +#include "../globalfunctions/timevalOperations.h" +#include "../globalfunctions/constants.h" +#include + + +template +class Jgm3Model { +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 + + Jgm3Model() { + y0[0] = 0; + y0[1] = 0; + y0[2] = 0; + y0[3] = 0; + y0[4] = 0; + y0[5] = 0; + + lastExecutionTime.tv_sec = 0; + lastExecutionTime.tv_usec = 0; + } + virtual ~Jgm3Model(){}; + + //double acsNavOrbit(double posECF[3],double velECF[3],timeval gpsTime); + + double y0[6]; //position and velocity at beginning of RK step in EC + 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){ + //Get radius of this position + double r = VectorOperations::norm(pos,3); + + + + //Initialize the V and W matrix + double V[DEGREE+2][ORDER+2] = {{0}}; + double W[DEGREE+2][ORDER+2] = {{0}}; + + for(uint8_t m=0;m<(ORDER+2);m++){ + for(uint8_t n=m;n<(DEGREE+2);n++){ + if((n==0) && (m==0)){ + //Montenbruck "Satellite Orbits Eq.3.31" + V[0][0] = Earth::MEAN_RADIUS / r; + W[0][0] = 0; + }else{ + if(n==m){ + //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]); + 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{ + //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]; + 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)){ + 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]); + }//End of if(n!=(m+1)) + }//End of if(n==m){ + }//End of if(n==0 and m==0) + }//End of for(uint8_t n=0;n<(DEGREE+1);n++) + }//End of for(uint8_t m=0;m<(ORDER+1);m++) + + + //overwrite accel if not properly initialized + accel[0] = 0; + accel[1] = 0; + accel[2] = 0; + + for(uint8_t m=0;m<(ORDER+1);m++){ + for(uint8_t n=m;n<(DEGREE+1);n++){ + //Use table lookup to get factorial + double partAccel[3] = {0}; + double factor = Earth::STANDARD_GRAVITATIONAL_PARAMETER/pow(Earth::MEAN_RADIUS,2); + if(m==0){ + //Montenbruck "Satellite Orbits Eq.3.33" + partAccel[0] = factor * (-C[n][0]*V[n+1][1]); + partAccel[1] = factor * (-C[n][0]*W[n+1][1]); + }else{ + double factMN = static_cast(factorialLookupTable[n-m+2]) / static_cast(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[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])); + + + accel[0] += partAccel[0]; + accel[1] += partAccel[1]; + accel[2] += partAccel[2]; + }//End of for(uint8_t n=0;n::mulScalar(y0dot,deltaT/2,yA,6); + VectorOperations::add(y0,yA,yA,6); + rungeKuttaStep(yA,yAdot,lastExecutionTime,S,C); + + //Step Three + VectorOperations::mulScalar(yAdot,deltaT/2,yB,6); + VectorOperations::add(y0,yB,yB,6); + rungeKuttaStep(yB,yBdot,lastExecutionTime,S,C); + + //Step Four + VectorOperations::mulScalar(yBdot,deltaT,yC,6); + VectorOperations::add(y0,yC,yC,6); + rungeKuttaStep(yC,yCdot,lastExecutionTime,S,C); + + //Calc new State + VectorOperations::mulScalar(yAdot,2,yAdot,6); + VectorOperations::mulScalar(yBdot,2,yBdot,6); + VectorOperations::add(y0dot,yAdot,y0dot,6); + VectorOperations::add(y0dot,yBdot,y0dot,6); + VectorOperations::add(y0dot,yCdot,y0dot,6); + VectorOperations::mulScalar(y0dot,1./6.*deltaT,y0dot,6); + VectorOperations::add(y0,y0dot,y0,6); + + CoordinateTransformations::positionEciToEcf(&y0[0],outputPos,&timeUTC); + CoordinateTransformations::velocityEciToEcf(&y0[3],&y0[0],outputVel,&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]){ + double rECF[3] = {0,0,0}; + double rDotECF[3] = {0,0,0}; + double accelECF[3] = {0,0,0}; + double accelECI[3] = {0,0,0}; + + + CoordinateTransformations::positionEciToEcf(&yIn[0],rECF,&time); + CoordinateTransformations::velocityEciToEcf(&yIn[3],&yIn[0],rDotECF,&time); + 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 + //Tests showed that a wrong velocity does make the equation worse than neglecting it + CoordinateTransformations::positionEcfToEci(accelECF,accelECI,&time); + memcpy(&yOut[0],&yIn[3],sizeof(yOut[0])*3); + memcpy(&yOut[3],accelECI,sizeof(yOut[0])*3); + } +}; + + + + + + +#endif /* FRAMEWORK_COORDINATES_JGM3MODEL_H_ */ diff --git a/coordinates/Sgp4Propagator.cpp b/coordinates/Sgp4Propagator.cpp index 84a52799..5cb1497c 100644 --- a/coordinates/Sgp4Propagator.cpp +++ b/coordinates/Sgp4Propagator.cpp @@ -1,228 +1,228 @@ -#include "../coordinates/CoordinateTransformations.h" -#include "../coordinates/Sgp4Propagator.h" -#include "../globalfunctions/constants.h" -#include "../globalfunctions/math/MatrixOperations.h" -#include "../globalfunctions/math/VectorOperations.h" -#include "../globalfunctions/timevalOperations.h" -#include -Sgp4Propagator::Sgp4Propagator() : - initialized(false), epoch({0, 0}), whichconst(wgs84) { - -} - -Sgp4Propagator::~Sgp4Propagator() { - -} - -void jday(int year, int mon, int day, int hr, int minute, double sec, - double& jd) { - jd = 367.0 * year - floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25) - + floor(275 * mon / 9.0) + day + 1721013.5 - + ((sec / 60.0 + minute) / 60.0 + hr) / 24.0; // ut in days - // - 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, - double& sec) { - int i, inttemp, dayofyr; - double temp; - int lmonth[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; - - dayofyr = (int) floor(days); - /* ----------------- find month and day of month ---------------- */ - if ((year % 4) == 0) - lmonth[1] = 29; - - i = 1; - inttemp = 0; - while ((dayofyr > inttemp + lmonth[i - 1]) && (i < 12)) { - inttemp = inttemp + lmonth[i - 1]; - i++; - } - mon = i; - day = dayofyr - inttemp; - - /* ----------------- find hours minutes and seconds ------------- */ - temp = (days - dayofyr) * 24.0; - hr = (int) floor(temp); - temp = (temp - hr) * 60.0; - minute = (int) floor(temp); - sec = (temp - minute) * 60.0; -} - -ReturnValue_t Sgp4Propagator::initialize(const uint8_t* line1, - const uint8_t* line2) { - - char longstr1[130]; - char longstr2[130]; - - //need some space for decimal points - memcpy(longstr1, line1, 69); - memcpy(longstr2, line2, 69); - - const double deg2rad = Math::PI / 180.0; // 0.0174532925199433 - const double xpdotp = 1440.0 / (2.0 * Math::PI); // 229.1831180523293 - - double sec, mu, radiusearthkm, tumin, xke, j2, j3, j4, j3oj2; - int cardnumb, numb, j; - long revnum = 0, elnum = 0; - char classification, intldesg[11]; - int year = 0; - int mon, day, hr, minute, nexp, ibexp; - - getgravconst(whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2); - - satrec.error = 0; - - // set the implied decimal points since doing a formated read - // fixes for bad input data values (missing, ...) - for (j = 10; j <= 15; j++) - if (longstr1[j] == ' ') - longstr1[j] = '_'; - - if (longstr1[44] != ' ') - longstr1[43] = longstr1[44]; - longstr1[44] = '.'; - if (longstr1[7] == ' ') - longstr1[7] = 'U'; - if (longstr1[9] == ' ') - longstr1[9] = '.'; - for (j = 45; j <= 49; j++) - if (longstr1[j] == ' ') - longstr1[j] = '0'; - if (longstr1[51] == ' ') - longstr1[51] = '0'; - if (longstr1[53] != ' ') - longstr1[52] = longstr1[53]; - longstr1[53] = '.'; - longstr2[25] = '.'; - for (j = 26; j <= 32; j++) - if (longstr2[j] == ' ') - longstr2[j] = '0'; - if (longstr1[62] == ' ') - longstr1[62] = '0'; - if (longstr1[68] == ' ') - longstr1[68] = '0'; - - sscanf(longstr1, - "%2d %5ld %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ", - &cardnumb, &satrec.satnum, &classification, intldesg, - &satrec.epochyr, &satrec.epochdays, &satrec.ndot, &satrec.nddot, - &nexp, &satrec.bstar, &ibexp, &numb, &elnum); - - if (longstr2[52] == ' ') { - sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n", - &cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo, - &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum); - } else { - sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n", - &cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo, - &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum); - } - - // ---- find no, ndot, nddot ---- - satrec.no = satrec.no / xpdotp; //* rad/min - satrec.nddot = satrec.nddot * pow(10.0, nexp); - satrec.bstar = satrec.bstar * pow(10.0, ibexp); - - // ---- convert to sgp4 units ---- - satrec.a = pow(satrec.no * tumin, (-2.0 / 3.0)); - satrec.ndot = satrec.ndot / (xpdotp * 1440.0); //* ? * minperday - satrec.nddot = satrec.nddot / (xpdotp * 1440.0 * 1440); - - // ---- find standard orbital elements ---- - satrec.inclo = satrec.inclo * deg2rad; - satrec.nodeo = satrec.nodeo * deg2rad; - satrec.argpo = satrec.argpo * deg2rad; - satrec.mo = satrec.mo * deg2rad; - - satrec.alta = satrec.a * (1.0 + satrec.ecco) - 1.0; - satrec.altp = satrec.a * (1.0 - satrec.ecco) - 1.0; - - // ---------------------------------------------------------------- - // find sgp4epoch time of element set - // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch) - // and minutes from the epoch (time) - // ---------------------------------------------------------------- - - // ---------------- temp fix for years from 1957-2056 ------------------- - // --------- correct fix will occur when year is 4-digit in tle --------- - if (satrec.epochyr < 57) { - year = satrec.epochyr + 2000; - } else { - year = satrec.epochyr + 1900; - } - - days2mdhms(year, satrec.epochdays, mon, day, hr, minute, sec); - jday(year, mon, day, hr, minute, sec, satrec.jdsatepoch); - - double unixSeconds = (satrec.jdsatepoch - 2451544.5) * 24 * 3600 - + 946684800; - - epoch.tv_sec = unixSeconds; - double subseconds = unixSeconds - epoch.tv_sec; - epoch.tv_usec = subseconds * 1000000; - - // ---------------- initialize the orbit at sgp4epoch ------------------- - uint8_t result = sgp4init(whichconst, satrec.satnum, - satrec.jdsatepoch - 2433281.5, satrec.bstar, satrec.ecco, - satrec.argpo, satrec.inclo, satrec.mo, satrec.no, satrec.nodeo, - satrec); - - if (result != 00) { - return MAKE_RETURN_CODE(result); - } else { - initialized = true; - return HasReturnvaluesIF::RETURN_OK; - } - -} - -ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity, - timeval time, uint8_t gpsUtcOffset) { - - if (!initialized) { - return TLE_NOT_INITIALIZED; - } - - //Time since epoch in minutes - timeval timeSinceEpoch = time - epoch; - double minutesSinceEpoch = timeSinceEpoch.tv_sec / 60. - + timeSinceEpoch.tv_usec / 60000000.; - - double yearsSinceEpoch = minutesSinceEpoch / 60 / 24 / 365; - - if ((yearsSinceEpoch > 1) || (yearsSinceEpoch < -1)) { - return TLE_TOO_OLD; - } - - double positionTEME[3]; - double velocityTEME[3]; - - uint8_t result = sgp4(whichconst, satrec, minutesSinceEpoch, positionTEME, - velocityTEME); - - VectorOperations::mulScalar(positionTEME, 1000, positionTEME, 3); - VectorOperations::mulScalar(velocityTEME, 1000, velocityTEME, 3); - - //Transform to ECF - double earthRotationMatrix[3][3]; - CoordinateTransformations::getEarthRotationMatrix(time, - earthRotationMatrix); - - MatrixOperations::multiply(earthRotationMatrix[0], positionTEME, - position, 3, 3, 1); - MatrixOperations::multiply(earthRotationMatrix[0], velocityTEME, - velocity, 3, 3, 1); - - double omegaEarth[3] = { 0, 0, Earth::OMEGA }; - double velocityCorrection[3]; - VectorOperations::cross(omegaEarth, position, velocityCorrection); - VectorOperations::subtract(velocity, velocityCorrection, velocity); - - if (result != 0) { - return MAKE_RETURN_CODE(result || 0xB0); - } else { - return HasReturnvaluesIF::RETURN_OK; - } -} +#include "CoordinateTransformations.h" +#include "Sgp4Propagator.h" +#include "../globalfunctions/constants.h" +#include "../globalfunctions/math/MatrixOperations.h" +#include "../globalfunctions/math/VectorOperations.h" +#include "../globalfunctions/timevalOperations.h" +#include +Sgp4Propagator::Sgp4Propagator() : + initialized(false), epoch({0, 0}), whichconst(wgs84) { + +} + +Sgp4Propagator::~Sgp4Propagator() { + +} + +void jday(int year, int mon, int day, int hr, int minute, double sec, + double& jd) { + jd = 367.0 * year - floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25) + + floor(275 * mon / 9.0) + day + 1721013.5 + + ((sec / 60.0 + minute) / 60.0 + hr) / 24.0; // ut in days + // - 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, + double& sec) { + int i, inttemp, dayofyr; + double temp; + int lmonth[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; + + dayofyr = (int) floor(days); + /* ----------------- find month and day of month ---------------- */ + if ((year % 4) == 0) + lmonth[1] = 29; + + i = 1; + inttemp = 0; + while ((dayofyr > inttemp + lmonth[i - 1]) && (i < 12)) { + inttemp = inttemp + lmonth[i - 1]; + i++; + } + mon = i; + day = dayofyr - inttemp; + + /* ----------------- find hours minutes and seconds ------------- */ + temp = (days - dayofyr) * 24.0; + hr = (int) floor(temp); + temp = (temp - hr) * 60.0; + minute = (int) floor(temp); + sec = (temp - minute) * 60.0; +} + +ReturnValue_t Sgp4Propagator::initialize(const uint8_t* line1, + const uint8_t* line2) { + + char longstr1[130]; + char longstr2[130]; + + //need some space for decimal points + memcpy(longstr1, line1, 69); + memcpy(longstr2, line2, 69); + + const double deg2rad = Math::PI / 180.0; // 0.0174532925199433 + const double xpdotp = 1440.0 / (2.0 * Math::PI); // 229.1831180523293 + + double sec, mu, radiusearthkm, tumin, xke, j2, j3, j4, j3oj2; + int cardnumb, numb, j; + long revnum = 0, elnum = 0; + char classification, intldesg[11]; + int year = 0; + int mon, day, hr, minute, nexp, ibexp; + + getgravconst(whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2); + + satrec.error = 0; + + // set the implied decimal points since doing a formated read + // fixes for bad input data values (missing, ...) + for (j = 10; j <= 15; j++) + if (longstr1[j] == ' ') + longstr1[j] = '_'; + + if (longstr1[44] != ' ') + longstr1[43] = longstr1[44]; + longstr1[44] = '.'; + if (longstr1[7] == ' ') + longstr1[7] = 'U'; + if (longstr1[9] == ' ') + longstr1[9] = '.'; + for (j = 45; j <= 49; j++) + if (longstr1[j] == ' ') + longstr1[j] = '0'; + if (longstr1[51] == ' ') + longstr1[51] = '0'; + if (longstr1[53] != ' ') + longstr1[52] = longstr1[53]; + longstr1[53] = '.'; + longstr2[25] = '.'; + for (j = 26; j <= 32; j++) + if (longstr2[j] == ' ') + longstr2[j] = '0'; + if (longstr1[62] == ' ') + longstr1[62] = '0'; + if (longstr1[68] == ' ') + longstr1[68] = '0'; + + sscanf(longstr1, + "%2d %5ld %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ", + &cardnumb, &satrec.satnum, &classification, intldesg, + &satrec.epochyr, &satrec.epochdays, &satrec.ndot, &satrec.nddot, + &nexp, &satrec.bstar, &ibexp, &numb, &elnum); + + if (longstr2[52] == ' ') { + sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n", + &cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo, + &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum); + } else { + sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n", + &cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo, + &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum); + } + + // ---- find no, ndot, nddot ---- + satrec.no = satrec.no / xpdotp; //* rad/min + satrec.nddot = satrec.nddot * pow(10.0, nexp); + satrec.bstar = satrec.bstar * pow(10.0, ibexp); + + // ---- convert to sgp4 units ---- + satrec.a = pow(satrec.no * tumin, (-2.0 / 3.0)); + satrec.ndot = satrec.ndot / (xpdotp * 1440.0); //* ? * minperday + satrec.nddot = satrec.nddot / (xpdotp * 1440.0 * 1440); + + // ---- find standard orbital elements ---- + satrec.inclo = satrec.inclo * deg2rad; + satrec.nodeo = satrec.nodeo * deg2rad; + satrec.argpo = satrec.argpo * deg2rad; + satrec.mo = satrec.mo * deg2rad; + + satrec.alta = satrec.a * (1.0 + satrec.ecco) - 1.0; + satrec.altp = satrec.a * (1.0 - satrec.ecco) - 1.0; + + // ---------------------------------------------------------------- + // find sgp4epoch time of element set + // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch) + // and minutes from the epoch (time) + // ---------------------------------------------------------------- + + // ---------------- temp fix for years from 1957-2056 ------------------- + // --------- correct fix will occur when year is 4-digit in tle --------- + if (satrec.epochyr < 57) { + year = satrec.epochyr + 2000; + } else { + year = satrec.epochyr + 1900; + } + + days2mdhms(year, satrec.epochdays, mon, day, hr, minute, sec); + jday(year, mon, day, hr, minute, sec, satrec.jdsatepoch); + + double unixSeconds = (satrec.jdsatepoch - 2451544.5) * 24 * 3600 + + 946684800; + + epoch.tv_sec = unixSeconds; + double subseconds = unixSeconds - epoch.tv_sec; + epoch.tv_usec = subseconds * 1000000; + + // ---------------- initialize the orbit at sgp4epoch ------------------- + uint8_t result = sgp4init(whichconst, satrec.satnum, + satrec.jdsatepoch - 2433281.5, satrec.bstar, satrec.ecco, + satrec.argpo, satrec.inclo, satrec.mo, satrec.no, satrec.nodeo, + satrec); + + if (result != 00) { + return MAKE_RETURN_CODE(result); + } else { + initialized = true; + return HasReturnvaluesIF::RETURN_OK; + } + +} + +ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity, + timeval time, uint8_t gpsUtcOffset) { + + if (!initialized) { + return TLE_NOT_INITIALIZED; + } + + //Time since epoch in minutes + timeval timeSinceEpoch = time - epoch; + double minutesSinceEpoch = timeSinceEpoch.tv_sec / 60. + + timeSinceEpoch.tv_usec / 60000000.; + + double yearsSinceEpoch = minutesSinceEpoch / 60 / 24 / 365; + + if ((yearsSinceEpoch > 1) || (yearsSinceEpoch < -1)) { + return TLE_TOO_OLD; + } + + double positionTEME[3]; + double velocityTEME[3]; + + uint8_t result = sgp4(whichconst, satrec, minutesSinceEpoch, positionTEME, + velocityTEME); + + VectorOperations::mulScalar(positionTEME, 1000, positionTEME, 3); + VectorOperations::mulScalar(velocityTEME, 1000, velocityTEME, 3); + + //Transform to ECF + double earthRotationMatrix[3][3]; + CoordinateTransformations::getEarthRotationMatrix(time, + earthRotationMatrix); + + MatrixOperations::multiply(earthRotationMatrix[0], positionTEME, + position, 3, 3, 1); + MatrixOperations::multiply(earthRotationMatrix[0], velocityTEME, + velocity, 3, 3, 1); + + double omegaEarth[3] = { 0, 0, Earth::OMEGA }; + double velocityCorrection[3]; + VectorOperations::cross(omegaEarth, position, velocityCorrection); + VectorOperations::subtract(velocity, velocityCorrection, velocity); + + if (result != 0) { + return MAKE_RETURN_CODE(result || 0xB0); + } else { + return HasReturnvaluesIF::RETURN_OK; + } +} diff --git a/coordinates/Sgp4Propagator.h b/coordinates/Sgp4Propagator.h index f041ec07..3949547e 100644 --- a/coordinates/Sgp4Propagator.h +++ b/coordinates/Sgp4Propagator.h @@ -1,44 +1,44 @@ -#ifndef SGP4PROPAGATOR_H_ -#define SGP4PROPAGATOR_H_ - -#include -#include "../contrib/sgp4/sgp4unit.h" -#include "../returnvalues/HasReturnvaluesIF.h" - -class Sgp4Propagator { -public: - 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_MEAN_MOTION = MAKE_RETURN_CODE(0xA2); - 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_EPOCH_ELEMENTS = MAKE_RETURN_CODE(0xA5); - 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_NOT_INITIALIZED = MAKE_RETURN_CODE(0xB2); - - - - Sgp4Propagator(); - virtual ~Sgp4Propagator(); - - ReturnValue_t initialize(const uint8_t *line1, const uint8_t *line2); - - /** - * - * @param[out] position in ECF - * @param[out] velocity in ECF - * @param time to which to propagate - * @return - */ - ReturnValue_t propagate(double *position, double *velocity, timeval time, uint8_t gpsUtcOffset); - -private: - bool initialized; - timeval epoch; - elsetrec satrec; - gravconsttype whichconst; - -}; - -#endif /* SGP4PROPAGATOR_H_ */ +#ifndef SGP4PROPAGATOR_H_ +#define SGP4PROPAGATOR_H_ + +#include +#include "../contrib/sgp4/sgp4unit.h" +#include "../returnvalues/HasReturnvaluesIF.h" + +class Sgp4Propagator { +public: + 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_MEAN_MOTION = MAKE_RETURN_CODE(0xA2); + 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_EPOCH_ELEMENTS = MAKE_RETURN_CODE(0xA5); + 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_NOT_INITIALIZED = MAKE_RETURN_CODE(0xB2); + + + + Sgp4Propagator(); + virtual ~Sgp4Propagator(); + + ReturnValue_t initialize(const uint8_t *line1, const uint8_t *line2); + + /** + * + * @param[out] position in ECF + * @param[out] velocity in ECF + * @param time to which to propagate + * @return + */ + ReturnValue_t propagate(double *position, double *velocity, timeval time, uint8_t gpsUtcOffset); + +private: + bool initialized; + timeval epoch; + elsetrec satrec; + gravconsttype whichconst; + +}; + +#endif /* SGP4PROPAGATOR_H_ */