From 5fbee9fce96e943ce84d28cd9f53513b9b224ec9 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 20 Dec 2017 17:32:37 +0100 Subject: [PATCH] ControlMath tests --- .../mc_pos_control_tests/CMakeLists.txt | 2 + .../mc_pos_control_tests/test_controlmath.cpp | 215 ++++++++++++++++++ src/systemcmds/tests/tests_main.c | 1 + src/systemcmds/tests/tests_main.h | 2 +- 4 files changed, 219 insertions(+), 1 deletion(-) create mode 100644 src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp diff --git a/src/modules/mc_pos_control/mc_pos_control_tests/CMakeLists.txt b/src/modules/mc_pos_control/mc_pos_control_tests/CMakeLists.txt index 0a0a80f60b..a82bdb18f1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_tests/CMakeLists.txt +++ b/src/modules/mc_pos_control/mc_pos_control_tests/CMakeLists.txt @@ -36,6 +36,8 @@ px4_add_module( SRCS mc_pos_control_tests.cpp ../mc_pos_control_main.cpp + test_controlmath.cpp + ../Utility/ControlMath.cpp DEPENDS platforms__common ) diff --git a/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp b/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp new file mode 100644 index 0000000000..6b78f7c363 --- /dev/null +++ b/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp @@ -0,0 +1,215 @@ +#include +#include "../Utility/ControlMath.hpp" +#include + +static const float EPS = 0.00000001f; + +class ControlMathTest : public UnitTest +{ +public: + virtual bool run_tests(); + +private: + bool testConstrainTilt(); + bool testConstrainPIDu(); + + +}; + +bool ControlMathTest::run_tests() +{ + ut_run_test(testConstrainTilt); + ut_run_test(testConstrainPIDu); + + return (_tests_failed == 0); +} + +bool ControlMathTest::testConstrainTilt() +{ + // expected: return same vector + // reason: tilt exceeds maximum tilt + matrix::Vector3f v(0.5f, 0.5f, 0.1f); + float tilt_max = math::radians(91.0f); + matrix::Vector3f vr = PosControl::constrainTilt(v, tilt_max); + ut_assert_true((v - vr).length() < EPS); + + // expected: return zero vector + // reason: v points down, but cone generated by tilt is only + // defined in negative z (upward). + v = matrix::Vector3f(1.0f, 1.0f, 0.1f); + tilt_max = math::radians(45.0f); + vr = PosControl::constrainTilt(v, tilt_max); + ut_assert_true((vr).length() < EPS); + + // expected: length vr_xy same as vr_z + // reason: it is a 45 cone and v_xy exceeds v_z + v = matrix::Vector3f(1.0f, 1.0f, -0.5f); + tilt_max = math::radians(45.0f); + vr = PosControl::constrainTilt(v, tilt_max); + float vr_xy = matrix::Vector2f(vr(0), vr(1)).length(); + ut_assert_true(fabsf(vr(2)) - vr_xy < EPS); + + // expected: length vr_z larger than vr_xy + // reason: it is a 30 cone and v_xy exceeds v_z + v = matrix::Vector3f(1.0f, 1.0f, -0.5f); + tilt_max = math::radians(20.0f); + vr = PosControl::constrainTilt(v, tilt_max); + vr_xy = matrix::Vector2f(vr(0), vr(1)).length(); + ut_assert_true(fabsf(vr(2)) - vr_xy > EPS); + + // expected: length of vr_xy larger than vr_z + // reason: it is a 80 cone and v_xy exceeds v_z + v = matrix::Vector3f(10.0f, 10.0f, -0.5f); + tilt_max = math::radians(80.f); + vr = PosControl::constrainTilt(v, tilt_max); + vr_xy = matrix::Vector2f(vr(0), vr(1)).length(); + ut_assert_true(fabsf(vr(2)) - vr_xy < EPS); + + // expected: same vector is return + // reson: vector is within cond + v = matrix::Vector3f(1.0f, 1.0f, -0.5f); + tilt_max = math::radians(89.f); + vr = PosControl::constrainTilt(v, tilt_max); + ut_assert_true((v - vr).length() < EPS); + + return true; + +} + +bool ControlMathTest::testConstrainPIDu() +{ + /* Notation: + * u: input thrust that gets modified + * u_o: unmodified thrust input + * sat: saturation flags + * Ulim: max and min thrust + * d: flags for xy and z, indicating sign of (r-y) + * r: reference; not used here + * y: measurement; not used here + */ + + // expected: same u + // reason: no direction change and within bounds + bool sat[2] = {false, false}; + float Ulim[2] = {0.8f, 0.2f}; + matrix::Vector3f u{0.1f, 0.1f, -0.4f}; + matrix::Vector3f u_o = u; + float d[2] = {1.0f, 1.0f}; + PosControl::constrainPIDu(u, sat, Ulim, d); + ut_assert_true((u - u_o).length() < EPS); + ut_assert_false(sat[1]); + ut_assert_false(sat[0]); + + // expected: u_xy smaller than u_o_xy and sat[0] = true + // reason: u_o_xy exceeds Ulim[0] and d[0] is positive + sat[0] = false; + sat[1] = false; + Ulim[0] = 0.5f; + Ulim[1] = 0.2f; + u = matrix::Vector3f(0.4f, 0.4f, -0.1f); + u_o = u; + d[0] = 1.0f; + d[1] = 1.0f; + PosControl::constrainPIDu(u, sat, Ulim, d); + float u_xy = matrix::Vector2f(u(0), u(1)).length(); + float u_o_xy = matrix::Vector2f(u_o(0), u_o(1)).length(); + ut_assert_true(u_xy < u_o_xy); + ut_assert_true(fabsf(u(2)) - fabsf(u_o(2)) < EPS); + ut_assert_true(sat[0]); + ut_assert_false(sat[1]); + + // expected: u_xy smaller than u_o_xy and sat[0] = false + // reason: u_o_xy exceeds Ulim[0] and d[0] is negative + d[0] = -1.0f; + ut_assert_true(u_xy < u_o_xy); + ut_assert_true(fabsf(u(2)) - fabsf(u_o(2)) < EPS); + ut_assert_true(sat[0]); + ut_assert_false(sat[1]); + + // expected: u_xy = 0 and sat[0] = true + // expected: u_z = -0.6 (maximum) and sat[1] = true + // reason: u_o_z exceeds maximum and since altitude + // has higher priority, u_xy will be set to 0. No direction + // change desired. + sat[0] = false; + sat[1] = false; + Ulim[0] = 0.5f; + Ulim[1] = 0.2f; + u = matrix::Vector3f(0.4f, 0.4f, -0.6f); + u_o = u; + d[0] = 1.0f; + d[1] = 1.0f; + PosControl::constrainPIDu(u, sat, Ulim, d); + u_xy = matrix::Vector2f(u(0), u(1)).length(); + u_o_xy = matrix::Vector2f(u_o(0), u_o(1)).length(); + ut_assert_true(u_xy < u_o_xy); + ut_assert_true(u_o(2) - (-0.6f) < EPS); + ut_assert_true(u_xy < EPS); + ut_assert_true(sat[0]); + ut_assert_true(sat[1]); + + // expected: u_xy = 0 and sat[0] = true because u_z is saturate + // => altitude priority + // expected: u_z = -0.6 (maximum) and sat[1] = true + // reason: u_o_z exceeds maximum and since altitude + // has higher priority, u_xy will be set to 0. Direction + // change desired for xy + d[0] = -1.0f; + PosControl::constrainPIDu(u, sat, Ulim, d); + u_xy = matrix::Vector2f(u(0), u(1)).length(); + u_o_xy = matrix::Vector2f(u_o(0), u_o(1)).length(); + ut_assert_true(u_xy < u_o_xy); + ut_assert_true(u_o(2) - (-0.6f) < EPS); + ut_assert_true(u_xy < EPS); + ut_assert_true(sat[0]); + ut_assert_true(sat[1]); + + // expected: nothing + // reason: thottle within bounds + sat[0] = false; + sat[1] = false; + Ulim[0] = 0.7f; + Ulim[1] = 0.2f; + u = matrix::Vector3f(0.3f, 0.3f, 0.0f); + u_o = u; + d[0] = 1.0f; + d[1] = 1.0f; + PosControl::constrainPIDu(u, sat, Ulim, d); + ut_assert_true((u - u_o).length() < EPS); + ut_assert_false(sat[1]); + ut_assert_false(sat[0]); + + // expected: u_xy at minimum, no saturation + // reason: u_o is below minimum with u_o_z = 0, which + // means that Ulim[1] is in xy direction. + // No saturation because no direction change. + sat[0] = false; + sat[1] = false; + Ulim[0] = 0.7f; + Ulim[1] = 0.2f; + u = matrix::Vector3f(0.05f, 0.05f, 0.0f); + u_o = u; + d[0] = 1.0f; + d[1] = 1.0f; + PosControl::constrainPIDu(u, sat, Ulim, d); + u_xy = matrix::Vector2f(u(0), u(1)).length(); + ut_assert_true(u_xy - Ulim[1] < EPS); + ut_assert_true(fabsf(u(2)) < EPS); + ut_assert_false(sat[1]); + ut_assert_false(sat[0]); + + // expected: u_xy at minimum, saturation in z + // reason: u_o is below minimum with u_o_z = 0, which + // means that Ulim[1] is in xy direction. + // Direction change in z. + d[1] = -1.0f; + PosControl::constrainPIDu(u, sat, Ulim, d); + ut_assert_true(u_xy - Ulim[1] < EPS); + ut_assert_true(fabsf(u(2)) < EPS); + ut_assert_true(sat[1]); + ut_assert_false(sat[0]); + + return true; +} + +ut_declare_test_c(test_controlmath, ControlMathTest) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 7ffca840a5..27743f20fd 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -126,6 +126,7 @@ const struct { {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST}, {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST}, {"versioning", test_versioning, 0}, + {"ctlmath", test_controlmath, 0}, {NULL, NULL, 0} }; diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 44b4ecff8a..9be476eb18 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -97,7 +97,7 @@ extern int uorb_tests_main(int argc, char *argv[]); extern int rc_tests_main(int argc, char *argv[]); extern int sf0x_tests_main(int argc, char *argv[]); extern int mc_pos_control_tests_main(int argc, char *argv[]); - +extern int test_controlmath(int argc, char *argv[]); __END_DECLS