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

i shouldnt be doing this
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2024-01-19 14:19:02 +01:00
parent 883ff4f6de
commit 4e8776ff68
1 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -6,6 +6,10 @@
#include <cmath> #include <cmath>
// 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 {
public: public:
@ -361,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
@ -392,7 +396,7 @@ class MathOperations {
rowIndex++; rowIndex++;
} }
if (!swaped) { if (!swaped) {
return 1; // matrix not invertible return returnvalue::FAILED; // matrix not invertible
} }
} }
} }
@ -411,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
} }
} }
} }
@ -434,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;
} }
} }
@ -449,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_ */