eive/eive-obsw
eive
/
eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests Releases 117 Wiki Activity

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

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