Template low pass filter

common_libraries/CMakeLists.txt

@@ -13,7 +13,7 @@ set(BUILD_DIR ${CMAKE_CURRENT_LIST_DIR}/build)
 
 set(COMMON_LIB_SOURCES 
     ${CMAKE_CURRENT_LIST_DIR}/example/some_lib.cpp
-    ${CMAKE_CURRENT_LIST_DIR}/low_pass_fir_filter/low_pass_fir_filter.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/low_pass_fir_filter/low_pass_fir_filter.h
     # Autogenerated files:
     # ${BUILD_DIR}/autogen/*.cpp
     # ${BUILD_DIR}/autogen/*.c

common_libraries/low_pass_fir_filter/low_pass_fir_filter.h

@@ -0,0 +1,55 @@
+#ifndef FIR_FILTER_HPP
+#define FIR_FILTER_HPP
+
+/*
+This class is used to apply a low pass FIR filter to a stream of inputs with a specific order. This filter is simply taking S=[ current_input, previous_input_1, previous_input_2, ... , previous_input_{order} ], and returning the average.
+
+The buffer is initially filled with the first input to simplfy the algorithm (this removes the need for taking the average of less than "order" + 1 number of inputs).
+
+The buffer is of size "order" + 1 since we need to store the current input, and also remember the oldest input (to remove from the total output in a subsequent update).
+*/
+
+template <int order=1>
+class LowPassFIRFilter {
+private:
+    bool buffer_is_empty = true;
+    unsigned int buffer[order+1];
+    unsigned int buffer_index = 0;
+    float coefficient = 1.0f / (order + 1);
+    float output = 0;
+
+public:
+    LowPassFIRFilter() {
+        // Valid input check
+        if (order < 1) {
+            throw std::invalid_argument("order must be at least 1");
+        }
+    };
+
+    unsigned int getOrder() const {
+        return order;
+    }
+
+    // Update filter with new input and return filtered output
+    unsigned int update(unsigned int input) {
+        if (this->buffer_is_empty) {
+            // fill entire buffer with the first input
+            for (int i = 0; i < order + 1; i++) {
+                this->buffer[i] = input;
+            }
+            this->buffer_is_empty = false;
+            return this->output = input;
+        }
+
+        // `input` is the new data point that is begin added. `buffer[buffer_index]` is the old data point that must be removed. 
+        this->output = this->output + input * this->coefficient - this->buffer[this->buffer_index] * this->coefficient;
+        this->buffer[this->buffer_index] = input;
+
+        // the index must wrap around once it reaches the right most side of the buffer. Note that buffer size is order + 1.
+        this->buffer_index = (this->buffer_index + 1) % (order + 1);
+
+        return this->output;
+    };
+};
+
+#endif

common_libraries/low_pass_fir_filter/test/test_low_pass_fir_filter.cpp

@@ -1,44 +1,53 @@
 #define CATCH_CONFIG_MAIN // Let catch2 handle the main function and boiler plate code.
 #include <catch2/catch_all.hpp>
-#include <low_pass_fir_filter.hpp>
+#include <low_pass_fir_filter.h>
 
 
+TEST_CASE("test_getOrder")
+{
+    LowPassFIRFilter<2> filter_instance_1;
+    REQUIRE(filter_instance_1.getOrder() == 2);
+
+    LowPassFIRFilter<5> filter_instance_5;
+    REQUIRE(filter_instance_5.getOrder() == 5);
+}
+
 TEST_CASE("test_constructor")
 {
 
     SECTION("test_no_order")
     {
         LowPassFIRFilter filter_instance;
-        REQUIRE(filter_instance.order == 1);
+        REQUIRE(filter_instance.getOrder() == 1);
     }
 
-    SECTION("valid_orders")
+    SECTION("test_valid_orders")
     {
-        LowPassFIRFilter filter_instance_1(1);
-        REQUIRE(filter_instance_1.order == 1);
+        LowPassFIRFilter<1> filter_instance_1;
+        REQUIRE(filter_instance_1.getOrder() == 1);
 
-        LowPassFIRFilter filter_instance_5(5);
-        REQUIRE(filter_instance_5.order == 5);
+        LowPassFIRFilter<5> filter_instance_5;
+        REQUIRE(filter_instance_5.getOrder() == 5);
 
-        LowPassFIRFilter filter_instance_10(MAX_LP_FIR_ORDER);
-        REQUIRE(filter_instance_10.order == MAX_LP_FIR_ORDER);
+        LowPassFIRFilter<10> filter_instance_10;
+        REQUIRE(filter_instance_10.getOrder() == 10);
     }
 
     SECTION("test_order_too_low_or_too_high")
     {
-        REQUIRE_THROWS_AS(LowPassFIRFilter(0), std::invalid_argument);
-        REQUIRE_THROWS_AS(LowPassFIRFilter(MAX_LP_FIR_ORDER + 1), std::invalid_argument);
+        REQUIRE_THROWS_AS(LowPassFIRFilter<-1>(), std::invalid_argument);
+        REQUIRE_THROWS_AS(LowPassFIRFilter<0>(), std::invalid_argument);
     }
 
 }
 
 TEST_CASE("test_update")
 {
     SECTION("test_empty_buffer_same_input") {
-        LowPassFIRFilter filter_instance(5);
+        LowPassFIRFilter<5> filter_instance;
         unsigned int val = 50;
         for (unsigned int i = 0; i < 7; i++) {
-            REQUIRE(filter_instance.update(val) == 50);
+            REQUIRE(filter_instance.update(val) == val);
         }
     }
 
@@ -48,7 +57,7 @@ TEST_CASE("test_update")
         buffer = [100, 20, 50, 50, 50] -> output = 54
     */
     SECTION("test_empty_buffer_different_inputs") {
-        LowPassFIRFilter filter_instance(4);
+        LowPassFIRFilter<4> filter_instance;
         unsigned int inputs[] = {50, 50, 50, 100, 20};
         unsigned int expected[] = {50, 50, 50, 60, 54};
         unsigned int n = 5;
@@ -59,7 +68,7 @@ TEST_CASE("test_update")
 
 
     SECTION("test_input_spikes") {
-        LowPassFIRFilter filter_instance(1);
+        LowPassFIRFilter filter_instance;
         unsigned int inputs[] = {50, 60, 70, 500, 100, 60, 50, 40, 50, 40, 50};
         unsigned int expected[] = {50, 55, 65, 285, 300, 80, 55, 45, 45, 45, 45};