diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f4a1f309..6bc20594 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(); @@ -884,7 +887,7 @@ class AcsParameters : public HasParametersIF { uint8_t avoidBlindStr = true; double blindAvoidStart = 1.5; double blindAvoidStop = 2.5; - double blindRotRate = 1 * DEG2RAD; + double blindRotRate = 1. * DEG2RAD; } targetModeControllerParameters; struct GsTargetModeControllerParameters : PointingLawParameters { @@ -911,7 +914,7 @@ class AcsParameters : public HasParametersIF { } inertialModeControllerParameters; struct StrParameters { - double exclusionAngle = 20 * DEG2RAD; + double exclusionAngle = 20. * DEG2RAD; double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame } strParameters; @@ -937,13 +940,13 @@ class AcsParameters : public HasParametersIF { } sunModelParameters; struct KalmanFilterParameters { - double sensorNoiseSTR = 0.1 * DEG2RAD; - double sensorNoiseSS = 8 * DEG2RAD; - double sensorNoiseMAG = 4 * DEG2RAD; - double sensorNoiseGYR = 0.1 * DEG2RAD; + double sensorNoiseStr = 0.1 * DEG2RAD; + double sensorNoiseSus = 8. * DEG2RAD; + double sensorNoiseMgm = 4. * DEG2RAD; + double sensorNoiseGyr = 0.1 * DEG2RAD; - double sensorNoiseArwGYR = 3 * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk - double sensorNoiseBsGYR = 3 * DEG2RAD / 3600; // Bias Stability + double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk + double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability } kalmanFilterParameters; struct MagnetorquerParameter { @@ -965,8 +968,6 @@ class AcsParameters : public HasParametersIF { double gainFull = pow(10.0, -2.3); uint8_t useFullDetumbleLaw = false; } detumbleParameter; - - private: - static constexpr double DEG2RAD = M_PI / 180.; }; + #endif /* ACSPARAMETERS_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_ */ diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 023e9379..7bf7319b 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -1,14 +1,14 @@ #ifndef MATH_MATHOPERATIONS_H_ #define MATH_MATHOPERATIONS_H_ -#include #include #include -#include #include -#include -#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 { @@ -365,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 @@ -396,7 +396,7 @@ class MathOperations { rowIndex++; } if (!swaped) { - return 1; // matrix not invertible + return returnvalue::FAILED; // matrix not invertible } } } @@ -415,7 +415,7 @@ class MathOperations { } row--; 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)); - 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; } } @@ -453,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_ */