eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

whole lot of cleanup
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2024-02-12 14:43:34 +01:00
parent b68bbe64a3
commit 236ca64de3
11 changed files with 42 additions and 538 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
mission/controller/acs/AcsParameters.cpp
View File

@ -717,22 +717,22 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x11): // KalmanFilterParameters case (0x11): // KalmanFilterParameters
switch (parameterId) { switch (parameterId) {
case 0x0: case 0x0:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); parameterWrapper->set(kalmanFilterParameters.sensorNoiseStr);
break; break;
case 0x1: case 0x1:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); parameterWrapper->set(kalmanFilterParameters.sensorNoiseSus);
break; break;
case 0x2: case 0x2:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); parameterWrapper->set(kalmanFilterParameters.sensorNoiseMgm);
break; break;
case 0x3: case 0x3:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyr);
break; break;
case 0x4: case 0x4:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrArw);
break; break;
case 0x5: case 0x5:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs);
break; break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;

4
mission/controller/acs/AttitudeEstimation.cpp
View File

@ -41,8 +41,8 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
// Sensor Weights // Sensor Weights
double kSus = 0, kMgm = 0; double kSus = 0, kMgm = 0;
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2); kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSus, -2);
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2); kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMgm, -2);
// Weighted Vectors // Weighted Vectors
double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0}, double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},

4
mission/controller/acs/Guidance.cpp
View File

@ -44,7 +44,7 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF) // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
double targetF[3] = {0, 0, 0}; double targetF[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( CoordinateTransformations::cartesianFromLatLongAlt(
acsParameters->targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetF); acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
@ -98,7 +98,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF) // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
double posGroundStationF[3] = {0, 0, 0}; double posGroundStationF[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt( CoordinateTransformations::cartesianFromLatLongAlt(
acsParameters->gsTargetModeControllerParameters.latitudeTgt, acsParameters->gsTargetModeControllerParameters.latitudeTgt,
acsParameters->gsTargetModeControllerParameters.longitudeTgt, acsParameters->gsTargetModeControllerParameters.longitudeTgt,
acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);

1
mission/controller/acs/Guidance.h
View File

@ -8,7 +8,6 @@
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h> #include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h> #include <mission/controller/acs/SensorValues.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h> #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <time.h> #include <time.h>

22
mission/controller/acs/Igrf13Model.cpp
View File

@ -1,19 +1,5 @@
#include "Igrf13Model.h" #include "Igrf13Model.h"
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <stdint.h>
#include <string.h>
#include <time.h>
#include <cmath>
#include "util/MathOperations.h"
using namespace Math;
Igrf13Model::Igrf13Model() {} Igrf13Model::Igrf13Model() {}
Igrf13Model::~Igrf13Model() {} Igrf13Model::~Igrf13Model() {}
@ -23,7 +9,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
double magFieldModel[3] = {0, 0, 0}; double magFieldModel[3] = {0, 0, 0};
double phi = longitude, theta = gcLatitude; // geocentric double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/ /* Here is the co-latitude needed*/
theta -= 90 * PI / 180; theta -= 90. * M_PI / 180.;
theta *= (-1); theta *= (-1);
double rE = 6371200.0; // radius earth [m] double rE = 6371200.0; // radius earth [m]
@ -83,13 +69,13 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModel[1] *= -1; magFieldModel[1] *= -1;
magFieldModel[2] *= (-1 / sin(theta)); magFieldModel[2] *= (-1 / sin(theta));
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement); double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement);
double UT1 = JD2000 / 36525.; double UT1 = JD2000 / 36525.;
double gst = double gst =
280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3); 280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
gst = std::fmod(gst, 360.); gst = std::fmod(gst, 360.);
gst *= PI / 180.; gst *= M_PI / 180.;
double lst = gst + longitude; // local sidereal time [rad] double lst = gst + longitude; // local sidereal time [rad]
magFieldModelInertial[0] = magFieldModelInertial[0] =
@ -107,7 +93,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000 double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement); double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement);
double days = ceil(JD2000 - JD2000Igrf); double days = ceil(JD2000 - JD2000Igrf);
for (int i = 0; i <= igrfOrder; i++) { for (int i = 0; i <= igrfOrder; i++) {
for (int j = 0; j <= (igrfOrder - 1); j++) { for (int j = 0; j <= (igrfOrder - 1); j++) {

9
mission/controller/acs/Igrf13Model.h
View File

@ -16,10 +16,11 @@
#ifndef IGRF13MODEL_H_ #ifndef IGRF13MODEL_H_
#define IGRF13MODEL_H_ #define IGRF13MODEL_H_
#include <fsfw/parameters/HasParametersIF.h> #include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <stdint.h> #include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <string.h> #include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <time.h> #include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/src/fsfw/globalfunctions/timeSystems.h>
#include <cmath> #include <cmath>

36
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -9,9 +9,6 @@
#include <cmath> #include <cmath>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {} MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {}
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
@ -25,9 +22,9 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
if (validMagField_ && validSS && validSSModel && validMagModel) { if (validMagField_ && validSS && validSSModel && validMagModel) {
// QUEST ALGO ----------------------------------------------------------------------- // QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR; sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGyr;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0}; normSunJ[3] = {0, 0, 0};
@ -234,9 +231,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// If we are here, MEKF will perform // If we are here, MEKF will perform
double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR; sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseStr;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0}; normSunJ[3] = {0, 0, 0};
@ -264,8 +261,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss
double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag
double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str
MathOperations<double>::skewMatrix(sunEstB, *measSensMatrix11); MatrixOperations<double>::skewMatrix(sunEstB, *measSensMatrix11);
MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22); MatrixOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
double measVecQuat[3] = {0, 0, 0}; double measVecQuat[3] = {0, 0, 0};
if (validSTR_) { if (validSTR_) {
@ -837,8 +834,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF);
// <<INVERSE residualCov HIER>> // <<INVERSE residualCov HIER>>
double invResidualCov[MDF][MDF] = {{0}}; double invResidualCov[MDF][MDF] = {{0}};
int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF); ReturnValue_t result =
if (inversionFailed) { MatrixOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
if (result != returnvalue::OK) {
updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
} }
@ -874,7 +872,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// State Vector Elements // State Vector Elements
double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::skewMatrix(propagatedQuaternion, *xi2); MatrixOperations<double>::skewMatrix(propagatedQuaternion, *xi2);
double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
MatrixOperations<double>::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3);
MatrixOperations<double>::add(*xi1, *xi2, *xi1, 3, 3); MatrixOperations<double>::add(*xi1, *xi2, *xi1, 3, 3);
@ -898,8 +896,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
biasGYR[2] = updatedGyroBias[2]; biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/ /* ----------- PROPAGATION ----------*/
double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR; double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs;
double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR; double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
@ -1057,7 +1055,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3); VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::skewMatrix(rotSin, *skewSin); MatrixOperations<double>::skewMatrix(rotSin, *skewSin);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3); MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3);
double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -1080,8 +1078,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) || if (not(VectorOperations<double>::isFinite(propagatedQuaternion, 4)) ||
not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) { not(MatrixOperations<double>::isFinite(initialQuaternion, 6, 6))) {
updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE); updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
return MEKF_NOT_FINITE; return MEKF_NOT_FINITE;
} }

3
mission/controller/acs/Navigation.cpp
View File

@ -5,9 +5,6 @@
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h> #include <math.h>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Navigation::Navigation() {} Navigation::Navigation() {}
Navigation::~Navigation() {} Navigation::~Navigation() {}

8
mission/controller/acs/SensorProcessing.cpp
View File

@ -180,7 +180,7 @@ void SensorProcessing::processSus(
const AcsParameters::SunModelParameters *sunModelParameters, const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed) { acsctrl::SusDataProcessed *susDataProcessed) {
/* -------- Sun Model Direction (IJK frame) ------- */ /* -------- Sun Model Direction (IJK frame) ------- */
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute); double JD2000 = TimeSystems::convertUnixToJD2000(timeAbsolute);
// Julean Centuries // Julean Centuries
double sunIjkModel[3] = {0.0, 0.0, 0.0}; double sunIjkModel[3] = {0.0, 0.0, 0.0};
@ -528,8 +528,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
uint8_t gpsSource = acs::gps::Source::NONE; uint8_t gpsSource = acs::gps::Source::NONE;
// We do not trust the GPS and therefore it shall die here if SPG4 is running // We do not trust the GPS and therefore it shall die here if SPG4 is running
if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) { if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude, CoordinateTransformations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value,
gdLongitude, altitude); gdLatitude, gdLongitude, altitude);
double factor = 1 - pow(ECCENTRICITY_WGS84, 2); double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
gcLatitude = atan(factor * tan(gdLatitude)); gcLatitude = atan(factor * tan(gdLatitude));
{ {
@ -559,7 +559,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
// Calculation of the satellite velocity in earth fixed frame // Calculation of the satellite velocity in earth fixed frame
double deltaDistance[3] = {0, 0, 0}; double deltaDistance[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); CoordinateTransformations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) { if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3); VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3); VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);

3
mission/controller/acs/SensorProcessing.h
View File

@ -2,11 +2,13 @@
#define SENSORPROCESSING_H_ #define SENSORPROCESSING_H_
#include <common/config/eive/resultClassIds.h> #include <common/config/eive/resultClassIds.h>
#include <fsfw/coordinates/CoordinateTransformations.h>
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/constants.h> #include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/timeSystems.h>
#include <fsfw/globalfunctions/timevalOperations.h> #include <fsfw/globalfunctions/timevalOperations.h>
#include <fsfw/returnvalues/returnvalue.h> #include <fsfw/returnvalues/returnvalue.h>
#include <mission/acs/defs.h> #include <mission/acs/defs.h>
@ -14,7 +16,6 @@
#include <mission/controller/acs/Igrf13Model.h> #include <mission/controller/acs/Igrf13Model.h>
#include <mission/controller/acs/SensorValues.h> #include <mission/controller/acs/SensorValues.h>
#include <mission/controller/acs/SusConverter.h> #include <mission/controller/acs/SusConverter.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h> #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <cmath> #include <cmath>

478
mission/controller/acs/util/MathOperations.h
View File

@ -1,478 +0,0 @@
#ifndef MATH_MATHOPERATIONS_H_
#define MATH_MATHOPERATIONS_H_
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/sign.h>
#include <cmath>
// Most of these functions already exist within the FSFW and are redundant
// All others should have been merged into the FSFW
// So if you are that bored, that you are reading this, you were better of merging these now
template <typename T1, typename T2 = T1>
class MathOperations {
public:
static void skewMatrix(const T1 vector[], T2 *result) {
// Input Dimension [3], Output [3][3]
result[0] = 0;
result[1] = -vector[2];
result[2] = vector[1];
result[3] = vector[2];
result[4] = 0;
result[5] = -vector[0];
result[6] = -vector[1];
result[7] = vector[0];
result[8] = 0;
}
static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
uint8_t size = 3) {
// Looks like MatrixOpertions::multiply is able to do the same thing
for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
result[resultColumn + size * resultRow] =
vector1[resultRow] * transposeVector2[resultColumn];
}
}
/*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
matrixMag[i][j] = magEstB[i] * magEstB[j];
matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
}
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
int min_idx;
T1 temp;
std::memcpy(result, matrix, rowSize * colSize * sizeof(*result));
// One by one move boundary of unsorted subarray
for (int k = 0; k < rowSize; k++) {
for (int i = 0; i < colSize - 1; i++) {
// Find the minimum element in unsorted array
min_idx = i;
for (int j = i + 1; j < colSize; j++) {
if (result[j + k * colSize] < result[min_idx + k * colSize]) {
min_idx = j;
}
}
// Swap the found minimum element with the first element
temp = result[i + k * colSize];
result[i + k * colSize] = result[min_idx + k * colSize];
result[min_idx + k * colSize] = temp;
}
}
}
static void convertDateToJD2000(const T1 time, T2 julianDate) {
// time = { Y, M, D, h, m,s}
// time in sec and microsec -> The Epoch (unixtime)
julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
floor(275 * time[1] / 9) + time[2] +
(60 * time[3] + time[4] + (time(5) / 60)) / 1440;
}
static T1 convertUnixToJD2000(timeval time) {
// time = {{s},{us}}
T1 julianDate2000;
julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
return julianDate2000;
}
static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
// convention q = [qx,qy,qz, qw]
outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]);
outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]);
outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);
outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
}
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
T2 *cartesianOutput) {
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
* longitude and altitude
* @param: lat geodetic latitude [rad]
* longi longitude [rad]
* alt altitude [m]
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
* Landis Markley and John L. Crassidis*/
double radiusPolar = 6356752.314;
double radiusEqua = 6378137;
double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
}
static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) {
/* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from
* cartesian coordinates in ECEF
* @param: x x-value of position vector [m]
* y y-value of position vector [m]
* z z-value of position vector [m]
* latitude geodetic latitude [rad]
* longitude longitude [rad]
* altitude altitude [m]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
* Landis Markley and John L. Crassidis*/
// From World Geodetic System the Earth Radii
double a = 6378137.0; // semimajor axis [m]
double b = 6356752.3142; // semiminor axis [m]
// Calculation
double e2 = 1 - pow(b, 2) / pow(a, 2);
double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
double p = std::abs(vector[2]) / epsilon2;
double s = pow(rho, 2) / (e2 * epsilon2);
double q = pow(p, 2) - pow(b, 2) + s;
double u = p / sqrt(q);
double v = pow(b, 2) * pow(u, 2) / q;
double P = 27 * v * s / q;
double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
double t = (1 + Q + 1 / Q) / 6;
double c = sqrt(pow(u, 2) - 1 + 2 * t);
double w = (c - u) / 2;
double d =
sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
latitude = asin((epsilon2 + 1) * d / N);
altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
longitude = atan2(vector[1], vector[0]);
}
static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* outputDotDcmEJ Derivative of transformation matrix [3][3]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
* Landis Markley and John L. Crassidis*/
double JD2000Floor = 0;
double JD2000 = convertUnixToJD2000(time);
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
JD2000Floor = floor(JD2000);
if ((JD2000 - JD2000Floor) < 0.5) {
JD2000Floor -= 0.5;
} else {
JD2000Floor += 0.5;
}
double JC2000 = JD2000Floor / 36525;
double sec = (JD2000 - JD2000Floor) * 86400;
double gmst = 0; // greenwich mean sidereal time
gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) -
0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec;
double rest = gmst / 86400;
double FloorRest = floor(rest);
double secOfDay = rest - FloorRest;
secOfDay *= 86400;
gmst = secOfDay / 240 * M_PI / 180;
outputDcmEJ[0] = cos(gmst);
outputDcmEJ[1] = sin(gmst);
outputDcmEJ[2] = 0;
outputDcmEJ[3] = -sin(gmst);
outputDcmEJ[4] = cos(gmst);
outputDcmEJ[5] = 0;
outputDcmEJ[6] = 0;
outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1;
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = 0.000072921158553;
double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
}
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
* give also the back the derivative of this matrix
* @param: unixTime Current time in Unix format
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* outputDotDcmEJ Derivative of transformation matrix [3][3]
* @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
* Oliver Zeile
*
https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10
//(initial Offset) International Atomic Time (TAI)
double JD2000UTC1 = convertUnixToJD2000(unixTime);
// Julian Date / century from TT
timeval terestrialTime = unixTime;
terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
double JD2000TT = convertUnixToJD2000(terestrialTime);
double JC2000TT = JD2000TT / 36525;
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth rotation Theta
//-------------------------------------------------------------------------------------
double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// Earth Rotation angle
double era = 0;
era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
// Greenwich Mean Sidereal Time
double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
double arcsecFactor = 1 * M_PI / (180 * 3600);
gmst2000 *= arcsecFactor;
gmst2000 += era;
theta[0][0] = cos(gmst2000);
theta[0][1] = sin(gmst2000);
theta[0][2] = 0;
theta[1][0] = -sin(gmst2000);
theta[1][1] = cos(gmst2000);
theta[1][2] = 0;
theta[2][0] = 0;
theta[2][1] = 0;
theta[2][2] = 1;
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Precession P
//-------------------------------------------------------------------------------------
double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3);
double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3);
double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3);
zeta *= arcsecFactor;
theta2 *= arcsecFactor;
ze *= arcsecFactor;
precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta);
precession[2][0] = sin(theta2) * cos(zeta);
precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta);
precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta);
precession[2][1] = -sin(theta2) * sin(zeta);
precession[0][2] = -cos(ze) * sin(theta2);
precession[1][2] = -sin(ze) * sin(theta2);
precession[2][2] = cos(theta2);
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Nutation N
//-------------------------------------------------------------------------------------
double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// lunar asc node
double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3);
Om *= arcsecFactor;
// delta psi approx
double dp = -17.2 * arcsecFactor * sin(Om);
// delta eps approx
double de = 9.203 * arcsecFactor * cos(Om);
// % true obliquity of the ecliptic eps p.71 (simplified)
double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180;
nutation[0][0] = cos(dp);
nutation[1][0] = cos(e + de) * sin(dp);
nutation[2][0] = sin(e + de) * sin(dp);
nutation[0][1] = -cos(e) * sin(dp);
nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de);
nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de);
nutation[0][2] = -sin(e) * sin(dp);
nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de);
nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de);
//-------------------------------------------------------------------------------------
// Calculation of Derivative of rotation matrix from earth
//-------------------------------------------------------------------------------------
double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = 0.000072921158553;
MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
//-------------------------------------------------------------------------------------
// Calculation of transformation matrix and Derivative of transformation matrix
//-------------------------------------------------------------------------------------
double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
}
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
int i, j;
double determinant = 0;
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
{matrix[3], matrix[4], matrix[5]},
{matrix[6], matrix[7], matrix[8]}};
for (i = 0; i < 3; i++) {
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
}
// cout<<"\n\ndeterminant: "<<determinant;
// cout<<"\n\nInverse of matrix is: \n";
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
(mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
determinant;
}
}
}
static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
/* do not use this. takes 300ms */
float det = 0;
T1 matrix[size][size], submatrix[size - 1][size - 1];
for (uint8_t row = 0; row < size; row++) {
for (uint8_t col = 0; col < size; col++) {
matrix[row][col] = inputMatrix[row * size + col];
}
}
if (size == 2)
return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
else {
for (uint8_t col = 0; col < size; col++) {
int subRow = 0;
for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
int subCol = 0;
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
if (colIndex == col) continue;
submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
subCol++;
}
subRow++;
}
det += (pow(-1, col) * matrix[0][col] *
MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
}
}
return det;
}
static ReturnValue_t inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
// Stopwatch stopwatch;
T1 matrix[size][size], identity[size][size];
// reformat array to matrix
for (uint8_t row = 0; row < size; row++) {
for (uint8_t col = 0; col < size; col++) {
matrix[row][col] = inputMatrix[row * size + col];
}
}
// init identity matrix
std::memset(identity, 0.0, sizeof(identity));
for (uint8_t diag = 0; diag < size; diag++) {
identity[diag][diag] = 1;
}
// gauss-jordan algo
// sort matrix such as no diag entry shall be 0
for (uint8_t row = 0; row < size; row++) {
if (matrix[row][row] == 0.0) {
bool swaped = false;
uint8_t rowIndex = 0;
while ((rowIndex < size) && !swaped) {
if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
}
swaped = true;
}
rowIndex++;
}
if (!swaped) {
return returnvalue::FAILED; // matrix not invertible
}
}
}
for (int row = 0; row < size; row++) {
if (matrix[row][row] == 0.0) {
uint8_t rowIndex;
if (row == 0) {
rowIndex = size - 1;
} else {
rowIndex = row - 1;
}
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
}
row--;
if (row < 0) {
return returnvalue::FAILED; // Matrix is not invertible
}
}
}
// remove non diag elements in matrix (jordan)
for (int row = 0; row < size; row++) {
for (int rowIndex = 0; rowIndex < size; rowIndex++) {
if (row != rowIndex) {
double ratio = matrix[rowIndex][row] / matrix[row][row];
for (int colIndex = 0; colIndex < size; colIndex++) {
matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
}
}
}
}
// normalize rows in matrix (gauss)
for (int row = 0; row < size; row++) {
for (int col = 0; col < size; col++) {
identity[row][col] = identity[row][col] / matrix[row][row];
}
}
std::memcpy(inverse, identity, sizeof(identity));
return returnvalue::OK; // successful inversion
}
static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
for (uint8_t i = 0; i < size; i++) {
if (not std::isfinite(inputVector[i])) {
return false;
}
}
return true;
}
static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
for (uint8_t col = 0; col < cols; col++) {
for (uint8_t row = 0; row < rows; row++) {
if (not std::isfinite(inputMatrix[row * cols + col])) {
return false;
}
}
}
return true;
}
static void writeSubmatrix(T1 *mainMatrix, T1 *subMatrix, uint8_t subRows, uint8_t subCols,
uint8_t mainRows, uint8_t mainCols, uint8_t startRow,
uint8_t startCol) {
if ((startRow + subRows > mainRows) or (startCol + subCols > mainCols)) {
return;
}
for (uint8_t row = 0; row < subRows; row++) {
for (uint8_t col = 0; col < subCols; col++) {
mainMatrix[(startRow + row) * mainCols + (startCol + col)] = subMatrix[row * subCols + col];
}
}
}
};
#endif /* ACS_MATH_MATHOPERATIONS_H_ */