Skip to content

Commit

Permalink
test(Input): cover filters
Browse files Browse the repository at this point in the history
  • Loading branch information
@leon0399
leon0399 committed Feb 10, 2024
1 parent 01c9a06 commit 825e6f0
Show file tree
Hide file tree
Showing 3 changed files with 207 additions and 18 deletions.
49 changes: 31 additions & 18 deletions lib/io/senseshift/input/filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <cstdlib>
#include <deque>
Expand Down Expand Up @@ -33,17 +34,6 @@ namespace SenseShift::Input::Filter {
virtual auto filter(ISimpleSensor<ValueType>* sensor, ValueType value) -> ValueType = 0;
};

template<typename Tp>
class AddFilter : public IFilter<Tp> {
public:
explicit AddFilter(Tp offset) : offset_(offset){};

auto filter(ISimpleSensor<Tp>* /*sensor*/, Tp value) -> Tp override { return value + this->offset_; }

private:
Tp offset_;
};

template<typename Tp>
class IFiltered {
public:
Expand All @@ -58,6 +48,17 @@ namespace SenseShift::Input::Filter {
void clearFilters() = 0;
};

template<typename Tp>
class AddFilter : public IFilter<Tp> {
public:
explicit AddFilter(Tp offset) : offset_(offset){};

auto filter(ISimpleSensor<Tp>* /*sensor*/, Tp value) -> Tp override { return value + this->offset_; }

private:
Tp offset_;
};

template<typename Tp>
class SubtractFilter : public IFilter<Tp> {
public:
Expand All @@ -82,6 +83,8 @@ namespace SenseShift::Input::Filter {

class VoltageDividerFilter : public MultiplyFilter<float> {
public:
/// Calculates the original voltage from the voltage divider.
///
/// \param r1 The resistance in Ohms of the first resistor in the voltage divider.
/// Example: 27000.0F.
/// \param r2 The resistance in Ohms of the second resistor in the voltage divider.
Expand All @@ -91,7 +94,7 @@ namespace SenseShift::Input::Filter {
/// \code
/// new VoltageDividerFilter(27000.0F, 100000.0F);
/// \endcode
explicit VoltageDividerFilter(float r1, float r2) : MultiplyFilter<float>(r2 / (r1 + r2)){};
explicit VoltageDividerFilter(float r1, float r2) : MultiplyFilter<float>((r1 + r2) / r2){};
};

template<typename Tp>
Expand Down Expand Up @@ -220,7 +223,7 @@ namespace SenseShift::Input::Filter {

private:
std::size_t window_size_;
std::deque<Tp> values_;
std::deque<Tp> queue_;

[[nodiscard]] auto getAverage() const -> Tp
{
Expand All @@ -239,14 +242,24 @@ namespace SenseShift::Input::Filter {
public:
explicit ExponentialMovingAverageFilter(float alpha) : alpha_(alpha){};

template<typename U = Tp, std::enable_if_t<std::is_same_v<U, float>, int> = 0>
explicit ExponentialMovingAverageFilter(float alpha) : alpha_(alpha), acc_(std::nanf){};

auto filter(ISimpleSensor<Tp>* /*sensor*/, Tp value) -> Tp override
{
this->acc_ = this->alpha_ * value + (1 - this->alpha_) * this->acc_;
if (this->is_first_) {
this->is_first_ = false;

this->acc_ = value;
return this->acc_;
}

this->acc_ = (this->alpha_ * value) + ((1 - this->alpha_) * this->acc_);
return this->acc_;
}

private:
bool is_first_ = true;
float alpha_;
Tp acc_;
};
Expand All @@ -255,16 +268,16 @@ namespace SenseShift::Input::Filter {
/// Usually used to filter out noise in the joystick.
class CenterDeadzoneFilter : public IFilter<float> {
public:
explicit CenterDeadzoneFilter(float deadzone) : deadzone_(deadzone){};
explicit CenterDeadzoneFilter(float deadzone, float center = 0.5f) : deadzone_(deadzone), center_(center){};

auto filter(ISimpleSensor<float>* /*sensor*/, float value) -> float override
{
float const deviation = std::abs(CENTER - value);
return deviation < deadzone_ ? CENTER : value;
float const deviation = std::abs(value - this->center_);
return deviation < deadzone_ ? this->center_ : value;
}

private:
static constexpr float CENTER = 0.5F;
const float center_;

float deadzone_;
};
Expand Down
172 changes: 172 additions & 0 deletions test/test_io_filter/main.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,172 @@
#include <senseshift/input/filter.hpp>
#include <senseshift/input/sensor.hpp>
#include <unity.h>

#define ASSERT_EQUAL_FLOAT_ROUNDED(expected, actual, precision) \
TEST_ASSERT_EQUAL_FLOAT( \
std::round(expected* std::pow(10, precision)) / std::pow(10, precision), \
std::round(actual * std::pow(10, precision)) / std::pow(10, precision) \
)

using namespace SenseShift::Input;
using namespace SenseShift::Input::Filter;

void test_add_filter(void)
{
IFilter<float>* filter = new AddFilter<float>(5.0f);

TEST_ASSERT_EQUAL_FLOAT(10.0f, filter->filter(nullptr, 5.0f));
TEST_ASSERT_EQUAL_FLOAT(11.0f, filter->filter(nullptr, 6.0f));
TEST_ASSERT_EQUAL_FLOAT(12.0f, filter->filter(nullptr, 7.0f));
}

void test_subtract_filter(void)
{
IFilter<float>* filter = new SubtractFilter<float>(5.0f);

TEST_ASSERT_EQUAL_FLOAT(0.0f, filter->filter(nullptr, 5.0f));
TEST_ASSERT_EQUAL_FLOAT(-1.0f, filter->filter(nullptr, 4.0f));
TEST_ASSERT_EQUAL_FLOAT(-2.0f, filter->filter(nullptr, 3.0f));
}

void test_multiply_filter(void)
{
IFilter<float>* filter = new MultiplyFilter<float>(5.0f);

TEST_ASSERT_EQUAL_FLOAT(25.0f, filter->filter(nullptr, 5.0f));
TEST_ASSERT_EQUAL_FLOAT(30.0f, filter->filter(nullptr, 6.0f));
TEST_ASSERT_EQUAL_FLOAT(35.0f, filter->filter(nullptr, 7.0f));
}

void test_voltage_divider_filter(void)
{
IFilter<float>* filter = new VoltageDividerFilter(27000.0F, 100000.0F);

TEST_ASSERT_EQUAL_FLOAT(0.0F, filter->filter(nullptr, 0.0F));
TEST_ASSERT_EQUAL_FLOAT(3.429F, filter->filter(nullptr, 2.7F));
TEST_ASSERT_EQUAL_FLOAT(3.81F, filter->filter(nullptr, 3.0F));
TEST_ASSERT_EQUAL_FLOAT(4.191F, filter->filter(nullptr, 3.3F));
}

void test_clamp_filter(void)
{
IFilter<float>* filter = new ClampFilter<float>(0.0f, 1.0f);

TEST_ASSERT_EQUAL_FLOAT(0.0f, filter->filter(nullptr, -1.0f));
TEST_ASSERT_EQUAL_FLOAT(0.0f, filter->filter(nullptr, 0.0f));
TEST_ASSERT_EQUAL_FLOAT(0.5f, filter->filter(nullptr, 0.5f));
TEST_ASSERT_EQUAL_FLOAT(1.0f, filter->filter(nullptr, 1.0f));
TEST_ASSERT_EQUAL_FLOAT(1.0f, filter->filter(nullptr, 2.0f));
}

void test_lambda_filter(void)
{
IFilter<float>* filter = new LambdaFilter<float>([](float value) {
return value * 42.F; // Cause after all, 42 is the answer to everything.
});

TEST_ASSERT_EQUAL_FLOAT(42.0f, filter->filter(nullptr, 1.0f));
TEST_ASSERT_EQUAL_FLOAT(84.0f, filter->filter(nullptr, 2.0f));
TEST_ASSERT_EQUAL_FLOAT(126.0f, filter->filter(nullptr, 3.0f));
}

void test_sliding_window_moving_average_filter(void)
{
IFilter<float>* filter = new SlidingWindowMovingAverageFilter<float>(3);

TEST_ASSERT_EQUAL_FLOAT(1.0f, filter->filter(nullptr, 1.0f)); // 1 / 1 = 1
TEST_ASSERT_EQUAL_FLOAT(1.5f, filter->filter(nullptr, 2.0f)); // (1 + 2) / 2 = 1.5
TEST_ASSERT_EQUAL_FLOAT(2.0f, filter->filter(nullptr, 3.0f)); // (1 + 2 + 3) / 3 = 2
TEST_ASSERT_EQUAL_FLOAT(3.0f, filter->filter(nullptr, 4.0f)); // (2 + 3 + 4) / 3 = 3
TEST_ASSERT_EQUAL_FLOAT(4.0f, filter->filter(nullptr, 5.0f)); // (3 + 4 + 5) / 3 = 4
TEST_ASSERT_EQUAL_FLOAT(5.0f, filter->filter(nullptr, 6.0f)); // (4 + 5 + 6) / 3 = 5
TEST_ASSERT_EQUAL_FLOAT(6.0f, filter->filter(nullptr, 7.0f)); // (5 + 6 + 7) / 3 = 6
TEST_ASSERT_EQUAL_FLOAT(7.0f, filter->filter(nullptr, 8.0f)); // (6 + 7 + 8) / 3 = 7
TEST_ASSERT_EQUAL_FLOAT(8.0f, filter->filter(nullptr, 9.0f)); // (7 + 8 + 9) / 3 = 8
TEST_ASSERT_EQUAL_FLOAT(9.0f, filter->filter(nullptr, 10.0f)); // (8 + 9 + 10) / 3 = 9
TEST_ASSERT_EQUAL_FLOAT(10.0f, filter->filter(nullptr, 11.0f)); // (9 + 10 + 11) / 3 = 10

ASSERT_EQUAL_FLOAT_ROUNDED(10.67f, filter->filter(nullptr, 11.0f), 2); // (10 + 11 + 11) / 3 = 10.67
TEST_ASSERT_EQUAL_FLOAT(11.0f, filter->filter(nullptr, 11.0f)); // (11 + 11 + 11) / 3 = 11
ASSERT_EQUAL_FLOAT_ROUNDED(11.33f, filter->filter(nullptr, 12.0f), 2); // (11 + 11 + 12) / 3 = 11.33
}

void test_exponential_moving_average_filter(void)
{
IFilter<float>* filter = new ExponentialMovingAverageFilter<float>(0.5f);
ASSERT_EQUAL_FLOAT_ROUNDED(1.0f, filter->filter(nullptr, 1.0f), 2); // 1.0
ASSERT_EQUAL_FLOAT_ROUNDED(1.5f, filter->filter(nullptr, 2.0f), 2); // (0.5 * 1.0) + (0.5 * 2.0) = 1.5
ASSERT_EQUAL_FLOAT_ROUNDED(2.25f, filter->filter(nullptr, 3.0f), 2); // (0.5 * 1.5) + (0.5 * 3.0) = 2.25
ASSERT_EQUAL_FLOAT_ROUNDED(3.125f, filter->filter(nullptr, 4.0f), 2); // (0.5 * 2.25) + (0.5 * 4.0) = 3.125
ASSERT_EQUAL_FLOAT_ROUNDED(4.0625f, filter->filter(nullptr, 5.0f), 2); // (0.5 * 3.125) + (0.5 * 5.0) = 4.0625

filter = new ExponentialMovingAverageFilter<float>(0.1f);
ASSERT_EQUAL_FLOAT_ROUNDED(1.0f, filter->filter(nullptr, 1.0f), 2); // 1.0
ASSERT_EQUAL_FLOAT_ROUNDED(1.1f, filter->filter(nullptr, 2.0f), 2); // (0.1 * 2.0) + (0.9 * 1.0) = 1.1
ASSERT_EQUAL_FLOAT_ROUNDED(1.29f, filter->filter(nullptr, 3.0f), 2); // (0.1 * 3.0) + (0.9 * 1.1) = 1.29
ASSERT_EQUAL_FLOAT_ROUNDED(1.561f, filter->filter(nullptr, 4.0f), 2); // (0.1 * 4.0) + (0.9 * 1.29) = 1.561
ASSERT_EQUAL_FLOAT_ROUNDED(1.9049f, filter->filter(nullptr, 5.0f), 2); // (0.1 * 5.0) + (0.9 * 1.561) = 1.9049

filter = new ExponentialMovingAverageFilter<float>(0.9f);
ASSERT_EQUAL_FLOAT_ROUNDED(1.0f, filter->filter(nullptr, 1.0f), 2); // 1.0
ASSERT_EQUAL_FLOAT_ROUNDED(1.9f, filter->filter(nullptr, 2.0f), 2); // (0.9 * 2.0) + (0.1 * 1.0) = 1.9
ASSERT_EQUAL_FLOAT_ROUNDED(2.89f, filter->filter(nullptr, 3.0f), 2); // (0.9 * 3.0) + (0.1 * 1.9) = 2.89
ASSERT_EQUAL_FLOAT_ROUNDED(3.889f, filter->filter(nullptr, 4.0f), 2); // (0.9 * 4.0) + (0.1 * 2.89) = 3.889
ASSERT_EQUAL_FLOAT_ROUNDED(4.8889f, filter->filter(nullptr, 5.0f), 2); // (0.9 * 5.0) + (0.1 * 3.889) = 4.8889
ASSERT_EQUAL_FLOAT_ROUNDED(4.98889, filter->filter(nullptr, 5.0f), 2); // (0.9 * 5.0) + (0.1 * 4.8889) = 4.98889
ASSERT_EQUAL_FLOAT_ROUNDED(4.99889, filter->filter(nullptr, 5.0f), 2); // (0.9 * 5.0) + (0.1 * 4.98889) = 4.99889
ASSERT_EQUAL_FLOAT_ROUNDED(4.999889, filter->filter(nullptr, 5.0f), 2); // (0.9 * 5.0) + (0.1 * 4.99889) = 4.999889
}

void test_center_deadzone_filter(void)
{
IFilter<float>* filter = new CenterDeadzoneFilter(0.1f);

TEST_ASSERT_EQUAL_FLOAT(0.0f, filter->filter(nullptr, 0.0f));
TEST_ASSERT_EQUAL_FLOAT(0.1f, filter->filter(nullptr, 0.1f));

// Inside the deadzone
TEST_ASSERT_EQUAL_FLOAT(0.5f, filter->filter(nullptr, 0.43f));
TEST_ASSERT_EQUAL_FLOAT(0.5f, filter->filter(nullptr, 0.5f));
TEST_ASSERT_EQUAL_FLOAT(0.5f, filter->filter(nullptr, 0.57f));

TEST_ASSERT_EQUAL_FLOAT(0.9f, filter->filter(nullptr, 0.9f));
TEST_ASSERT_EQUAL_FLOAT(1.0f, filter->filter(nullptr, 1.0f));
}

int process(void)
{
UNITY_BEGIN();

RUN_TEST(test_add_filter);
RUN_TEST(test_subtract_filter);
RUN_TEST(test_multiply_filter);
RUN_TEST(test_voltage_divider_filter);
RUN_TEST(test_clamp_filter);
RUN_TEST(test_lambda_filter);
RUN_TEST(test_sliding_window_moving_average_filter);
RUN_TEST(test_exponential_moving_average_filter);
RUN_TEST(test_center_deadzone_filter);

return UNITY_END();
}

#ifdef ARDUINO

#include <Arduino.h>

void setup(void)
{
process();
}

void loop(void) {}

#else

int main(int argc, char** argv)
{
return process();
}

#endif
4 changes: 4 additions & 0 deletions test/test_opengloves_alpha_encoding/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,10 @@ void test_encode_input_peripherals(void)
const IEncoder* encoder = new AlphaEncoder();

const std::vector<std::tuple<InputPeripheralData, std::string>> cases = {
{
InputPeripheralData{},
"A0B0C0D0E0\n",
},
{
InputPeripheralData({
.curl = {
Expand Down

0 comments on commit 825e6f0

Please sign in to comment.