mavsdk_tests: ensure motor is stopped for motor failure test

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);
+		}
+	}
+}

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);

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();