diff --git a/src/fsfw/globalfunctions/math/VectorOperations.h b/src/fsfw/globalfunctions/math/VectorOperations.h index 1e84330e..b8f6b00f 100644 --- a/src/fsfw/globalfunctions/math/VectorOperations.h +++ b/src/fsfw/globalfunctions/math/VectorOperations.h @@ -53,8 +53,9 @@ class VectorOperations { mulScalar(vector, 1 / norm(vector, size), normalizedVector, size); } - static T maxAbsValue(const T *vector, uint8_t size, uint8_t *index = 0) { + static T maxAbsValue(const T *vector, uint8_t size, uint8_t *index = nullptr) { T max = vector[size - 1]; + uint8_t foundIndex = size - 1; for (; size > 0; size--) { T abs = vector[size - 1]; @@ -64,24 +65,35 @@ class VectorOperations { if (abs > max) { max = abs; if (index != 0) { - *index = size - 1; + foundIndex = size - 1; } } } + + if (index != nullptr) { + *index = foundIndex; + } + return max; } - static T maxValue(const T *vector, uint8_t size, uint8_t *index = 0) { + static T maxValue(const T *vector, uint8_t size, uint8_t *index = nullptr) { T max = vector[size - 1]; + uint8_t foundIndex = size - 1; for (; size > 0; size--) { if (vector[size - 1] > max) { max = vector[size - 1]; if (index != 0) { - *index = size - 1; + foundIndex = size - 1; } } } + + if (index != nullptr) { + *index = foundIndex; + } + return max; }