Add reset() and configure() tests

nobleo · Mar 14, 2022 · b485259 · b485259
1 parent f63223e
commit b485259
Show file tree

Hide file tree

Showing 3 changed files with 94 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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);
+  }
+}