From f4ed9810030012e1faac7925e9248c9393d05425 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 16 Jan 2024 15:49:58 +0100 Subject: [PATCH 1/4] cleanup --- mission/controller/acs/util/MathOperations.h | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index dd4c3f57..c240b9ed 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -1,17 +1,10 @@ #ifndef MATH_MATHOPERATIONS_H_ #define MATH_MATHOPERATIONS_H_ -#include #include #include -#include -#include -#include #include -#include - -#include "fsfw/serviceinterface.h" template class MathOperations { @@ -46,7 +39,7 @@ class MathOperations { static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) { int min_idx; T1 temp; - memcpy(result, matrix, rowSize * colSize * sizeof(*result)); + 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++) { From 13f37393862e15b9a0a795f2428723cdbf669f47 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 14:13:00 +0100 Subject: [PATCH 2/4] some cleanup --- mission/controller/acs/AcsParameters.h | 53 ++++++++++++++------------ 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f11a673b..4b5eef5b 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -8,6 +8,9 @@ typedef unsigned char uint8_t; class AcsParameters : public HasParametersIF { + private: + static constexpr double DEG2RAD = M_PI / 180.; + public: AcsParameters(); virtual ~AcsParameters(); @@ -854,7 +857,7 @@ class AcsParameters : public HasParametersIF { struct PointingLawParameters { double zeta = 0.3; double om = 0.3; - double omMax = 1 * M_PI / 180; + double omMax = 1 * DEG2RAD; double qiMin = 0.1; double gainNullspace = 0.01; @@ -877,15 +880,15 @@ class AcsParameters : public HasParametersIF { uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS - double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeTgt = 500; // [m] + double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude + double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude + double altitudeTgt = 500; // [m] // For one-axis control: uint8_t avoidBlindStr = true; double blindAvoidStart = 1.5; double blindAvoidStop = 2.5; - double blindRotRate = 1 * M_PI / 180; + double blindRotRate = 1. * DEG2RAD; } targetModeControllerParameters; struct GsTargetModeControllerParameters : PointingLawParameters { @@ -893,9 +896,9 @@ class AcsParameters : public HasParametersIF { uint8_t timeElapsedMax = 10; // rot rate calculations // Default is Stuttgart GS - double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude - double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude - double altitudeTgt = 500; // [m] + double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude + double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude + double altitudeTgt = 500; // [m] } gsTargetModeControllerParameters; struct NadirModeControllerParameters : PointingLawParameters { @@ -912,7 +915,7 @@ class AcsParameters : public HasParametersIF { } inertialModeControllerParameters; struct StrParameters { - double exclusionAngle = 20 * M_PI / 180; + double exclusionAngle = 20. * DEG2RAD; double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame } strParameters; @@ -926,25 +929,25 @@ class AcsParameters : public HasParametersIF { struct SunModelParameters { float domega = 36000.771; - float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of - // perigee - float m_0 = 357.5277; // coefficients for mean anomaly - float dm = 35999.049; // coefficients for mean anomaly - float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis - float e1 = 0.74508 * M_PI / 180.; + float omega_0 = 280.46 * DEG2RAD; // RAAN plus argument of + // perigee + float m_0 = 357.5277; // coefficients for mean anomaly + float dm = 35999.049; // coefficients for mean anomaly + float e = 23.4392911 * DEG2RAD; // angle of earth's rotation axis + float e1 = 0.74508 * DEG2RAD; - float p1 = 6892. / 3600. * M_PI / 180.; // some parameter - float p2 = 72. / 3600. * M_PI / 180.; // some parameter + float p1 = 6892. / 3600. * DEG2RAD; // some parameter + float p2 = 72. / 3600. * DEG2RAD; // some parameter } sunModelParameters; struct KalmanFilterParameters { - double sensorNoiseSTR = 0.1 * M_PI / 180; - double sensorNoiseSS = 8 * M_PI / 180; - double sensorNoiseMAG = 4 * M_PI / 180; - double sensorNoiseGYR = 0.1 * M_PI / 180; + double sensorNoiseStr = 0.1 * DEG2RAD; + double sensorNoiseSus = 8. * DEG2RAD; + double sensorNoiseMgm = 4. * DEG2RAD; + double sensorNoiseGyr = 0.1 * DEG2RAD; - double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk - double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability + double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk + double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability } kalmanFilterParameters; struct MagnetorquerParameter { @@ -960,8 +963,8 @@ class AcsParameters : public HasParametersIF { struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s - double omegaDetumbleStart = 2 * M_PI / 180; - double omegaDetumbleEnd = 1 * M_PI / 180; + double omegaDetumbleStart = 2 * DEG2RAD; + double omegaDetumbleEnd = 1 * DEG2RAD; double gainBdot = pow(10.0, -3.3); double gainFull = pow(10.0, -2.3); uint8_t useFullDetumbleLaw = false; From 883ff4f6de62d4f0f87b34156ccfb9ed07795c57 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 14:18:12 +0100 Subject: [PATCH 3/4] never needed this anyways --- .../acs/util/CholeskyDecomposition.h | 98 ------------------- 1 file changed, 98 deletions(-) delete mode 100644 mission/controller/acs/util/CholeskyDecomposition.h diff --git a/mission/controller/acs/util/CholeskyDecomposition.h b/mission/controller/acs/util/CholeskyDecomposition.h deleted file mode 100644 index 667f9f63..00000000 --- a/mission/controller/acs/util/CholeskyDecomposition.h +++ /dev/null @@ -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 -// typedef unsigned int uint8_t; - -template -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_ */ From 4e8776ff6861288c50fa9c07fd9ef414c3ce7faa Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 14:19:02 +0100 Subject: [PATCH 4/4] i shouldnt be doing this --- mission/controller/acs/util/MathOperations.h | 29 ++++++++++++++++---- 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index c240b9ed..7bf7319b 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -6,6 +6,10 @@ #include +// 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 class MathOperations { public: @@ -361,7 +365,7 @@ class MathOperations { 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; T1 matrix[size][size], identity[size][size]; // reformat array to matrix @@ -392,7 +396,7 @@ class MathOperations { rowIndex++; } if (!swaped) { - return 1; // matrix not invertible + return returnvalue::FAILED; // matrix not invertible } } } @@ -411,7 +415,7 @@ class MathOperations { } row--; if (row < 0) { - return 1; // Matrix is not invertible + return returnvalue::FAILED; // Matrix is not invertible } } } @@ -434,12 +438,12 @@ class MathOperations { } } std::memcpy(inverse, identity, sizeof(identity)); - return 0; // successful inversion + return returnvalue::OK; // successful inversion } static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) { for (uint8_t i = 0; i < size; i++) { - if (not isfinite(inputVector[i])) { + if (not std::isfinite(inputVector[i])) { return false; } } @@ -449,13 +453,26 @@ class MathOperations { 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 isfinite(inputMatrix[row * cols + cols])) { + 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_ */