mohr/fsfw
1
0
Fork 0
forked from fsfw/fsfw
Code Pull Requests Activity

updating code from Flying Laptop

Browse Source 
This is the framework of Flying Laptop OBSW version A.13.0.
This commit is contained in:
Ulrich Mohr
2018-07-12 16:29:32 +02:00
parent 1d22a6c97e
commit 575f70ba03
395 changed files with 12807 additions and 8404 deletions
Download Patch File Download Diff File Expand all files Collapse all files

269
coordinates/CoordinateTransformations.cpp
View File

@ -6,36 +6,31 @@
#include <cmath>
//TODO move time stuff to OSAL
void CoordinateTransformations::positionEcfToEci(const double* ecfPosition,
double* eciPosition) {
ecfToEci(ecfPosition, eciPosition, NULL);
double* eciPosition, timeval *timeUTC) {
ecfToEci(ecfPosition, eciPosition, NULL, timeUTC);
}
void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity,
const double* ecfPosition, double* eciVelocity) {
ecfToEci(ecfVelocity, eciVelocity, ecfPosition);
const double* ecfPosition, double* eciVelocity, timeval *timeUTC) {
ecfToEci(ecfVelocity, eciVelocity, ecfPosition, timeUTC);
}
double CoordinateTransformations::getEarthRotationAngle(timeval time) {
void CoordinateTransformations::positionEciToEcf(const double* eciCoordinates, double* ecfCoordinates,timeval *timeUTC){
eciToEcf(eciCoordinates,ecfCoordinates,NULL,timeUTC);
};
double jD2000UTC = (time.tv_sec - 946728000. + time.tv_usec / 1000000.)
/ 24. / 3600.;
void CoordinateTransformations::velocityEciToEcf(const double* eciVelocity,const double* eciPosition, double* ecfVelocity,timeval* timeUTC){
eciToEcf(eciVelocity,ecfVelocity,eciPosition,timeUTC);
}
//value of unix time at J2000TT
static const double J2000TtUnix = 946727935.816;
double CoordinateTransformations::getEarthRotationAngle(timeval timeUTC) {
//TT does not have leap seconds
//so we need to add the leap seconds since J2000 to our UTC based clock
//Conveniently, GPS gives us access to the leap seconds since 1980
//between 1980 and 2000 13 leap seconds happened
uint8_t leapSecondsSinceJ2000 = utcGpsOffset - 13;
double jD2000UTC;
Clock::convertTimevalToJD2000(timeUTC, &jD2000UTC);
//Julean centuries since J2000 //TODO fails for dates before now?
double TTt2000 = (time.tv_sec + time.tv_usec / 1000000. - J2000TtUnix
+ leapSecondsSinceJ2000) / 24. / 3600. / 36525.;
double TTt2000 = getJuleanCenturiesTT(timeUTC);
double theta = 2 * Math::PI
* (0.779057273264 + 1.00273781191135448 * jD2000UTC);
@ -48,9 +43,9 @@ double CoordinateTransformations::getEarthRotationAngle(timeval time) {
return theta;
}
void CoordinateTransformations::getEarthRotationMatrix(timeval time,
void CoordinateTransformations::getEarthRotationMatrix(timeval timeUTC,
double matrix[][3]) {
double theta = getEarthRotationAngle(time);
double theta = getEarthRotationAngle(timeUTC);
matrix[0][0] = cos(theta);
matrix[0][1] = sin(theta);
@ -65,120 +60,35 @@ void CoordinateTransformations::getEarthRotationMatrix(timeval time,
void CoordinateTransformations::ecfToEci(const double* ecfCoordinates,
double* eciCoordinates,
const double* ecfPositionIfCoordinatesAreVelocity) {
//TODO all calculations only work with a correct time
const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin) {
timeval time;
OSAL::getClock_timeval(&time);
//value of unix time at J2000TT
static const double J2000TtUnix = 946727935.816;
//we need TT which does not have leap seconds
//so we need to add the leap seconds since J2000 to our UTC based clock
//Conveniently, GPS gives us access to the leap seconds since 1980
//between 1980 and 2000 13 leap seconds happened
uint8_t leapSecondsSinceJ2000 = utcGpsOffset - 13;
//Julean centuries since J2000 //TODO fails for dates before now?
double TTt2000 = (time.tv_sec + time.tv_usec / 1000000. - J2000TtUnix
+ leapSecondsSinceJ2000) / 24. / 3600. / 36525.;
//////////////////////////////////////////////////////////
// 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];
getEarthRotationMatrix(time, mTheta);
//polar motion is neglected
timeval timeUTC;
if (timeUTCin != NULL) {
timeUTC = *timeUTCin;
} else {
Clock::getClock_timeval(&timeUTC);
}
double Tfi[3][3];
double Ttemp[3][3];
double Tif[3][3];
MatrixOperations<double>::multiply(mNutation[0], mPrecession[0], Ttemp[0],
3, 3, 3);
MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 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 } };
double Ttemp2[3][3];
MatrixOperations<double>::multiply(mNutation[0], mPrecession[0],
Ttemp[0], 3, 3, 3);
MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Ttemp2[0], 3, 3,
3);
MatrixOperations<double>::multiply(Trot[0], Ttemp2[0], Tdotfi[0], 3, 3,
MatrixOperations<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3,
3);
MatrixOperations<double>::transpose(Tdotfi[0], Tdotif[0], 3);
@ -194,13 +104,124 @@ void CoordinateTransformations::ecfToEci(const double* ecfCoordinates,
}
}
CoordinateTransformations::CoordinateTransformations(uint8_t offset) :
utcGpsOffset(offset) {
double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) {
timeval timeTT;
Clock::convertUTCToTT(timeUTC, &timeTT);
double jD2000TT;
Clock::convertTimevalToJD2000(timeTT, &jD2000TT);
return jD2000TT / 36525.;
}
CoordinateTransformations::~CoordinateTransformations() {
}
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);
}
void CoordinateTransformations::setUtcGpsOffset(uint8_t offset) {
}
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);
};

32
coordinates/CoordinateTransformations.h
View File

@ -1,28 +1,32 @@
#ifndef COORDINATETRANSFORMATIONS_H_
#define COORDINATETRANSFORMATIONS_H_
#include <framework/osal/OSAL.h>
#include <framework/timemanager/Clock.h>
class CoordinateTransformations {
public:
CoordinateTransformations(uint8_t utcGpsOffset);
static void positionEcfToEci(const double* ecfCoordinates, double* eciCoordinates, timeval *timeUTC = NULL);
virtual ~CoordinateTransformations();
void positionEcfToEci(const double* ecfCoordinates, double* eciCoordinates);
void velocityEcfToEci(const double* ecfVelocity,
static void velocityEcfToEci(const double* ecfVelocity,
const double* ecfPosition,
double* eciVelocity);
double* eciVelocity, timeval *timeUTC = NULL);
double getEarthRotationAngle(timeval time);
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);
void getEarthRotationMatrix(timeval time, double matrix[][3]);
void setUtcGpsOffset(uint8_t offset);
static double getEarthRotationAngle(timeval timeUTC);
static void getEarthRotationMatrix(timeval timeUTC, double matrix[][3]);
private:
uint8_t utcGpsOffset;
void ecfToEci(const double* ecfCoordinates, double* eciCoordinates,
const double* ecfPositionIfCoordinatesAreVelocity);
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]);
};

180
coordinates/Jgm3Model.h Normal file
View File

@ -0,0 +1,180 @@
#ifndef FRAMEWORK_COORDINATES_JGM3MODEL_H_
#define FRAMEWORK_COORDINATES_JGM3MODEL_H_
#include <stdint.h>
#include <framework/coordinates/CoordinateTransformations.h>
#include <framework/globalfunctions/math/VectorOperations.h>
#include <framework/globalfunctions/timevalOperations.h>
#include <framework/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_ */

28
coordinates/Sgp4Propagator.cpp
View File

@ -1,14 +1,12 @@
#include <framework/osal/OSAL.h>
#include <framework/coordinates/CoordinateTransformations.h>
#include <framework/coordinates/Sgp4Propagator.h>
#include <framework/globalfunctions/constants.h>
#include <framework/globalfunctions/math/MatrixOperations.h>
#include <framework/globalfunctions/math/VectorOperations.h>
#include <framework/globalfunctions/timevalOperations.h>
#include <framework/osal/OSAL.h>
#include <cstring>
Sgp4Propagator::Sgp4Propagator() :
whichconst(wgs84) {
initialized(false), epoch({0, 0}), whichconst(wgs84) {
}
@ -16,7 +14,6 @@ Sgp4Propagator::~Sgp4Propagator() {
}
//TODO move to OSAL
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)
@ -25,7 +22,6 @@ void jday(int year, int mon, int day, int hr, int minute, double sec,
// - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5;
}
//TODO move to OSAL
void days2mdhms(int year, double days, int& mon, int& day, int& hr, int& minute,
double& sec) {
int i, inttemp, dayofyr;
@ -169,14 +165,14 @@ ReturnValue_t Sgp4Propagator::initialize(const uint8_t* line1,
// ---------------- initialize the orbit at sgp4epoch -------------------
uint8_t result = sgp4init(whichconst, satrec.satnum,
satrec.jdsatepoch
- 2433282.5 /*TODO verify, source says it's 2433281.5*/,
satrec.bstar, satrec.ecco, satrec.argpo, satrec.inclo, satrec.mo,
satrec.no, satrec.nodeo, satrec);
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;
}
@ -185,9 +181,14 @@ ReturnValue_t Sgp4Propagator::initialize(const uint8_t* line1,
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 minutesSinceEpoch = timeSinceEpoch.tv_sec / 60.
+ timeSinceEpoch.tv_usec / 60000000.;
double yearsSinceEpoch = minutesSinceEpoch / 60 / 24 / 365;
@ -206,8 +207,7 @@ ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity,
//Transform to ECF
double earthRotationMatrix[3][3];
CoordinateTransformations transform(gpsUtcOffset);
transform.getEarthRotationMatrix(time,
CoordinateTransformations::getEarthRotationMatrix(time,
earthRotationMatrix);
MatrixOperations<double>::multiply(earthRotationMatrix[0], positionTEME,
@ -221,7 +221,7 @@ ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity,
VectorOperations<double>::subtract(velocity, velocityCorrection, velocity);
if (result != 0) {
return MAKE_RETURN_CODE(result);
return MAKE_RETURN_CODE(result || 0xB0);
} else {
return HasReturnvaluesIF::RETURN_OK;
}

18
coordinates/Sgp4Propagator.h
View File

@ -7,14 +7,15 @@
class Sgp4Propagator {
public:
static const uint8_t INTERFACE_ID = SGP4PROPAGATOR_CLASS;
static const ReturnValue_t INVALID_ECCENTRICITY = MAKE_RETURN_CODE(0x01);
static const ReturnValue_t INVALID_MEAN_MOTION = MAKE_RETURN_CODE(0x02);
static const ReturnValue_t INVALID_PERTURBATION_ELEMENTS = MAKE_RETURN_CODE(0x03);
static const ReturnValue_t INVALID_SEMI_LATUS_RECTUM = MAKE_RETURN_CODE(0x04);
static const ReturnValue_t INVALID_EPOCH_ELEMENTS = MAKE_RETURN_CODE(0x05);
static const ReturnValue_t SATELLITE_HAS_DECAYED = MAKE_RETURN_CODE(0x06);
static const ReturnValue_t TLE_TOO_OLD = MAKE_RETURN_CODE(0xA1);
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);
@ -33,6 +34,7 @@ public:
ReturnValue_t propagate(double *position, double *velocity, timeval time, uint8_t gpsUtcOffset);
private:
bool initialized;
timeval epoch;
elsetrec satrec;
gravconsttype whichconst;