diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index b8d1fa4d..d8c8d0d6 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -10,6 +10,8 @@ #include +#include "fsfw/timemanager/Stopwatch.h" + using namespace Math; template @@ -300,6 +302,7 @@ class MathOperations { } static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) { + /* do not use this. takes 300ms */ float det = 0; T1 matrix[size][size], submatrix[size - 1][size - 1]; for (uint8_t row = 0; row < size; row++) { @@ -329,9 +332,11 @@ class MathOperations { } static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { - if (MathOperations::matrixDeterminant(inputMatrix, size) == 0) { - return 1; // Matrix is singular and not invertible - } + // Stopwatch stopwatch; + // if (MathOperations::matrixDeterminant(inputMatrix, size) == 0) { + // return 1; // Matrix is singular and not invertible + // } + // stopwatch.stop(true); T1 matrix[size][size], identity[size][size]; // reformat array to matrix for (uint8_t row = 0; row < size; row++) {