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

Prevent STR Blinding #859

Merged
meggert merged 35 commits from prevent-str-blinding into main 2024-02-27 13:48:20 +01:00
Conversation 0 Commits 35 Files Changed 15 +35 -119
3 changed files with 35 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit f56d1c2b12 - Show all commits

23
mission/controller/acs/AcsParameters.h
View File

@ -8,6 +8,9 @@
typedef unsigned char uint8_t; typedef unsigned char uint8_t;
class AcsParameters : public HasParametersIF { class AcsParameters : public HasParametersIF {
private:
static constexpr double DEG2RAD = M_PI / 180.;
public: public:
AcsParameters(); AcsParameters();
virtual ~AcsParameters(); virtual ~AcsParameters();
@ -884,7 +887,7 @@ class AcsParameters : public HasParametersIF {
uint8_t avoidBlindStr = true; uint8_t avoidBlindStr = true;
double blindAvoidStart = 1.5; double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5; double blindAvoidStop = 2.5;
double blindRotRate = 1 * DEG2RAD; double blindRotRate = 1. * DEG2RAD;
} targetModeControllerParameters; } targetModeControllerParameters;
struct GsTargetModeControllerParameters : PointingLawParameters { struct GsTargetModeControllerParameters : PointingLawParameters {
@ -911,7 +914,7 @@ class AcsParameters : public HasParametersIF {
} inertialModeControllerParameters; } inertialModeControllerParameters;
struct StrParameters { struct StrParameters {
double exclusionAngle = 20 * DEG2RAD; double exclusionAngle = 20. * DEG2RAD;
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame
} strParameters; } strParameters;
@ -937,13 +940,13 @@ class AcsParameters : public HasParametersIF {
} sunModelParameters; } sunModelParameters;
struct KalmanFilterParameters { struct KalmanFilterParameters {
double sensorNoiseSTR = 0.1 * DEG2RAD; double sensorNoiseStr = 0.1 * DEG2RAD;
double sensorNoiseSS = 8 * DEG2RAD; double sensorNoiseSus = 8. * DEG2RAD;
double sensorNoiseMAG = 4 * DEG2RAD; double sensorNoiseMgm = 4. * DEG2RAD;
double sensorNoiseGYR = 0.1 * DEG2RAD; double sensorNoiseGyr = 0.1 * DEG2RAD;
double sensorNoiseArwGYR = 3 * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk
double sensorNoiseBsGYR = 3 * DEG2RAD / 3600; // Bias Stability double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability
} kalmanFilterParameters; } kalmanFilterParameters;
struct MagnetorquerParameter { struct MagnetorquerParameter {
@ -965,8 +968,6 @@ class AcsParameters : public HasParametersIF {
double gainFull = pow(10.0, -2.3); double gainFull = pow(10.0, -2.3);
uint8_t useFullDetumbleLaw = false; uint8_t useFullDetumbleLaw = false;
} detumbleParameter; } detumbleParameter;
private:
static constexpr double DEG2RAD = M_PI / 180.;
}; };
#endif /* ACSPARAMETERS_H_ */ #endif /* ACSPARAMETERS_H_ */

98
mission/controller/acs/util/CholeskyDecomposition.h
View File

@ -1,98 +0,0 @@
/*
* TinyEKF: Extended Kalman Filter for embedded processors
*
* Copyright (C) 2015 Simon D. Levy
*
* MIT License
*/
#ifndef CHOLESKYDECOMPOSITION_H_
#define CHOLESKYDECOMPOSITION_H_
#include <math.h>
// typedef unsigned int uint8_t;
template <typename T1, typename T2 = T1, typename T3 = T2>
class CholeskyDecomposition {
public:
static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
return cholsl(matrix, result, tempMatrix, dimension);
}
private:
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldc1(double *a, double *p, uint8_t n) {
int8_t i, j, k;
double sum;
for (i = 0; i < n; i++) {
for (j = i; j < n; j++) {
sum = a[i * n + j];
for (k = i - 1; k >= 0; k--) {
sum -= a[i * n + k] * a[j * n + k];
}
if (i == j) {
if (sum <= 0) {
return 1; /* error */
}
p[i] = sqrt(sum);
} else {
a[j * n + i] = sum / p[i];
}
}
}
return 0; /* success */
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
uint8_t i, j, k;
double sum;
for (i = 0; i < n; i++)
for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
if (choldc1(a, p, n)) return 1;
for (i = 0; i < n; i++) {
a[i * n + i] = 1 / p[i];
for (j = i + 1; j < n; j++) {
sum = 0;
for (k = i; k < j; k++) {
sum -= a[j * n + k] * a[k * n + i];
}
a[j * n + i] = sum / p[j];
}
}
return 0; /* success */
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
uint8_t i, j, k;
if (choldcsl(A, a, p, n)) return 1;
for (i = 0; i < n; i++) {
for (j = i + 1; j < n; j++) {
a[i * n + j] = 0.0;
}
}
for (i = 0; i < n; i++) {
a[i * n + i] *= a[i * n + i];
for (k = i + 1; k < n; k++) {
a[i * n + i] += a[k * n + i] * a[k * n + i];
}
for (j = i + 1; j < n; j++) {
for (k = j; k < n; k++) {
a[i * n + j] += a[k * n + i] * a[k * n + j];
}
}
}
for (i = 0; i < n; i++) {
for (j = 0; j < i; j++) {
a[i * n + j] = a[j * n + i];
}
}
return 0; /* success */
}
};
#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */

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

@ -1,14 +1,14 @@
#ifndef MATH_MATHOPERATIONS_H_ #ifndef MATH_MATHOPERATIONS_H_
#define MATH_MATHOPERATIONS_H_ #define MATH_MATHOPERATIONS_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/sign.h> #include <fsfw/src/fsfw/globalfunctions/sign.h>
#include <fsfw/src/fsfw/serviceinterface.h>
#include <cmath> #include <cmath>
#include <cstring>
#include <iostream> // 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> template <typename T1, typename T2 = T1>
class MathOperations { class MathOperations {
@ -365,7 +365,7 @@ class MathOperations {
return det; return det;
} }
static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { static ReturnValue_t inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
// Stopwatch stopwatch; // Stopwatch stopwatch;
T1 matrix[size][size], identity[size][size]; T1 matrix[size][size], identity[size][size];
// reformat array to matrix // reformat array to matrix
@ -396,7 +396,7 @@ class MathOperations {
rowIndex++; rowIndex++;
} }
if (!swaped) { if (!swaped) {
return 1; // matrix not invertible return returnvalue::FAILED; // matrix not invertible
} }
} }
} }
@ -415,7 +415,7 @@ class MathOperations {
} }
row--; row--;
if (row < 0) { if (row < 0) {
return 1; // Matrix is not invertible return returnvalue::FAILED; // Matrix is not invertible
} }
} }
} }
@ -438,12 +438,12 @@ class MathOperations {
} }
} }
std::memcpy(inverse, identity, sizeof(identity)); std::memcpy(inverse, identity, sizeof(identity));
return 0; // successful inversion return returnvalue::OK; // successful inversion
} }
static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) { static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
for (uint8_t i = 0; i < size; i++) { for (uint8_t i = 0; i < size; i++) {
if (not isfinite(inputVector[i])) { if (not std::isfinite(inputVector[i])) {
return false; return false;
} }
} }
@ -453,13 +453,26 @@ class MathOperations {
static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) { static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
for (uint8_t col = 0; col < cols; col++) { for (uint8_t col = 0; col < cols; col++) {
for (uint8_t row = 0; row < rows; row++) { for (uint8_t row = 0; row < rows; row++) {
if (not isfinite(inputMatrix[row * cols + cols])) { if (not std::isfinite(inputMatrix[row * cols + col])) {
return false; return false;
} }
} }
} }
return true; 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_ */ #endif /* ACS_MATH_MATHOPERATIONS_H_ */