diff --git a/src/shared/algorithms/Filters/AdjustableMedFilter.h b/src/shared/algorithms/Filters/AdjustableMedFilter.h index a068864c3ba774aad3a12db02d24d14dc9ea5d26..6b806c6ce99569e72890f90df4a8b0439e2c1a19 100644 --- a/src/shared/algorithms/Filters/AdjustableMedFilter.h +++ b/src/shared/algorithms/Filters/AdjustableMedFilter.h @@ -24,6 +24,7 @@ #include <utils/SlidingWindow.h> +#include <algorithm> #include <array> namespace Boardcore diff --git a/src/shared/sensors/SFD/PressureSFD.cpp b/src/shared/sensors/SFD/PressureSFD.cpp index 6740322cd9f4939b2170444d7fec5291cfbbb165..390378b149d587dab96d51f9db7dfa1d27e2ea71 100644 --- a/src/shared/sensors/SFD/PressureSFD.cpp +++ b/src/shared/sensors/SFD/PressureSFD.cpp @@ -67,6 +67,9 @@ void PressureSFD::setMode(SFDMode mode) case SFDMode::DESCENT: k = K_DESCENT; break; + + case SFDMode::UNSET: + break; } } @@ -168,6 +171,9 @@ void PressureSFD::setDisabledSensors() } } break; + + case SFDMode::UNSET: + break; } // reset all sensors if all of them are faulty diff --git a/src/shared/sensors/SFD/PressureSFD.h b/src/shared/sensors/SFD/PressureSFD.h index 070966abf8798cec791fe04321798022904ddd3f..553c77d3acb95d7b9f5c37891c4ccbe9572111a5 100644 --- a/src/shared/sensors/SFD/PressureSFD.h +++ b/src/shared/sensors/SFD/PressureSFD.h @@ -52,6 +52,7 @@ public: static constexpr int WIN_LEN = MASKED_SENSORS * LEN_CHUNK; // TODO: set these values + static constexpr int MED_FILTER_WIN_LEN = 25; static constexpr float K_ACTIVE_ASCENT = 0; static constexpr float K_PASSIVE_ASCENT = 0; static constexpr float K_APOGEE_PROXIMITY = 0; @@ -92,7 +93,7 @@ private: std::array<float, MASKED_SENSORS> weights; LowPass lowPassFilter; - AdjustableMedFilter<WIN_LEN> medianFilter; + AdjustableMedFilter<MED_FILTER_WIN_LEN> medianFilter; std::array<std::function<PressureData()>, MASKED_SENSORS> pressSamplers; std::array<bool, MASKED_SENSORS> disabledSensors; diff --git a/src/shared/utils/SlidingWindow.h b/src/shared/utils/SlidingWindow.h index 37c0cbc58c5d8ceb4c440256d871f4c8ec8d60bb..2494f623fcad1ec5d6179e5b1f50e87a43f6c5d7 100644 --- a/src/shared/utils/SlidingWindow.h +++ b/src/shared/utils/SlidingWindow.h @@ -31,25 +31,25 @@ template <typename T, size_t D> class SlidingWindow { public: - explicit SlidingWindow() : window({0}), filled(0) {} + explicit SlidingWindow() : window({0}), valuesFilled(0) {} void push(T value) { shiftWindow(1); - filled < D ? filled++ : filled; + valuesFilled < D ? valuesFilled++ : valuesFilled; setLast(value); } - bool isFull() { return filled == D; } + bool isFull() { return valuesFilled == D; } /** * @brief Get the actual number of elements in the window */ - size_t filled() { return filled; } + size_t filled() { return valuesFilled; } T last() { return window[D - 1]; } - std::array<T, D>& all() { return &window; } + std::array<T, D>& all() { return window; } private: inline void setLast(T value) { window[D - 1] = value; } @@ -62,14 +62,14 @@ private: */ void shiftWindow(size_t n) { - for (int i = 0; i < D - n; i++) + for (size_t i = 0; i < D - n; i++) { window[i] = window[i + n]; } } std::array<T, D> window; - size_t filled; + size_t valuesFilled; }; } // namespace Boardcore