From b485259ab4cc52e28797593aeef8c8756818650d Mon Sep 17 00:00:00 2001
From: Ramon Wijnands <ramon.wijnands@nobleo.nl>
Date: Mon, 7 Mar 2022 11:12:24 +0100
Subject: [PATCH] Add reset() and configure() tests

---
 test/unittests/test_derivative.cpp           | 11 ++++
 test/unittests/test_integral.cpp             | 29 +++++++++++
 test/unittests/test_second_order_lowpass.cpp | 54 ++++++++++++++++++++
 3 files changed, 94 insertions(+)

diff --git a/test/unittests/test_derivative.cpp b/test/unittests/test_derivative.cpp
index d98c7def..23f033e5 100644
--- a/test/unittests/test_derivative.cpp
+++ b/test/unittests/test_derivative.cpp
@@ -21,3 +21,14 @@ TEST(Derivative, StepResponse)
     EXPECT_NEAR(result, expected_response[i], eps);
   }
 }
+
+TEST(Derivative, Reset)
+{
+  double dt = 0.1;
+
+  Derivative filter;
+
+  EXPECT_NEAR(filter.filter(1, dt), 10, eps);
+  filter.reset();
+  EXPECT_NEAR(filter.filter(0, dt), 0, eps);
+}
diff --git a/test/unittests/test_integral.cpp b/test/unittests/test_integral.cpp
index 1510fcc7..1032cfc9 100644
--- a/test/unittests/test_integral.cpp
+++ b/test/unittests/test_integral.cpp
@@ -22,3 +22,32 @@ TEST(Integral, StepResponse)
     EXPECT_NEAR(result, expected_response[i], eps);
   }
 }
+
+TEST(Integral, Reset)
+{
+  double dt = 0.1;
+  double windup_limit = 0.5;
+
+  Integral filter{windup_limit};
+
+  EXPECT_NEAR(filter.filter(1, dt), 0.05, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.15, eps);
+  filter.reset();
+  EXPECT_NEAR(filter.filter(1, dt), 0.05, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.15, eps);
+}
+
+TEST(Integral, Configure)
+{
+  double dt = 0.1;
+  double windup_limit = 0.2;
+
+  Integral filter{windup_limit};
+
+  EXPECT_NEAR(filter.filter(1, dt), 0.05, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.15, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.20, eps);
+  filter.configure(0.35);
+  EXPECT_NEAR(filter.filter(1, dt), 0.30, eps);
+  EXPECT_NEAR(filter.filter(1, dt), 0.35, eps);
+}
diff --git a/test/unittests/test_second_order_lowpass.cpp b/test/unittests/test_second_order_lowpass.cpp
index 1489be0b..93d23ea8 100644
--- a/test/unittests/test_second_order_lowpass.cpp
+++ b/test/unittests/test_second_order_lowpass.cpp
@@ -77,3 +77,57 @@ TEST(SecondOrderLowpass, StepResponseDamping)
     EXPECT_NEAR(result, expected_response[i], eps);
   }
 }
+
+TEST(SecondOrderLowpass, Reset)
+{
+  double dt = 0.1;
+  double cutoff = 1 / dt / 4;
+  double damping = sqrt(2);
+
+  SecondOrderLowpass filter(cutoff, damping);
+
+  std::vector<double> expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488};
+  for (int i = 0; i < static_cast<int>(expected_response.size()); ++i) {
+    SCOPED_TRACE(i);
+
+    auto result = filter.filter(1, dt);
+    EXPECT_NEAR(result, expected_response[i], eps);
+  }
+
+  filter.reset();
+
+  for (int i = 0; i < static_cast<int>(expected_response.size()); ++i) {
+    SCOPED_TRACE(i);
+
+    auto result = filter.filter(1, dt);
+    EXPECT_NEAR(result, expected_response[i], eps);
+  }
+}
+
+TEST(SecondOrderLowpass, Configure)
+{
+  double dt = 0.1;
+  double cutoff = 1 / dt / 4;
+  double damping = sqrt(2);
+
+  SecondOrderLowpass filter(cutoff, damping);
+
+  std::vector<double> expected_response = {0.16071, 0.514214, 0.770813, 0.877725, 0.939488};
+  for (int i = 0; i < static_cast<int>(expected_response.size()); ++i) {
+    SCOPED_TRACE(i);
+
+    auto result = filter.filter(1, dt);
+    EXPECT_NEAR(result, expected_response[i], eps);
+  }
+
+  filter.configure(cutoff / 2, damping);
+
+  // no configure step response is {0.968659, 0.984211, 0.991911, 0.995898, 0.997907}
+  expected_response = {0.957154, 0.969162, 0.977792, 0.984006, 0.988481};
+  for (int i = 0; i < static_cast<int>(expected_response.size()); ++i) {
+    SCOPED_TRACE(i);
+
+    auto result = filter.filter(1, dt);
+    EXPECT_NEAR(result, expected_response[i], eps);
+  }
+}