From 0a9378e0f613ae56152ef45ae4b1db4941ef63e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 20 Jan 2022 16:28:13 +0100 Subject: [PATCH] mavsdk_tests: ensure motor is stopped for motor failure test --- .../mavsdk_tests/autopilot_tester_failure.cpp | 20 +++++++++++++++++++ test/mavsdk_tests/autopilot_tester_failure.h | 3 +++ .../test_multicopter_control_allocation.cpp | 4 ++++ 3 files changed, 27 insertions(+) diff --git a/test/mavsdk_tests/autopilot_tester_failure.cpp b/test/mavsdk_tests/autopilot_tester_failure.cpp index 0737dea951..2bf406e506 100644 --- a/test/mavsdk_tests/autopilot_tester_failure.cpp +++ b/test/mavsdk_tests/autopilot_tester_failure.cpp @@ -78,3 +78,23 @@ void AutopilotTesterFailure::inject_failure(mavsdk::Failure::FailureUnit failure { CHECK(_failure->inject(failure_unit, failure_type, instance) == expected_result); } + +void AutopilotTesterFailure::enable_actuator_output_status() +{ + CHECK(getTelemetry()->set_rate_actuator_output_status(20.f) == Telemetry::Result::Success); +} + +void AutopilotTesterFailure::ensure_motor_stopped(unsigned index, unsigned num_motors) +{ + const Telemetry::ActuatorOutputStatus &status = getTelemetry()->actuator_output_status(); + CHECK(status.active >= num_motors); + + for (unsigned i = 0; i < num_motors; ++i) { + if (i == index) { + CHECK(status.actuator[i] <= 901.f); + + } else { // ensure all others are still running + CHECK(status.actuator[i] >= 999.f); + } + } +} diff --git a/test/mavsdk_tests/autopilot_tester_failure.h b/test/mavsdk_tests/autopilot_tester_failure.h index f0c4f3df6c..52124e6aaa 100644 --- a/test/mavsdk_tests/autopilot_tester_failure.h +++ b/test/mavsdk_tests/autopilot_tester_failure.h @@ -52,6 +52,9 @@ public: void set_param_ca_failure_mode(int value); void set_param_com_act_fail_act(int value); + void enable_actuator_output_status(); + void ensure_motor_stopped(unsigned index, unsigned num_motors); + void inject_failure(mavsdk::Failure::FailureUnit failure_unit, mavsdk::Failure::FailureType failure_type, int instance, mavsdk::Failure::Result expected_result); void connect(const std::string uri); diff --git a/test/mavsdk_tests/test_multicopter_control_allocation.cpp b/test/mavsdk_tests/test_multicopter_control_allocation.cpp index e23439536f..766a5200cc 100644 --- a/test/mavsdk_tests/test_multicopter_control_allocation.cpp +++ b/test/mavsdk_tests/test_multicopter_control_allocation.cpp @@ -61,6 +61,7 @@ TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]") tester.set_rtl_altitude(flight_altitude); tester.check_tracks_mission(5.f); tester.store_home(); + tester.enable_actuator_output_status(); std::this_thread::sleep_for(std::chrono::seconds( 1)); // This is necessary for the takeoff altitude to be applied properly @@ -74,12 +75,15 @@ TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]") // Motor failure mid-air tester.start_checking_altitude(altitude_tolerance); const int motor_instance = 1; + const unsigned num_motors = 6; // TODO: get from model tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance, mavsdk::Failure::Result::Success); std::this_thread::sleep_for(std::chrono::seconds(1)); + tester.ensure_motor_stopped(motor_instance - 1, num_motors); tester.execute_mission(); tester.stop_checking_altitude(); + tester.ensure_motor_stopped(motor_instance - 1, num_motors); // just to be sure // RTL tester.execute_rtl();