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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +66 -66
4 changed files with 66 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit edb64d2ec5 - Show all commits

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

@ -1,8 +1,8 @@
#include <fsfw/src/fsfw/globalfunctions/constants.h> #include <fsfw/src/fsfw/globalfunctions/constants.h>
#include "AcsParameters.h" #include "AcsParameters.h"
#include <stddef.h> //#include <stddef.h>
#include <cmath> #include <math.h>
AcsParameters::AcsParameters() { AcsParameters::AcsParameters() {

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

@ -6,17 +6,16 @@
*/ */
#include "Igrf13Model.h" #include "Igrf13Model.h"
#include <cmath> #include <math.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <time.h> //#include <time.h>
#include <fsfw/src/fsfw/globalfunctions/constants.h> #include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h> #include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <acs/util/MathOperations.h> #include <acs/util/MathOperations.h>
using namespace Math;
Igrf13Model::Igrf13Model(){ Igrf13Model::Igrf13Model(){
} }
@ -28,7 +27,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
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*Math::PI/180;
theta *= (-1); theta *= (-1);
double rE = 6371200.0; // radius earth [m] double rE = 6371200.0; // radius earth [m]

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

@ -16,15 +16,16 @@
#ifndef IGRF13MODEL_H_ #ifndef IGRF13MODEL_H_
#define IGRF13MODEL_H_ #define IGRF13MODEL_H_
#include <cmath> #include <fsfw/parameters/HasParametersIF.h>
//#include <cmath>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <time.h> #include <sys/time.h>
// Output should be transformed to [T] instead of [nT] // Output should be transformed to [T] instead of [nT]
// Updating Coefficients has to be implemented yet. Question, updating everyday ? // Updating Coefficients has to be implemented yet. Question, updating everyday ?
class Igrf13Model { class Igrf13Model:public HasParametersIF{
public: public:
Igrf13Model(); Igrf13Model();
@ -49,65 +50,65 @@ public:
double magFieldModel[3]; double magFieldModel[3];
private: private:
const 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}, const 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},
{-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, 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 , 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, 525.7, -309.4, -140.7, -121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3 ,0.7}, { 0 , 0 , 525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7},
{0 ,0 ,0, 48.0, -151.2, -36.2, 15.8, -21.1, -1.1, -0.9, -0.9, -1.2, -0.3}, { 0 , 0 , 0 , 48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3},
{0, 0, 0, 0, 13.5, 13.5, 6.4, 15.3, -13.2, 0.7, 0.3, 0.7, 0.8}, { 0 , 0 , 0 , 0 , 13.5, 13.5, 6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8},
{0, 0, 0, 0, 0,-64.7, -7.2, 13.7, 1.1, -0.9, -0.7, 0.3, 0.0}, { 0 , 0 , 0 , 0 , 0 , -64.7, -7.2, 13.7, 1.1,-0.9,-0.7, 0.3, 0.0},
{0, 0, 0, 0, 0, 0, 9.8, -16.5, 8.8, 1.9, -0.1, 0.5, 0.8}, { 0 , 0 , 0 , 0 , 0 , 0 , 9.8,-16.5, 8.8, 1.9,-0.1, 0.5, 0.8},
{0, 0, 0, 0, 0, 0, 0, -0.3, -9.3, 1.4, 1.4, -0.3, 0.0}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , -0.3, -9.3, 1.4, 1.4,-0.3, 0.0},
{0, 0, 0, 0, 0, 0, 0, 0, -11.9, -2.4, -0.6, -0.5, 0.4}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-11.9,-2.4,-0.6,-0.5, 0.4},
{0, 0, 0, 0, 0, 0, 0, 0, 0, -3.8, 0.2, 0.1, 0.1}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-3.8, 0.2, 0.1, 0.1},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3.1, -1.1, 0.5}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3.1,-1.1, 0.5},
{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.3,-0.5},
{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.4}}; // [m][n] in nT
const double coeffH[14][13] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, const double coeffH[14][13] = {{ 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, -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 , -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, -543.4, 199.7, -121.2, 52.8, 2.2, 12.8, 9.8, 3.6, -0.6, 1.4, 1.4}, { 0 , 0 ,-543.4, 199.7,-121.2, 52.8, 2.2, 12.8, 9.8, 3.6,-0.6, 1.4, 1.4},
{0, 0, 0, -349.7, 32.3, -64.5, 23.5, -11.7, -5.1, 4.8, -0.4, -1.8, -0.4}, { 0 , 0 , 0 ,-349.7, 32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4},
{0, 0, 0, 0, 98.9, 8.9, -2.2, 14.9, -6.3, -8.6, 0.6, 0.1, -1.3}, { 0 , 0 , 0 , 0 , 98.9, 8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3},
{0, 0, 0, 0, 0, 68.1, -27.2, 3.6, 7.8, -0.1, -0.2, 0.8, -0.1}, { 0 , 0 , 0 , 0 , 0 , 68.1,-27.2, 3.6, 7.8,-0.1,-0.2, 0.8,-0.1},
{0, 0, 0, 0, 0, 0, -1.8, -6.9, 0.4, -4.3, -1.7, -0.2, 0.3}, { 0 , 0 , 0 , 0 , 0 , 0 , -1.8, -6.9, 0.4,-4.3,-1.7,-0.2, 0.3},
{0, 0, 0, 0, 0, 0, 0, 2.8, -1.4, -3.4, -1.6, 0.6, -0.1}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 2.8, -1.4,-3.4,-1.6, 0.6,-0.1},
{0, 0, 0, 0, 0, 0, 0, 0, 9.6, -0.1, -3.0, 0.2, 0.5}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 9.6,-0.1,-3.0, 0.2, 0.5},
{0, 0, 0, 0, 0, 0, 0, 0, 0, -8.8, -2.0, -0.9, 0.5}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-8.8,-2.0,-0.9, 0.5},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2.6, 0.0, -0.4}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-2.6, 0.0,-0.4},
{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.5,-0.4},
{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.6}}; // [m][n] in nT
const double svG[14][13] = {{5.7, -11, 2.2, -1.2, -0.3, -0.5, -0.1, 0, 0, 0, 0, 0 ,0}, const double svG[14][13] = {{5.7,-11.0, 2.2,-1.2,-0.3,-0.5,-0.1, 0 , 0, 0, 0, 0 ,0},
{7.4, -7, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 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, -2.1, 3.1, -5.9, -0.6, 0.4, 0, -0.1, 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, -12, 5.2, 0.2, 1.3, 0.7, 0.4, 0, 0, 0, 0, 0}, {0 , 0 ,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0, 0, 0, 0, 0},
{0 ,0 ,0, -5.1, 1.3, -1.4, 0.1, -0.1, 0, 0, 0, 0, 0}, {0 , 0 , 0 ,-5.1, 1.3,-1.4, 0.1,-0.1, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0.9, 0, -0.5, 0.4, 0, 0, 0, 0, 0}, {0 , 0 , 0 , 0 , 0.9, 0.0,-0.5, 0.4, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0.9, -0.8, 0.3, 0, 0, 0, 0, 0}, {0 , 0 , 0 , 0 , 0 , 0.9,-0.8, 0.3, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0.8, -0.1, 0, 0, 0, 0, 0}, {0 , 0 , 0 , 0 , 0 , 0 , 0.8,-0.1, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0.4, 0, 0, 0, 0, 0}, {0 , 0 , 0 , 0 , 0 , 0 , 0 , 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 , 0 , 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0 , 0 , 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}}; // [m][n] in nT
const double svH[14][13] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, const double svH[14][13] = {{ 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.1, 0, 0, 0.6, -0.2, 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, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 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.5, 3.6, -0.6, -1.3, -0.8, -0.2, 0, 0, 0, 0, 0}, { 0 , 0 , 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0, 0, 0, 0, 0},
{0 ,0 ,0, -5, 3, 0.8, -0.2, 0.5, 0, 0, 0, 0, 0}, { 0 , 0 , 0 ,-5.0, 3.0, 0.8,-0.2, 0.5, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0.3, 0, -1.1, -0.3, 0, 0, 0, 0, 0}, { 0 , 0 , 0 , 0 , 0.3, 0.0,-1.1,-0.3, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 1, 0.1, -0.4, 0, 0, 0, 0, 0}, { 0 , 0 , 0 , 0 , 0 , 1.0, 0.1,-0.4, 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.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 , 0 , 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, { 0 , 0 , 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}}; // [m][n] in nT
double updatedG[14][13]; double updatedG[14][13];
double updatedH[14][13]; double updatedH[14][13];

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

@ -8,7 +8,7 @@
#ifndef MATH_MATHOPERATIONS_H_ #ifndef MATH_MATHOPERATIONS_H_
#define MATH_MATHOPERATIONS_H_ #define MATH_MATHOPERATIONS_H_
#include <cmath> #include <math.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <fsfw/src/fsfw/globalfunctions/constants.h> #include <fsfw/src/fsfw/globalfunctions/constants.h>