Wip low pass filter

README.md

@@ -16,10 +16,12 @@ We have some zephyr rtos projects under zephyr_projects. To build/develop them y
 ### Arduino
 We have some arduino projects, under arduino_firmware.
 
-### common libraries
-Under common_libs/ we have our common libraries. These should be hardware agnostic, modular and testable.
+### Common Libraries
+Under common_libraries/ we have our common libraries. These should be hardware agnostic, modular and testable.
 We have an example here, that shows the general workflow for creating a common library module, including Catch2 testing.
 
+1. [Low pass FIR filter](common_libraries/low_pass_fir_filter/README.md)
+
 ### Testing
 We use Catch2 for testing cpp and c code. It's an easy to use, and easy to integrate tool for cpp unit tests.
 Ideally, all our code has test coverage. Test driven development (TDD) is a powerful process where if done perfectly, you never push a bug that would impact system operations, because your tests would cover every needed operation of the system.

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
     # Autogenerated files:
     # ${BUILD_DIR}/autogen/*.cpp
     # ${BUILD_DIR}/autogen/*.c
@@ -23,10 +23,11 @@ add_library(common_lib STATIC ${COMMON_LIB_SOURCES})
 
 target_include_directories(common_lib PUBLIC
     ${CMAKE_CURRENT_LIST_DIR}/example/
+    ${CMAKE_CURRENT_LIST_DIR}/low_pass_fir_filter/
 )
 
 add_executable(common_lib_tests
-    ${CMAKE_CURRENT_LIST_DIR}/example/test/test_some_lib.cpp)
+    ${CMAKE_CURRENT_LIST_DIR}/example/test/test_some_lib.cpp ${CMAKE_CURRENT_LIST_DIR}/low_pass_fir_filter/test/test_low_pass_fir_filter.cpp)
 
 
 target_link_libraries(common_lib_tests PRIVATE

common_libraries/low_pass_fir_filter/README.md

@@ -0,0 +1,18 @@
+# Low Pass FIR Filter
+This is an FIR filter that can be used on any unsigned 32-bit integers. You can specify the order (AKA. the number of previous data points the algorithm considers) when creating an instance of LowPassFIRFilter (default order of 1). The following example filters incoming data with an order of 1:
+```cpp
+#include <low_pass_fir_filter.hpp>
+
+LowPassFIRFilter filter;
+
+void process_incoming_data(unsigned int input) {
+    unsigned int filtered_input = filter.update(input);
+    send_filtered_data_to_arm(filtered_input);
+}
+```
+
+1. Constructor: defaults to order of 1
+2. Buffer: used to store previous data points
+3. Update function: 
+    - Fills the entire buffer with the first input to simplify the algorithm (otherwise cases for when the buffer is not full yet have to be handled differently).
+    - Uses sliding window to calculate the average of the correct set of data points.

common_libraries/low_pass_fir_filter/low_pass_fir_filter.cpp

@@ -0,0 +1,34 @@
+#include "low_pass_fir_filter.hpp"
+#include <stdexcept>
+#include <cstdio>
+
+LowPassFIRFilter::LowPassFIRFilter() : LowPassFIRFilter(1) {}
+
+LowPassFIRFilter::LowPassFIRFilter(unsigned int order)
+    : buffer_is_empty(true), buffer(), buffer_index(0), order(order),
+    coefficient(1.0f / (order + 1)), output(0) {
+    // Valid input check
+    if (order < 1 || order > MAX_LP_FIR_ORDER) {
+        throw std::invalid_argument("order must be at least 1 and less than the max order");
+    }
+}
+
+unsigned int LowPassFIRFilter::update(unsigned int input) {
+    if (this->buffer_is_empty) {
+        // fill entire buffer with the first input
+        for (int i = 0; i < this->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) % (this->order + 1);
+
+    return this->output;
+}

common_libraries/low_pass_fir_filter/low_pass_fir_filter.hpp

@@ -0,0 +1,34 @@
+#ifndef FIR_FILTER_HPP
+#define FIR_FILTER_HPP
+#define MAX_LP_FIR_ORDER (10)
+
+/*
+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).
+*/
+
+class LowPassFIRFilter {
+private:
+    bool buffer_is_empty;
+    unsigned int buffer[MAX_LP_FIR_ORDER];
+    unsigned int buffer_index;
+    float coefficient;
+    float output;
+
+public:
+    unsigned int order;
+    // Default constructor (order 1, equal weights)
+    LowPassFIRFilter();
+
+    // Constructor with filter order (uses equal weights)
+    // order = number of previous inputs to use
+    LowPassFIRFilter(unsigned int order);
+
+    // Update filter with new input and return filtered output
+    unsigned int update(unsigned int input);
+};
+
+#endif

common_libraries/low_pass_fir_filter/test/test_low_pass_fir_filter.cpp

@@ -0,0 +1,71 @@
+#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>
+
+
+TEST_CASE("test_constructor")
+{
+
+    SECTION("test_no_order")
+    {
+        LowPassFIRFilter filter_instance;
+        REQUIRE(filter_instance.order == 1);
+    }
+
+    SECTION("valid_orders")
+    {
+        LowPassFIRFilter filter_instance_1(1);
+        REQUIRE(filter_instance_1.order == 1);
+
+        LowPassFIRFilter filter_instance_5(5);
+        REQUIRE(filter_instance_5.order == 5);
+
+        LowPassFIRFilter filter_instance_10(MAX_LP_FIR_ORDER);
+        REQUIRE(filter_instance_10.order == MAX_LP_FIR_ORDER);
+    }
+
+    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);
+    }
+
+}
+
+TEST_CASE("test_update")
+{
+    SECTION("test_empty_buffer_same_input") {
+        LowPassFIRFilter filter_instance(5);
+        unsigned int val = 50;
+        for (unsigned int i = 0; i < 7; i++) {
+            REQUIRE(filter_instance.update(val) == 50);
+        }
+    }
+
+    /*
+        buffer = [50, 50, 50, 50, 50] -> output = 50
+        buffer = [100, 50, 50, 50, 50] -> output = 60
+        buffer = [100, 20, 50, 50, 50] -> output = 54
+    */
+    SECTION("test_empty_buffer_different_inputs") {
+        LowPassFIRFilter filter_instance(4);
+        unsigned int inputs[] = {50, 50, 50, 100, 20};
+        unsigned int expected[] = {50, 50, 50, 60, 54};
+        unsigned int n = 5;
+        for (unsigned int i = 0; i < n; i++) {
+            REQUIRE(filter_instance.update(inputs[i]) == expected[i]);
+        }
+    }
+
+
+    SECTION("test_input_spikes") {
+        LowPassFIRFilter filter_instance(1);
+        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};
+
+        unsigned int n = 11;
+        for (unsigned int i = 0; i < n; i++) {
+            REQUIRE(filter_instance.update(inputs[i]) == expected[i]);
+        }
+    }
+}