From d77d258442c3e725297a0671df27d61a1e036665 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 20 May 2021 11:22:05 +1000
Subject: [PATCH] AP_Math: add tests for the sqrt_controller

---
 .../AP_Math/tests/test_sqrt_controller.cpp    | 64 +++++++++++++++++++
 1 file changed, 64 insertions(+)
 create mode 100644 libraries/AP_Math/tests/test_sqrt_controller.cpp

diff --git a/libraries/AP_Math/tests/test_sqrt_controller.cpp b/libraries/AP_Math/tests/test_sqrt_controller.cpp
new file mode 100644
index 0000000000..3ea7850bd6
--- /dev/null
+++ b/libraries/AP_Math/tests/test_sqrt_controller.cpp
@@ -0,0 +1,64 @@
+#include <AP_gtest.h>
+
+#include <AP_Math/AP_Math.h>  // FIXME: control.h has include issues...
+
+#include <AP_Math/control.h>
+
+#define EXPECT_VECTOR2F_EQ(v1, v2)              \
+    do {                                        \
+        EXPECT_FLOAT_EQ(v1[0], v2[0]);          \
+        EXPECT_FLOAT_EQ(v1[1], v2[1]);          \
+    } while (false);
+
+TEST(control, SqrtController2f)
+{
+    // a zero-length error gets a zero-length response:
+    Vector2f zero;
+    EXPECT_VECTOR2F_EQ(zero, sqrt_controller(zero, 17, 18, 0.01));
+}
+
+TEST(control, InvSqrtControllerIsActuallyInverse)
+{
+    // test input - these are the parameters and expected response for
+    // a call to sqrt_controller.
+    const struct InvSqrtTest {
+        float error;
+        float p;
+        float second_order_limit;
+        float dt;
+        float expected_sqrt_controller_output;
+    } tests[] {
+        { 5, 0.1, 1, 0.2, 0.5 },  // simple random number test case
+        { -5, 0.1, 1, 0.2, -0.5 },  // negative error should have negavtive response
+        { 5, 0.2, 1, 0.2, 1 },  // twice-the-P
+        // { 5, 0.1, 1, 0.4, 1 },  // twice-the-dt
+        { 0, 0.1, 1, 0.2, 0 },  // zero-error means zero response
+        { 0, 0, 0, 0, 0 },  // slightly corner-case-ish....
+    };
+
+    // iterate through all tests, calling sqrt_controller, and
+    // checking its output.  Take its output and some of the test
+    // parameters and ensure that the inverse sqrt controller gives us
+    // back the original error.
+    for (auto &test : tests) {
+        const float sqrt_controller_output = sqrt_controller(
+            test.error,
+            test.p,
+            test.second_order_limit,
+            test.dt
+        );
+        // ensure sqrt controller out put is what we expect
+        EXPECT_FLOAT_EQ(sqrt_controller_output, test.expected_sqrt_controller_output);
+        // ensure we can invert it back to the initial error
+        EXPECT_FLOAT_EQ(test.error, inv_sqrt_controller(
+                            sqrt_controller_output,
+                            test.p,
+                            test.second_order_limit
+                            )
+        );
+    }
+}
+
+int hal;
+
+AP_GTEST_MAIN()