diff --git a/common_libraries/CMakeLists.txt b/common_libraries/CMakeLists.txt index 0edec9f..248e666 100644 --- a/common_libraries/CMakeLists.txt +++ b/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 diff --git a/common_libraries/low_pass_fir_filter/low_pass_fir_filter.cpp b/common_libraries/low_pass_fir_filter/low_pass_fir_filter.cpp deleted file mode 100644 index 39900c2..0000000 --- a/common_libraries/low_pass_fir_filter/low_pass_fir_filter.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "low_pass_fir_filter.hpp" -#include -#include - -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; -} \ No newline at end of file diff --git a/common_libraries/low_pass_fir_filter/low_pass_fir_filter.h b/common_libraries/low_pass_fir_filter/low_pass_fir_filter.h new file mode 100644 index 0000000..926da48 --- /dev/null +++ b/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 +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 \ No newline at end of file diff --git a/common_libraries/low_pass_fir_filter/low_pass_fir_filter.hpp b/common_libraries/low_pass_fir_filter/low_pass_fir_filter.hpp deleted file mode 100644 index 150a9ed..0000000 --- a/common_libraries/low_pass_fir_filter/low_pass_fir_filter.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#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 \ No newline at end of file diff --git a/common_libraries/low_pass_fir_filter/test/test_low_pass_fir_filter.cpp b/common_libraries/low_pass_fir_filter/test/test_low_pass_fir_filter.cpp index 0e302df..aafb72c 100644 --- a/common_libraries/low_pass_fir_filter/test/test_low_pass_fir_filter.cpp +++ b/common_libraries/low_pass_fir_filter/test/test_low_pass_fir_filter.cpp @@ -1,33 +1,42 @@ #define CATCH_CONFIG_MAIN // Let catch2 handle the main function and boiler plate code. #include -#include +#include +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); } } @@ -35,10 +44,10 @@ TEST_CASE("test_constructor") 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};