Merge branch 'mekf-fix' into prevent-str-blinding

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_ */

mission/controller/acs/util/CholeskyDecomposition.h

@@ -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_ */

mission/controller/acs/util/MathOperations.h

@@ -1,14 +1,14 @@
 #ifndef 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/sign.h>
-#include <fsfw/src/fsfw/serviceinterface.h>
 
 #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>
 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_ */