taken over coordinates from upstream
This commit is contained in:
parent
2e23fc1201
commit
5ce954672b
|
|
@ -1,227 +1,227 @@
|
|||
#include "../coordinates/CoordinateTransformations.h"
|
||||
#include "../globalfunctions/constants.h"
|
||||
#include "../globalfunctions/math/MatrixOperations.h"
|
||||
#include "../globalfunctions/math/VectorOperations.h"
|
||||
#include <stddef.h>
|
||||
#include <cmath>
|
||||
|
||||
|
||||
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<double>::transpose(Tfi[0], Tif[0], 3);
|
||||
|
||||
MatrixOperations<double>::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<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3,
|
||||
3);
|
||||
|
||||
MatrixOperations<double>::transpose(Tdotfi[0], Tdotif[0], 3);
|
||||
|
||||
double velocityCorrection[3];
|
||||
|
||||
MatrixOperations<double>::multiply(Tdotif[0],
|
||||
ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3,
|
||||
1);
|
||||
|
||||
VectorOperations<double>::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<double>::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<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3,
|
||||
3);
|
||||
|
||||
double velocityCorrection[3];
|
||||
|
||||
MatrixOperations<double>::multiply(Tdotfi[0],
|
||||
eciPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3,
|
||||
1);
|
||||
|
||||
VectorOperations<double>::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<double>::multiply(mNutation[0], mPrecession[0], Ttemp[0],
|
||||
3, 3, 3);
|
||||
|
||||
MatrixOperations<double>::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 <stddef.h>
|
||||
#include <cmath>
|
||||
|
||||
|
||||
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<double>::transpose(Tfi[0], Tif[0], 3);
|
||||
|
||||
MatrixOperations<double>::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<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3,
|
||||
3);
|
||||
|
||||
MatrixOperations<double>::transpose(Tdotfi[0], Tdotif[0], 3);
|
||||
|
||||
double velocityCorrection[3];
|
||||
|
||||
MatrixOperations<double>::multiply(Tdotif[0],
|
||||
ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3,
|
||||
1);
|
||||
|
||||
VectorOperations<double>::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<double>::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<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3,
|
||||
3);
|
||||
|
||||
double velocityCorrection[3];
|
||||
|
||||
MatrixOperations<double>::multiply(Tdotfi[0],
|
||||
eciPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3,
|
||||
1);
|
||||
|
||||
VectorOperations<double>::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<double>::multiply(mNutation[0], mPrecession[0], Ttemp[0],
|
||||
3, 3, 3);
|
||||
|
||||
MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,34 +1,34 @@
|
|||
#ifndef COORDINATETRANSFORMATIONS_H_
|
||||
#define COORDINATETRANSFORMATIONS_H_
|
||||
|
||||
#include "../timemanager/Clock.h"
|
||||
#include <cstring>
|
||||
|
||||
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 <cstring>
|
||||
|
||||
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_ */
|
||||
|
|
|
|||
|
|
@ -1,180 +1,180 @@
|
|||
#ifndef FRAMEWORK_COORDINATES_JGM3MODEL_H_
|
||||
#define FRAMEWORK_COORDINATES_JGM3MODEL_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include "../coordinates/CoordinateTransformations.h"
|
||||
#include "../globalfunctions/math/VectorOperations.h"
|
||||
#include "../globalfunctions/timevalOperations.h"
|
||||
#include "../globalfunctions/constants.h"
|
||||
#include <memory.h>
|
||||
|
||||
|
||||
template<uint8_t DEGREE,uint8_t ORDER>
|
||||
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<double>::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<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[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<DEGREE;n++)
|
||||
}//End of uint8_t m=0;m<ORDER;m++
|
||||
}
|
||||
|
||||
void initializeNavOrbit(const double position[3],const double velocity[3], timeval timeUTC){
|
||||
CoordinateTransformations::positionEcfToEci(position,&y0[0],&timeUTC);
|
||||
CoordinateTransformations::velocityEcfToEci(velocity,position,&y0[3],&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]){
|
||||
|
||||
//RK4 Integration for this timestamp
|
||||
double deltaT = timevalOperations::toDouble(timeUTC-lastExecutionTime);
|
||||
|
||||
double y0dot[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 yB[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 yCdot[6] = {0,0,0,0,0,0};
|
||||
|
||||
//Step One
|
||||
rungeKuttaStep(y0,y0dot,lastExecutionTime,S,C);
|
||||
|
||||
//Step Two
|
||||
VectorOperations<double>::mulScalar(y0dot,deltaT/2,yA,6);
|
||||
VectorOperations<double>::add(y0,yA,yA,6);
|
||||
rungeKuttaStep(yA,yAdot,lastExecutionTime,S,C);
|
||||
|
||||
//Step Three
|
||||
VectorOperations<double>::mulScalar(yAdot,deltaT/2,yB,6);
|
||||
VectorOperations<double>::add(y0,yB,yB,6);
|
||||
rungeKuttaStep(yB,yBdot,lastExecutionTime,S,C);
|
||||
|
||||
//Step Four
|
||||
VectorOperations<double>::mulScalar(yBdot,deltaT,yC,6);
|
||||
VectorOperations<double>::add(y0,yC,yC,6);
|
||||
rungeKuttaStep(yC,yCdot,lastExecutionTime,S,C);
|
||||
|
||||
//Calc new State
|
||||
VectorOperations<double>::mulScalar(yAdot,2,yAdot,6);
|
||||
VectorOperations<double>::mulScalar(yBdot,2,yBdot,6);
|
||||
VectorOperations<double>::add(y0dot,yAdot,y0dot,6);
|
||||
VectorOperations<double>::add(y0dot,yBdot,y0dot,6);
|
||||
VectorOperations<double>::add(y0dot,yCdot,y0dot,6);
|
||||
VectorOperations<double>::mulScalar(y0dot,1./6.*deltaT,y0dot,6);
|
||||
VectorOperations<double>::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 <stdint.h>
|
||||
#include "CoordinateTransformations.h"
|
||||
#include "../globalfunctions/math/VectorOperations.h"
|
||||
#include "../globalfunctions/timevalOperations.h"
|
||||
#include "../globalfunctions/constants.h"
|
||||
#include <memory.h>
|
||||
|
||||
|
||||
template<uint8_t DEGREE,uint8_t ORDER>
|
||||
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<double>::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<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[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<DEGREE;n++)
|
||||
}//End of uint8_t m=0;m<ORDER;m++
|
||||
}
|
||||
|
||||
void initializeNavOrbit(const double position[3],const double velocity[3], timeval timeUTC){
|
||||
CoordinateTransformations::positionEcfToEci(position,&y0[0],&timeUTC);
|
||||
CoordinateTransformations::velocityEcfToEci(velocity,position,&y0[3],&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]){
|
||||
|
||||
//RK4 Integration for this timestamp
|
||||
double deltaT = timevalOperations::toDouble(timeUTC-lastExecutionTime);
|
||||
|
||||
double y0dot[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 yB[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 yCdot[6] = {0,0,0,0,0,0};
|
||||
|
||||
//Step One
|
||||
rungeKuttaStep(y0,y0dot,lastExecutionTime,S,C);
|
||||
|
||||
//Step Two
|
||||
VectorOperations<double>::mulScalar(y0dot,deltaT/2,yA,6);
|
||||
VectorOperations<double>::add(y0,yA,yA,6);
|
||||
rungeKuttaStep(yA,yAdot,lastExecutionTime,S,C);
|
||||
|
||||
//Step Three
|
||||
VectorOperations<double>::mulScalar(yAdot,deltaT/2,yB,6);
|
||||
VectorOperations<double>::add(y0,yB,yB,6);
|
||||
rungeKuttaStep(yB,yBdot,lastExecutionTime,S,C);
|
||||
|
||||
//Step Four
|
||||
VectorOperations<double>::mulScalar(yBdot,deltaT,yC,6);
|
||||
VectorOperations<double>::add(y0,yC,yC,6);
|
||||
rungeKuttaStep(yC,yCdot,lastExecutionTime,S,C);
|
||||
|
||||
//Calc new State
|
||||
VectorOperations<double>::mulScalar(yAdot,2,yAdot,6);
|
||||
VectorOperations<double>::mulScalar(yBdot,2,yBdot,6);
|
||||
VectorOperations<double>::add(y0dot,yAdot,y0dot,6);
|
||||
VectorOperations<double>::add(y0dot,yBdot,y0dot,6);
|
||||
VectorOperations<double>::add(y0dot,yCdot,y0dot,6);
|
||||
VectorOperations<double>::mulScalar(y0dot,1./6.*deltaT,y0dot,6);
|
||||
VectorOperations<double>::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_ */
|
||||
|
|
|
|||
|
|
@ -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 <cstring>
|
||||
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<double>::mulScalar(positionTEME, 1000, positionTEME, 3);
|
||||
VectorOperations<double>::mulScalar(velocityTEME, 1000, velocityTEME, 3);
|
||||
|
||||
//Transform to ECF
|
||||
double earthRotationMatrix[3][3];
|
||||
CoordinateTransformations::getEarthRotationMatrix(time,
|
||||
earthRotationMatrix);
|
||||
|
||||
MatrixOperations<double>::multiply(earthRotationMatrix[0], positionTEME,
|
||||
position, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(earthRotationMatrix[0], velocityTEME,
|
||||
velocity, 3, 3, 1);
|
||||
|
||||
double omegaEarth[3] = { 0, 0, Earth::OMEGA };
|
||||
double velocityCorrection[3];
|
||||
VectorOperations<double>::cross(omegaEarth, position, velocityCorrection);
|
||||
VectorOperations<double>::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 <cstring>
|
||||
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<double>::mulScalar(positionTEME, 1000, positionTEME, 3);
|
||||
VectorOperations<double>::mulScalar(velocityTEME, 1000, velocityTEME, 3);
|
||||
|
||||
//Transform to ECF
|
||||
double earthRotationMatrix[3][3];
|
||||
CoordinateTransformations::getEarthRotationMatrix(time,
|
||||
earthRotationMatrix);
|
||||
|
||||
MatrixOperations<double>::multiply(earthRotationMatrix[0], positionTEME,
|
||||
position, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(earthRotationMatrix[0], velocityTEME,
|
||||
velocity, 3, 3, 1);
|
||||
|
||||
double omegaEarth[3] = { 0, 0, Earth::OMEGA };
|
||||
double velocityCorrection[3];
|
||||
VectorOperations<double>::cross(omegaEarth, position, velocityCorrection);
|
||||
VectorOperations<double>::subtract(velocity, velocityCorrection, velocity);
|
||||
|
||||
if (result != 0) {
|
||||
return MAKE_RETURN_CODE(result || 0xB0);
|
||||
} else {
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,44 +1,44 @@
|
|||
#ifndef SGP4PROPAGATOR_H_
|
||||
#define SGP4PROPAGATOR_H_
|
||||
|
||||
#include <sys/time.h>
|
||||
#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 <sys/time.h>
|
||||
#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_ */
|
||||
|
|
|
|||
Loading…
Reference in New Issue