From d44c406fb20b90f009c2f3166235dcf470c87e66 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 13 Dec 2017 14:54:49 +0100 Subject: [PATCH] ManualSmoothingZ: unittests --- src/systemcmds/tests/CMakeLists.txt | 1 + src/systemcmds/tests/test_smooth_z.cpp | 239 +++++++++++++++++++++++++ src/systemcmds/tests/tests_main.c | 1 + src/systemcmds/tests/tests_main.h | 1 + 4 files changed, 242 insertions(+) create mode 100644 src/systemcmds/tests/test_smooth_z.cpp diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 1d75912e31..b6f015a3f5 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -64,6 +64,7 @@ set(srcs test_uart_loopback.c test_uart_send.c test_versioning.cpp + test_smooth_z.cpp tests_main.c ) diff --git a/src/systemcmds/tests/test_smooth_z.cpp b/src/systemcmds/tests/test_smooth_z.cpp new file mode 100644 index 0000000000..ef276877b5 --- /dev/null +++ b/src/systemcmds/tests/test_smooth_z.cpp @@ -0,0 +1,239 @@ +#include +#include +#include + +class SmoothZTest : public UnitTest +{ +public: + virtual bool run_tests(); + + bool brakeUpward(); + bool brakeDownward(); + bool accelerateUpwardFromBrake(); + bool accelerateDownwardFromBrake(); + +}; + +bool SmoothZTest::run_tests() +{ + ut_run_test(brakeUpward); + ut_run_test(brakeDownward); + ut_run_test(accelerateUpwardFromBrake); + ut_run_test(accelerateDownwardFromBrake); + + return (_tests_failed == 0); +} + +bool SmoothZTest::brakeUpward() +{ + /* Downward flight and want to stop */ + float stick_current = 0.0f; // sticks are at zero position + float vel_sp_current = 0.0f; // desired velocity is at 0m/s + float vel_sp_previous = 5.0f; // the demanded previous setpoint was 5m/s downwards + float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint + float acc_max_up = 5.0f; + float acc_max_down = 2.0f; + + float vel_sp[] = {vel_sp_current, vel_sp_previous}; + ManualSmoothingZ smooth(vel, stick_current); + + /* overwrite parameters since they might change depending on configuration */ + smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss + smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss + smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1 + + float dt = 0.1f; // dt is set to 0.1s + + /* It should start with acceleration */ + ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); + + for (int i = 0; i < 100; i++) { + + smooth.smoothVelFromSticks(vel_sp, dt); + + /* Test if intention is brake */ + ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake); + + /* we should always use upward acceleration */ + ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON)); + + /* New setpoint has to be lower than previous setpoint (NED frame) or equal 0. 0 velocity + * occurs once the vehicle is at perfect rest. */ + ut_assert_true((vel_sp[0] < vel_sp[1]) || (fabsf(vel_sp[0]) < FLT_EPSILON)); + + + /* We reset the previou setpoint to newest setpoint + * and set the current setpoint to 0 because we still want to brake. + * We also set vel to previous setpoint where we make the assumption that + * the vehicle can perfectly track the setpoints. + */ + vel_sp[1] = vel_sp[0]; + vel_sp[0] = 0.0f; + vel = vel_sp[1]; + + } + + return true; +} + +bool SmoothZTest::brakeDownward() +{ + /* Downward flight and want to stop */ + float stick_current = 0.0f; // sticks are at zero position + float vel_sp_current = 0.0f; // desired velocity is 0m/s + float vel_sp_previous = -5.0f; // the demanded previous setpoint was -5m/s downwards + float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint + float acc_max_up = 5.0f; + float acc_max_down = 2.0f; + + float vel_sp[] = { vel_sp_current, vel_sp_previous }; + ManualSmoothingZ smooth(vel, stick_current); + + /* overwrite parameters since they might change depending on configuration */ + smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss + smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss + smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1 + + float dt = 0.1f; // dt is set to 0.1s + + /* It should start with acceleration */ + ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); + + for (int i = 0; i < 100; i++) { + + smooth.smoothVelFromSticks(vel_sp, dt); + + /* Test if intention is brake */ + ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake); + + /* New setpoint has to be larger than previous setpoint (NED frame) or equal 0. 0 velocity + * occurs once the vehicle is at perfect rest. */ + ut_assert_true((vel_sp[0] > vel_sp[1]) || (fabsf(vel_sp[0]) < FLT_EPSILON)); + + /* we should always use downward acceleration except when vehicle is at rest*/ + if (fabsf(vel_sp[0]) < FLT_EPSILON) { + ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON)); + + } else { + ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_down < FLT_EPSILON)); + } + + + /* We reset the previou setpoint to newest setpoint + * and set the current setpoint to 0 because we still want to brake. + * We also set vel to previous setpoint where we make the assumption that + * the vehicle can perfectly track the setpoints. + */ + vel_sp[1] = vel_sp[0]; + vel_sp[0] = 0.0f; + vel = vel_sp[1]; + + } + + return true; +} + +bool SmoothZTest::accelerateUpwardFromBrake() +{ + /* Downward flight and want to stop */ + float stick_current = -1.0f; // sticks are at full upward position + float vel_sp_current = -5.0f; // desired velocity is at -5m/s + float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards + float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint + float acc_max_up = 5.0f; + float acc_max_down = 2.0f; + + float vel_sp[] = {vel_sp_current, vel_sp_previous}; + ManualSmoothingZ smooth(vel, stick_current); + + /* overwrite parameters since they might change depending on configuration */ + smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss + smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss + smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1 + + float dt = 0.1f; // dt is set to 0.1s + + for (int i = 0; i < 100; i++) { + + smooth.smoothVelFromSticks(vel_sp, dt); + + /* Test if intention is acceleration */ + ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); + + /* we should always use upward acceleration */ + ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON)); + + /* New setpoint has to be larger than previous setpoint or equal to target velocity + * vel_sp_current. The negative sign is because of NED frame. + */ + ut_assert_true((-vel_sp[0] > -vel_sp[1]) || (fabsf(vel_sp[0] - vel_sp_current) < FLT_EPSILON)); + + + /* We reset the previous setpoint to newest setpoint and reset the current setpoint. + * We also set the current velocity to the previous setpoint with the assumption that + * the vehicle does perfect tracking. + */ + vel_sp[1] = vel_sp[0]; + vel_sp[0] = vel_sp_current; + vel = vel_sp[1]; + + } + + return true; +} + +bool SmoothZTest::accelerateDownwardFromBrake() +{ + /* Downward flight and want to stop */ + float stick_current = 1.0f; // sticks are at full downward position + float vel_sp_current = 5.0f; // desired velocity is at 5m/s + float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards + float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint + float acc_max_up = 5.0f; + float acc_max_down = 2.0f; + + float vel_sp[] = {vel_sp_current, vel_sp_previous}; + ManualSmoothingZ smooth(vel, stick_current); + + /* overwrite parameters since they might change depending on configuration */ + smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss + smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss + smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1 + + float dt = 0.1f; // dt is set to 0.1s + + for (int i = 0; i < 100; i++) { + + smooth.smoothVelFromSticks(vel_sp, dt); + + /* Test if intention is acceleration */ + ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); + + /* we should always use downward acceleration except when target velocity is reached */ + if (fabsf(vel_sp[0] - vel_sp_current) < FLT_EPSILON) { + ut_assert_true(smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON); + + } else { + ut_assert_true(smooth.getMaxAcceleration(vel_sp) - acc_max_down < FLT_EPSILON); + } + + /* New setpoint has to be larger than previous setpoint or equal to target velocity + * vel_sp_current (NED frame). + */ + ut_assert_true((vel_sp[0] > vel_sp[1]) || (fabsf(vel_sp[0] - vel_sp_current) < FLT_EPSILON)); + + + /* We reset the previous setpoint to newest setpoint and reset the current setpoint. + * We also set the current velocity to the previous setpoint with the assumption that + * the vehicle does perfect tracking. + */ + vel_sp[1] = vel_sp[0]; + vel_sp[0] = vel_sp_current; + vel = vel_sp[1]; + + } + + return true; +} + +ut_declare_test_c(test_smooth_z, SmoothZTest) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 27743f20fd..1a3128d7c9 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -127,6 +127,7 @@ const struct { {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST}, {"versioning", test_versioning, 0}, {"ctlmath", test_controlmath, 0}, + {"smoothz", test_smooth_z, 0}, {NULL, NULL, 0} }; diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 9be476eb18..7e7d9166d7 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -88,6 +88,7 @@ extern int test_uart_loopback(int argc, char *argv[]); extern int test_uart_send(int argc, char *argv[]); extern int test_parameters(int argc, char *argv[]); extern int test_versioning(int argc, char *argv[]); +extern int test_smooth_z(int argc, char *argv[]); /* external */ extern int commander_tests_main(int argc, char *argv[]);