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

Final Version of the ACS Controller #367

Merged
muellerr merged 78 commits from eggert/acs into develop 2023-02-08 13:50:11 +01:00
Conversation 0 Commits 78 Files Changed 27 +119 -37
2 changed files with 119 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit fac2fc4971 - Show all commits

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

@ -1,22 +1,19 @@
/*
* Igrf13Model.cpp
*
* Created on: 10 Mar 2022
* Author: Robin Marquardt
*/
#include "Igrf13Model.h" #include "Igrf13Model.h"
#include <fsfw/globalfunctions/constants.h> #include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <time.h>
#include <cmath>
#include "util/MathOperations.h" #include "util/MathOperations.h"
using namespace Math;
Igrf13Model::Igrf13Model() {} Igrf13Model::Igrf13Model() {}
Igrf13Model::~Igrf13Model() {} Igrf13Model::~Igrf13Model() {}
@ -25,7 +22,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
double* magFieldModelInertial) { double* magFieldModelInertial) {
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 * Math::PI / 180; theta -= 90 * PI / 180;
theta *= (-1); theta *= (-1);
double rE = 6371200.0; // radius earth [m] double rE = 6371200.0; // radius earth [m]
@ -43,7 +40,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
/* Calculation of Legendre Polynoms (normalised) */ /* Calculation of Legendre Polynoms (normalised) */
if (n == m) { if (n == m) {
P2 = sin(theta) * P11; P2 = sin(theta) * P11;
dP2 = sin(theta) * dP11 - cos(theta) * P11; dP2 = sin(theta) * dP11 + cos(theta) * P11;
P11 = P2; P11 = P2;
P10 = P11; P10 = P11;
P20 = 0; P20 = 0;
@ -70,11 +67,11 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModel[0] += magFieldModel[0] +=
pow(rE / (altitude + rE), (n + 2)) * (n + 1) * pow(rE / (altitude + rE), (n + 2)) * (n + 1) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2); ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2);
/* gradient of scalar potential towards phi */ /* gradient of scalar potential towards theta */
magFieldModel[1] += magFieldModel[1] +=
pow(rE / (altitude + rE), (n + 2)) * pow(rE / (altitude + rE), (n + 2)) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2); ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2);
/* gradient of scalar potential towards theta */ /* gradient of scalar potential towards phi */
magFieldModel[2] += magFieldModel[2] +=
pow(rE / (altitude + rE), (n + 2)) * pow(rE / (altitude + rE), (n + 2)) *
((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m); ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m);
@ -85,31 +82,51 @@ 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));
/* Next step: transform into inertial KOS (IJK)*/ // std::cout << " X=" << -magFieldModel[1] << " Y=" << magFieldModel[2]
// << " Z=" << -magFieldModel[0] << std::endl;
/* Next step: transform into inertial RF (IJK)*/
// Julean Centuries // Julean Centuries
double JD2000Floor = 0; // double JD2000Floor = 0;
// double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
// JD2000Floor = floor(JD2000);
// double JC2000 = JD2000Floor / 36525.;
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement); double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
JD2000Floor = floor(JD2000); double UT1 = JD2000 / 36525.;
double JC2000 = JD2000Floor / 36525;
double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) - double gst =
0.000000026 * pow(JC2000, 3); // greenwich sidereal time 280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
gst *= PI / 180; // convert to radians gst = std::fmod(gst, 360.);
double sec = gst *= PI / 180.;
(JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ? // std::cout << " GMST=" << gst * 180. / Math::PI << std::endl;
double omega0 = 0.00007292115; // mean angular velocity earth [rad/s]
gst += omega0 * sec;
// double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) -
// 0.000000026 * pow(JC2000, 3); // greenwich sidereal time
// gst *= PI / 180; // convert to radians
// double sec =
// (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ?
// double omega0 = 0.00007292115; // mean angular velocity earth [rad/s]
// gst += omega0 * sec;
// std::cout << " GMST=" << gst * 180. / Math::PI << std::endl;
double lst = gst + longitude; // local sidereal time [rad] double lst = gst + longitude; // local sidereal time [rad]
// std::cout << " LMST=" << lst * 180. / Math::PI << std::endl;
magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + magFieldModelInertial[0] =
magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst); (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) -
magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + magFieldModel[2] * sin(lst);
magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst); magFieldModelInertial[1] =
magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst); (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * sin(lst) +
magFieldModel[2] * cos(lst);
magFieldModelInertial[2] =
magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude);
double normVecMagFieldInert[3] = {0, 0, 0}; double normVecMagFieldInert[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3); VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
magFieldModel[0] = 0;
magFieldModel[1] = 0;
magFieldModel[2] = 0;
} }
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
@ -123,3 +140,50 @@ void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
} }
} }
} }
void Igrf13Model::schmidtNormalization() {
double kronDelta = 0;
schmidtFactors[0][0] = 1;
// for (int n = 1; n <= igrfOrder; n++) {
// if (n == 1) {
// schmidtFactors[0][n - 1] = 1;
// } else {
// schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n;
// }
//
// for (int m = 1; m <= igrfOrder; m++) {
// if (m == 1) {
// kronDelta = 1;
// } else {
// kronDelta = 0;
// }
// schmidtFactors[m][n - 1] =
// schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m));
// }
// }
for (int n = 1; n <= igrfOrder; n++) {
for (int m = 0; m <= n; m++) {
if (m > 1) {
schmidtFactors[m][n - 1] = schmidtFactors[m - 1][n - 1] * pow((n - m + 1) / (n + m), .5);
} else if (m > 0) {
schmidtFactors[m][n - 1] =
schmidtFactors[m - 1][n - 1] * pow(2 * (n - m + 1) / (n + m), .5);
} else if (n == 1) {
schmidtFactors[m][n - 1] = 1;
} else {
schmidtFactors[m][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / (n);
}
}
}
for (int i = 0; i <= igrfOrder; i++) {
for (int j = 0; j <= (igrfOrder - 1); j++) {
coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j];
coeffH[i][j] = schmidtFactors[i][j] * coeffH[i][j];
svG[i][j] = schmidtFactors[i][j] * svG[i][j];
svH[i][j] = schmidtFactors[i][j] * svH[i][j];
}
}
}

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

@ -43,14 +43,15 @@ class Igrf13Model /*:public HasParametersIF*/ {
* - timeOfMagMeasurement: time of actual measurement [s] * - timeOfMagMeasurement: time of actual measurement [s]
* *
* Outputs: * Outputs:
* - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ * - magFieldModelInertial: Magnetic Field Vector in IJK RF [nT]*/
// Coefficient wary over year, could be updated sometimes. // Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3]; double magFieldModel[3];
void schmidtNormalization();
private: private:
const double coeffG[14][13] = { double coeffG[14][13] = {
{-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1}, {-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1},
{-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9}, {-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9},
{0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5}, {0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5},
@ -66,7 +67,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4}}; // [m][n] in nT
const double coeffH[14][13] = { double coeffH[14][13] = {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0},
{4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9}, {4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9},
{0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6}, {0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6},
@ -82,7 +83,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6}}; // [m][n] in nT
const double svG[14][13] = { double svG[14][13] = {
{5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
@ -98,7 +99,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
const double svH[14][13] = { double svH[14][13] = {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, {-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
@ -114,6 +115,23 @@ class Igrf13Model /*:public HasParametersIF*/ {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
double schmidtFactors[14][13] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
;
bool schmidtNorm = false;
double updatedG[14][13]; double updatedG[14][13];
double updatedH[14][13]; double updatedH[14][13];
static const int igrfOrder = 13; // degree of truncation static const int igrfOrder = 13; // degree of truncation