KSat/fsfw
19
6
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests Releases Wiki Activity

Today's the day. Renamed platform to framework.

Browse Source
This commit is contained in:
Bastian Baetz
2016-06-15 23:48:41 +02:00
committed by Ulrich Mohr
parent 40987d0b27
commit 1d22a6c97e
356 changed files with 33946 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

206
coordinates/CoordinateTransformations.cpp Normal file
View File

@ -0,0 +1,206 @@
#include <framework/coordinates/CoordinateTransformations.h>
#include <framework/globalfunctions/constants.h>
#include <framework/globalfunctions/math/MatrixOperations.h>
#include <framework/globalfunctions/math/VectorOperations.h>
#include <stddef.h>
#include <cmath>
//TODO move time stuff to OSAL
void CoordinateTransformations::positionEcfToEci(const double* ecfPosition,
double* eciPosition) {
ecfToEci(ecfPosition, eciPosition, NULL);
}
void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity,
const double* ecfPosition, double* eciVelocity) {
ecfToEci(ecfVelocity, eciVelocity, ecfPosition);
}
double CoordinateTransformations::getEarthRotationAngle(timeval time) {
double jD2000UTC = (time.tv_sec - 946728000. + time.tv_usec / 1000000.)
/ 24. / 3600.;
//value of unix time at J2000TT
static const double J2000TtUnix = 946727935.816;
//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;
//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 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 time,
double matrix[][3]) {
double theta = getEarthRotationAngle(time);
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) {
//TODO all calculations only work with a correct time
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
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);
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,
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);
}
}
CoordinateTransformations::CoordinateTransformations(uint8_t offset) :
utcGpsOffset(offset) {
}
CoordinateTransformations::~CoordinateTransformations() {
}
void CoordinateTransformations::setUtcGpsOffset(uint8_t offset) {
}

29
coordinates/CoordinateTransformations.h Normal file
View File

@ -0,0 +1,29 @@
#ifndef COORDINATETRANSFORMATIONS_H_
#define COORDINATETRANSFORMATIONS_H_
#include <framework/osal/OSAL.h>
class CoordinateTransformations {
public:
CoordinateTransformations(uint8_t utcGpsOffset);
virtual ~CoordinateTransformations();
void positionEcfToEci(const double* ecfCoordinates, double* eciCoordinates);
void velocityEcfToEci(const double* ecfVelocity,
const double* ecfPosition,
double* eciVelocity);
double getEarthRotationAngle(timeval time);
void getEarthRotationMatrix(timeval time, double matrix[][3]);
void setUtcGpsOffset(uint8_t offset);
private:
uint8_t utcGpsOffset;
void ecfToEci(const double* ecfCoordinates, double* eciCoordinates,
const double* ecfPositionIfCoordinatesAreVelocity);
};
#endif /* COORDINATETRANSFORMATIONS_H_ */

228
coordinates/Sgp4Propagator.cpp Normal file
View File

@ -0,0 +1,228 @@
#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>
Sgp4Propagator::Sgp4Propagator() :
whichconst(wgs84) {
}
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)
+ 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;
}
//TODO move to OSAL
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
- 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);
if (result != 00) {
return MAKE_RETURN_CODE(result);
} else {
return HasReturnvaluesIF::RETURN_OK;
}
}
ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity,
timeval time, uint8_t gpsUtcOffset) {
//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 transform(gpsUtcOffset);
transform.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);
} else {
return HasReturnvaluesIF::RETURN_OK;
}
}

42
coordinates/Sgp4Propagator.h Normal file
View File

@ -0,0 +1,42 @@
#ifndef SGP4PROPAGATOR_H_
#define SGP4PROPAGATOR_H_
#include <sys/time.h>
#include <contrib/sgp4/sgp4unit.h>
#include <framework/returnvalues/HasReturnvaluesIF.h>
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);
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:
timeval epoch;
elsetrec satrec;
gravconsttype whichconst;
};
#endif /* SGP4PROPAGATOR_H_ */