Utility: add filters library

Tests/CMakeLists.txt

@@ -30,6 +30,7 @@ add_executable(tests
     assert.cpp
     enum_helpers.cpp
     bytes.cpp
+    filters.cpp
 )
 
 target_link_libraries(tests

Tests/filters.cpp

@@ -0,0 +1,178 @@
+//
+// Created by erki on 12/11/22.
+//
+
+#include <catch2/catch.hpp>
+
+#include "utility_filters.hpp"
+
+TEST_CASE("LowPassFilter works as expected.")
+{
+  const int threshold = 50;
+  Utility::LowPassFilter<int> filter{threshold};
+
+  SECTION("Initial value is 0.")
+  {
+    REQUIRE(filter.currentValue() == 0);
+  }
+
+  SECTION("Passing a number higher than threshold gets filtered.")
+  {
+    filter.update(60);
+    REQUIRE(filter.currentValue() == 0);
+
+    SECTION("Filter updates state as expected.")
+    {
+      filter.update(40);
+      REQUIRE(filter.currentValue() == 40);
+    }
+  }
+
+  SECTION("Passing a number lower than threshold passes filter.")
+  {
+    filter.update(40);
+    REQUIRE(filter.currentValue() == 40);
+
+    SECTION("Filter retains state as expected.")
+    {
+      filter.update(60);
+      REQUIRE(filter.currentValue() == 40);
+    }
+  }
+}
+
+TEST_CASE("HighPassFilter works as expected.")
+{
+  const int threshold = 50;
+  Utility::HighPassFilter<int> filter{threshold};
+
+  SECTION("Initial value is 0.")
+  {
+    REQUIRE(filter.currentValue() == 0);
+  }
+
+  SECTION("Passing a number higher than threshold passes filter.")
+  {
+    filter.update(60);
+    REQUIRE(filter.currentValue() == 60);
+
+    SECTION("Filter retains state as expected.")
+    {
+      filter.update(40);
+      REQUIRE(filter.currentValue() == 60);
+    }
+  }
+
+  SECTION("Passing a number lower than threshold gets filtered.")
+  {
+    filter.update(40);
+    REQUIRE(filter.currentValue() == 0);
+
+    SECTION("Filter updates state as expected.")
+    {
+      filter.update(60);
+      REQUIRE(filter.currentValue() == 60);
+    }
+  }
+}
+
+TEST_CASE("Two filters can be linked together.")
+{
+  const int threshold_low = 40;
+  const int threshold_high = 50;
+
+  Utility::LowPassFilter<int> low_pass{threshold_high};
+  Utility::HighPassFilter<int> high_pass{threshold_low};
+
+  high_pass.assignPrecedingFilter(low_pass);
+
+  SECTION("Initial value is 0.")
+  {
+    REQUIRE(high_pass.currentValue() == 0);
+  }
+
+  SECTION("Values outside of band are filtered out.")
+  {
+    high_pass.update(30);
+    REQUIRE(high_pass.currentValue() == 0);
+
+    high_pass.update(60);
+    REQUIRE(high_pass.currentValue() == 0);
+
+    SECTION("Filter updates with in-band value.")
+    {
+      high_pass.update(45);
+      REQUIRE(high_pass.currentValue() == 45);
+    }
+  }
+
+  SECTION("Values inside of band are passed through.")
+  {
+    high_pass.update(44);
+    REQUIRE(high_pass.currentValue() == 44);
+
+    high_pass.update(46);
+    REQUIRE(high_pass.currentValue() == 46);
+
+    SECTION("Out of band values are filtered out.")
+    {
+      high_pass.update(30);
+      REQUIRE(high_pass.currentValue() == 46);
+
+      high_pass.update(60);
+      REQUIRE(high_pass.currentValue() == 46);
+    }
+  }
+}
+
+TEST_CASE("Median filter works as expected.")
+{
+  Utility::MedianFilter<int, 5> filter;
+
+  SECTION("Initial value is 0.")
+  {
+    REQUIRE(filter.currentValue() == 0);
+  }
+
+  SECTION("The median is filtered appropriately.")
+  {
+    std::array<int, 5> data = {5, 1, 4, 3, 2};
+    std::array<int, 5> expected_median = {0, 0, 1, 3, 3};
+    for (int i = 0; i < 5; i++)
+    {
+      const int x = data[i];
+      filter.update(x);
+      REQUIRE(filter.currentValue() == expected_median[i]);
+    }
+  }
+}
+
+TEST_CASE("Rolling average filter works as expected.")
+{
+  Utility::RollingAverageFilter<int> filter{5};
+
+  SECTION("Initial value is 0.")
+  {
+    REQUIRE(filter.currentValue() == 0);
+  }
+
+  SECTION("Value below step size is filtered out for int filter.")
+  {
+    filter.update(4);
+    REQUIRE(filter.currentValue() == 0);
+  }
+
+  SECTION("Value is updated appropriately.")
+  {
+    filter.update(20);
+    REQUIRE(filter.currentValue() == 20 / 5);
+
+    SECTION("Saturation is reached appropriately")
+    {
+      for (int i = 0; i < 5 * 2 - 1; i++)
+        filter.update(20);
+
+      REQUIRE(filter.currentValue() == 20);
+    }
+  }
+}

Utility/Inc/utility_filters.hpp

@@ -0,0 +1,124 @@
+//
+// Created by erki on 12/11/22.
+//
+
+#ifndef SKULLC_UTILITY_FILTERS_HPP_
+#define SKULLC_UTILITY_FILTERS_HPP_
+
+#include <algorithm>
+#include <array>
+
+#include "utility_ifilter.hpp"
+
+namespace Utility
+{
+
+template<typename T>
+class RollingAverageFilter : public IFilter<T>
+{
+public:
+  RollingAverageFilter() = delete;
+  explicit RollingAverageFilter(const std::size_t step)
+      : step_size_(step)
+  {}
+
+  T currentValue() const override
+  {
+    return value_;
+  }
+
+protected:
+  void updateImpl_(const T& data) override
+  {
+    value_ -= value_ / step_size_;
+    value_ += data / step_size_;
+  }
+
+private:
+  std::size_t step_size_;
+  T value_ = 0;
+};
+
+template<typename T, std::size_t N>
+class MedianFilter : public IFilter<T>
+{
+public:
+  MedianFilter()
+  {
+    data_sequence_.fill(T(0));
+  }
+
+  T currentValue() const override
+  {
+    auto sorted = data_sequence_;
+    std::sort(sorted.begin(), sorted.end());
+
+    return sorted[N / 2];
+  }
+
+protected:
+  void updateImpl_(const T& data) override
+  {
+    std::move(data_sequence_.begin() + 1, data_sequence_.end(), data_sequence_.begin());
+    data_sequence_[N - 1] = data;
+  }
+
+private:
+  std::array<T, N> data_sequence_;
+};
+
+template<typename T>
+class LowPassFilter : public IFilter<T>
+{
+public:
+  LowPassFilter() = delete;
+  explicit LowPassFilter(const T threshold)
+      : threshold_(threshold)
+  {}
+
+  T currentValue() const override
+  {
+    return value_;
+  }
+
+protected:
+  void updateImpl_(const T& data) override
+  {
+    if (data < threshold_)
+      value_ = data;
+  }
+
+private:
+  T threshold_;
+  T value_ = 0;
+};
+
+template<typename T>
+class HighPassFilter : public IFilter<T>
+{
+public:
+  HighPassFilter() = delete;
+  explicit HighPassFilter(const T threshold)
+      : threshold_(threshold)
+  {}
+
+  T currentValue() const override
+  {
+    return value_;
+  }
+
+protected:
+  void updateImpl_(const T& data) override
+  {
+    if (data > threshold_)
+      value_ = data;
+  }
+
+private:
+  T threshold_;
+  T value_ = 0;
+};
+
+}// namespace Utility
+
+#endif//SKULLC_UTILITY_FILTERS_HPP_

Utility/Inc/utility_ifilter.hpp

@@ -0,0 +1,49 @@
+//
+// Created by erki on 12/11/22.
+//
+
+#ifndef SKULLC_UTILITY_IFILTER_HPP_
+#define SKULLC_UTILITY_IFILTER_HPP_
+
+namespace Utility
+{
+
+template<typename T>
+class IFilter
+{
+public:
+  virtual ~IFilter() {}
+
+  void assignPrecedingFilter(IFilter<T>& filter)
+  {
+    preceding_filter_ = &filter;
+  }
+
+  void clearPrecedingFilter()
+  {
+    preceding_filter_ = nullptr;
+  }
+
+  void update(T data)
+  {
+    if (preceding_filter_)
+    {
+      preceding_filter_->update(data);
+      data = preceding_filter_->currentValue();
+    }
+
+    updateImpl_(data);
+  }
+
+  virtual T currentValue() const = 0;
+
+protected:
+  virtual void updateImpl_(const T& data) = 0;
+
+private:
+  IFilter<T>* preceding_filter_ = nullptr;
+};
+
+}// namespace Utility
+
+#endif//SKULLC_UTILITY_IFILTER_HPP_