Add SITL tests for control allocation

ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post

@@ -1,6 +1,4 @@
 
-mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix
-
 mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p
 
 # shellcheck disable=SC2154

test/mavsdk_tests/CMakeLists.txt

@@ -16,6 +16,9 @@ if(MAVSDK_FOUND)
     add_executable(mavsdk_tests
         test_main.cpp
         autopilot_tester.cpp
+        autopilot_tester_failure.cpp
+        test_multicopter_control_allocation.cpp
+        test_multicopter_failure_injection.cpp
         test_multicopter_failsafe.cpp
         test_multicopter_mission.cpp
         test_multicopter_offboard.cpp

test/mavsdk_tests/autopilot_tester.cpp

@@ -131,6 +131,14 @@ void AutopilotTester::set_takeoff_altitude(const float altitude_m)
 	CHECK(result.second == Approx(altitude_m));
 }
 
+void AutopilotTester::set_rtl_altitude(const float altitude_m)
+{
+	CHECK(Action::Result::Success == _action->set_return_to_launch_altitude(altitude_m));
+	const auto result = _action->get_return_to_launch_altitude();
+	CHECK(result.first == Action::Result::Success);
+	CHECK(result.second == Approx(altitude_m));
+}
+
 void AutopilotTester::set_height_source(AutopilotTester::HeightSource height_source)
 {
 	switch (height_source) {
@@ -200,6 +208,21 @@ void AutopilotTester::wait_until_hovering()
 	wait_for_landed_state(Telemetry::LandedState::InAir, std::chrono::seconds(30));
 }
 
+void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout)
+{
+	auto prom = std::promise<void> {};
+	auto fut = prom.get_future();
+
+	_telemetry->subscribe_position([&prom, rel_altitude_m, this](Telemetry::Position new_position) {
+		if (fabs(rel_altitude_m - new_position.relative_altitude_m) <= 0.5) {
+			_telemetry->subscribe_position(nullptr);
+			prom.set_value();
+		}
+	});
+
+	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
+}
+
 void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
 {
 	const auto ct = get_coordinate_transformation();
@@ -367,6 +390,11 @@ void AutopilotTester::execute_rtl()
 	REQUIRE(Action::Result::Success == _action->return_to_launch());
 }
 
+void AutopilotTester::execute_land()
+{
+	REQUIRE(Action::Result::Success == _action->land());
+}
+
 void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m,
 				    std::chrono::seconds timeout_duration)
 {
@@ -461,6 +489,22 @@ void AutopilotTester::fly_forward_in_altctl()
 	}
 }
 
+void AutopilotTester::start_checking_altitude(const float max_deviation_m)
+{
+	std::array<float, 3> initial_position = get_current_position_ned();
+	float target_altitude = initial_position[2];
+
+	_telemetry->subscribe_position([target_altitude, max_deviation_m, this](Telemetry::Position new_position) {
+		const float current_deviation = fabs((-target_altitude) - new_position.relative_altitude_m);
+		CHECK(current_deviation <= max_deviation_m);
+	});
+}
+
+void AutopilotTester::stop_checking_altitude()
+{
+	_telemetry->subscribe_position(nullptr);
+}
+
 void AutopilotTester::check_tracks_mission(float corridor_radius_m)
 {
 	auto mission = _mission->download_mission();
@@ -490,6 +534,12 @@ void AutopilotTester::check_tracks_mission(float corridor_radius_m)
 	});
 }
 
+std::array<float, 3> AutopilotTester::get_current_position_ned()
+{
+	mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
+	std::array<float, 3> position_ned{position_velocity_ned.position.north_m, position_velocity_ned.position.east_m, position_velocity_ned.position.down_m};
+	return position_ned;
+}
 
 void AutopilotTester::offboard_land()
 {
@@ -640,6 +690,27 @@ void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state,
 	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
 }
 
+void AutopilotTester::wait_until_speed_lower_than(float speed, std::chrono::seconds timeout)
+{
+	auto prom = std::promise<void> {};
+	auto fut = prom.get_future();
+
+	_telemetry->subscribe_position_velocity_ned([&prom, speed, this](Telemetry::PositionVelocityNed position_velocity_ned) {
+		std::array<float, 3> current_velocity;
+		current_velocity[0] = position_velocity_ned.velocity.north_m_s;
+		current_velocity[1] = position_velocity_ned.velocity.east_m_s;
+		current_velocity[2] = position_velocity_ned.velocity.down_m_s;
+		const float current_speed = norm(current_velocity);
+
+		if (current_speed <= speed) {
+			_telemetry->subscribe_position_velocity_ned(nullptr);
+			prom.set_value();
+		}
+	});
+
+	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
+}
+
 void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout)
 {
 	auto prom = std::promise<void> {};

test/mavsdk_tests/autopilot_tester.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -101,6 +101,7 @@ public:
 	void check_home_within(float acceptance_radius_m);
 	void check_home_not_within(float min_distance_m);
 	void set_takeoff_altitude(const float altitude_m);
+	void set_rtl_altitude(const float altitude_m);
 	void set_height_source(HeightSource height_source);
 	void set_rc_loss_exception(RcLossException mask);
 	void arm();
@@ -109,7 +110,9 @@ public:
 	void transition_to_fixedwing();
 	void transition_to_multicopter();
 	void wait_until_disarmed(std::chrono::seconds timeout_duration = std::chrono::seconds(60));
-	void wait_until_hovering();
+	void wait_until_hovering(); // TODO: name suggests, that function waits for drone velocity to be zero and not just drone in the air
+	void wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout);
+	void wait_until_speed_lower_than(float speed, std::chrono::seconds timeout);
 	void prepare_square_mission(MissionOptions mission_options);
 	void prepare_straight_mission(MissionOptions mission_options);
 	void execute_mission();
@@ -121,6 +124,7 @@ public:
 	void load_qgc_mission_raw_and_move_here(const std::string &plan_file);
 	void execute_mission_raw();
 	void execute_rtl();
+	void execute_land();
 	void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f,
 			   std::chrono::seconds timeout_duration = std::chrono::seconds(60));
 	void offboard_land();
@@ -129,7 +133,16 @@ public:
 	void request_ground_truth();
 	void check_mission_item_speed_above(int item_index, float min_speed_m_s);
 	void check_tracks_mission(float corridor_radius_m = 1.5f);
+	void start_checking_altitude(const float max_deviation_m);
+	void stop_checking_altitude();
 
+	// Blocking call to get the drone's current position in NED frame
+	std::array<float, 3> get_current_position_ned();
+
+protected:
+	mavsdk::Param *getParams() const { return _param.get();}
+	mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
+	std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
 
 private:
 	mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();

test/mavsdk_tests/autopilot_tester_failure.cpp

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "autopilot_tester_failure.h"
+
+#include "math_helpers.h"
+#include <iostream>
+#include <future>
+#include <thread>
+#include <unistd.h>
+
+
+void AutopilotTesterFailure::connect(const std::string uri)
+{
+	AutopilotTester::connect(uri);
+
+	auto system = get_system();
+	_failure.reset(new Failure(system));
+}
+
+void AutopilotTesterFailure::set_param_sys_failure_en(bool value)
+{
+	CHECK(getParams()->set_param_int("SYS_FAILURE_EN", value) == Param::Result::Success);
+}
+
+void AutopilotTesterFailure::set_param_fd_act_en(bool value)
+{
+	CHECK(getParams()->set_param_int("FD_ACT_EN", value) == Param::Result::Success);
+}
+
+void AutopilotTesterFailure::set_param_mc_airmode(int value)
+{
+	CHECK(getParams()->set_param_int("MC_AIRMODE", value) == Param::Result::Success);
+}
+
+void AutopilotTesterFailure::set_param_ca_failure_mode(int value)
+{
+	CHECK(getParams()->set_param_int("CA_FAILURE_MODE", value) == Param::Result::Success);
+}
+
+void AutopilotTesterFailure::set_param_com_act_fail_act(int value)
+{
+	CHECK(getParams()->set_param_int("COM_ACT_FAIL_ACT", value) == Param::Result::Success);
+}
+
+void AutopilotTesterFailure::inject_failure(mavsdk::Failure::FailureUnit failure_unit,
+		mavsdk::Failure::FailureType failure_type, int instance, mavsdk::Failure::Result expected_result)
+{
+	CHECK(_failure->inject(failure_unit, failure_type, instance) == expected_result);
+}

test/mavsdk_tests/autopilot_tester_failure.h

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "autopilot_tester.h"
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/failure/failure.h>
+
+
+class AutopilotTesterFailure : public AutopilotTester
+{
+public:
+	AutopilotTesterFailure() = default;
+	~AutopilotTesterFailure() = default;
+
+	void set_param_sys_failure_en(bool value);
+	void set_param_fd_act_en(bool value);
+	void set_param_mc_airmode(int value);
+	void set_param_ca_failure_mode(int value);
+	void set_param_com_act_fail_act(int value);
+
+	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);
+
+private:
+	std::unique_ptr<mavsdk::Failure> _failure{};
+};

test/mavsdk_tests/configs/sitl.json

@@ -21,6 +21,12 @@
             "vehicle": "tailsitter",
             "test_filter": "[vtol]",
             "timeout_min": 10
+        },
+        {
+            "model": "typhoon_h480_ctrlalloc",
+            "vehicle": "typhoon_h480_ctrlalloc",
+            "test_filter": "[controlallocation]",
+            "timeout_min": 10
         }
     ]
 }

test/mavsdk_tests/test_multicopter_control_allocation.cpp

@@ -0,0 +1,300 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <thread>
+#include <chrono>
+#include <math.h>
+
+#include "autopilot_tester_failure.h"
+
+TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]")
+{
+	const float flight_altitude = 10.0f;
+	const float altitude_tolerance = 4.0f;
+	const float hover_speed_tolerance = 1.0f;
+
+	AutopilotTester::MissionOptions mission_options;
+	mission_options.rtl_at_end = false;
+	mission_options.relative_altitude_m = flight_altitude;
+
+	AutopilotTesterFailure tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+
+	// Configuration
+	tester.set_param_sys_failure_en(true);  // Enable failure injection
+	tester.set_param_fd_act_en(true);	// Enable motor failure detection
+	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
+	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
+	tester.prepare_square_mission(mission_options);
+	tester.set_takeoff_altitude(flight_altitude);
+	tester.set_rtl_altitude(flight_altitude);
+	tester.check_tracks_mission(5.f);
+	tester.store_home();
+	std::this_thread::sleep_for(std::chrono::seconds(
+					    1));  // This is necessary for the takeoff altitude to be applied properly
+
+	// Takeoff
+	tester.arm();
+	tester.takeoff();
+	tester.wait_until_hovering();
+	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30));
+	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30));
+
+	// Motor failure mid-air
+	tester.start_checking_altitude(altitude_tolerance);
+	const int motor_instance = 1;
+	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.execute_mission();
+	tester.stop_checking_altitude();
+
+	// RTL
+	tester.execute_rtl();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+	tester.check_home_within(5.f);
+}
+
+TEST_CASE("Control Allocation - Remove two motors", "[controlallocation]")
+{
+	const float flight_altitude = 10.0f;
+	const float altitude_tolerance = 4.0f;
+	const float hover_speed_tolerance = 1.0f;
+
+	AutopilotTesterFailure tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.set_param_sys_failure_en(true);  // Enable failure injection
+	tester.set_param_fd_act_en(true);	// Enable motor failure detection
+	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
+	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
+
+	AutopilotTester::MissionOptions mission_options;
+	mission_options.rtl_at_end = false;
+	mission_options.relative_altitude_m = flight_altitude;
+
+	tester.prepare_square_mission(mission_options);
+	tester.set_takeoff_altitude(flight_altitude);
+	tester.set_rtl_altitude(flight_altitude);
+	tester.check_tracks_mission(5.f);
+	tester.store_home();
+	std::this_thread::sleep_for(std::chrono::seconds(
+					    1));  // This is necessary for the takeoff altitude to be applied properly
+
+	tester.arm();
+	tester.takeoff();
+	tester.wait_until_hovering();
+	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30));
+	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30));
+
+	// Remove two motors opposite of one another on the hexa airframe
+	const int first_motor_instance = 1;
+	const int second_motor_instance = 2;
+	tester.start_checking_altitude(altitude_tolerance);
+	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off,
+			      first_motor_instance,
+			      mavsdk::Failure::Result::Success);
+	std::this_thread::sleep_for(std::chrono::seconds(1));
+	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off,
+			      second_motor_instance,
+			      mavsdk::Failure::Result::Success);
+	std::this_thread::sleep_for(std::chrono::seconds(1));
+
+	tester.execute_mission();
+	tester.stop_checking_altitude();
+
+	// RTL with two motors out won't work because navigator will wait forever until
+	// the yaw setpoint is reached during RTL, and it won't land.
+	tester.land();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+}
+
+TEST_CASE("Control Allocation - Remove and restore every motor once", "[controlallocation]")
+{
+	const float flight_altitude = 10.0f;
+	const float altitude_tolerance = 4.0f;
+	const float hover_speed_tolerance = 1.0f;
+
+	AutopilotTesterFailure tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.set_param_sys_failure_en(true);  // Enable failure injection
+	tester.set_param_fd_act_en(true);	// Enable motor failure detection
+	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
+	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
+
+	AutopilotTester::MissionOptions mission_options;
+	mission_options.rtl_at_end = false;
+	mission_options.relative_altitude_m = flight_altitude;
+
+	tester.prepare_square_mission(mission_options);
+	tester.set_takeoff_altitude(flight_altitude);
+	tester.set_rtl_altitude(flight_altitude);
+	tester.check_tracks_mission(5.f);
+	tester.store_home();
+	std::this_thread::sleep_for(std::chrono::seconds(
+					    1));  // This is necessary for the takeoff altitude to be applied properly
+
+	tester.arm();
+	tester.takeoff();
+	tester.wait_until_hovering();
+	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30));
+	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30));
+
+	tester.start_checking_altitude(altitude_tolerance);
+
+	for (int m = 1; m <= 6; m++) {
+		tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, m,
+				      mavsdk::Failure::Result::Success);
+		std::this_thread::sleep_for(std::chrono::seconds(1));
+		tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Ok, m,
+				      mavsdk::Failure::Result::Success);
+		std::this_thread::sleep_for(std::chrono::seconds(1));
+	}
+
+	tester.execute_mission();
+	tester.stop_checking_altitude();
+
+	tester.execute_rtl();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+	tester.check_home_within(5.f);
+}
+
+TEST_CASE("Control Allocation - Return home on motor failure", "[controlallocation]")
+{
+	const float flight_altitude = 10.0f;
+	const float hover_speed_tolerance = 1.0f;
+
+	AutopilotTesterFailure tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+
+	// Configuration
+	tester.set_param_sys_failure_en(true);  // Enable failure injection
+	tester.set_param_fd_act_en(true);	// Enable motor failure detection
+	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
+	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
+	tester.set_param_com_act_fail_act(3);	// RTL on motor failure
+	tester.set_takeoff_altitude(flight_altitude);
+	tester.store_home();
+	std::this_thread::sleep_for(std::chrono::seconds(
+					    1));  // This is necessary for the takeoff altitude to be applied properly
+
+	// Takeoff
+	tester.arm();
+	tester.takeoff();
+	tester.wait_until_hovering();
+	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30));
+	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30));
+
+	// TODO: Minor improvement, fly forward for a little bit before triggering motor failure to distinguish "RTL" and "Land only"
+
+	// Motor failure mid-air
+	const int motor_instance = 1;
+	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));
+
+	// Wait for RTL to trigger automatically
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+	tester.check_home_within(5.f);
+}
+
+TEST_CASE("Control Allocation - Terminate on motor failure", "[controlallocation]")
+{
+	const float flight_altitude = 100.0f;
+	const float hover_speed_tolerance = 1.0f;
+
+	AutopilotTesterFailure tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+
+	// Configuration
+	tester.set_param_sys_failure_en(true);  // Enable failure injection
+	tester.set_param_fd_act_en(true);	// Enable motor failure detection
+	tester.set_param_mc_airmode(1);		// Enable airmode for control allocation with motor failure
+	tester.set_param_ca_failure_mode(1);	// Enable control allocation handling of failed motor
+	tester.set_param_com_act_fail_act(4);	// Terminate on motor failure
+	tester.set_takeoff_altitude(flight_altitude);
+	std::this_thread::sleep_for(std::chrono::seconds(
+					    1));  // This is necessary for the takeoff altitude to be applied properly
+
+	// Takeoff
+	tester.arm();
+	tester.takeoff();
+	tester.wait_until_altitude(flight_altitude, std::chrono::seconds(60));
+	tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(60));
+
+	// Motor failure mid-air
+	const int motor_instance = 1;
+	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));
+
+	// Wait for disarm with a low enough timeout such that it's necessary for the
+	// drone to freefall (terminate) in order to disarm quickly enough:
+	// h = g/2 * t^2 -> solve for t
+	const int seconds_to_touchdown = 2 + sqrt(flight_altitude * 2 / 10.0);
+	std::cout << "seconds_to_touchdown: " << seconds_to_touchdown << std::endl;
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(seconds_to_touchdown);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+}
+
+#if 0
+// This is for checking that the SITL test is actually capable of detecting the drone crash
+// when not reallocating the control allocation on a motor failure
+TEST_CASE("Control Allocation - Remove two motors and expect crash", "[controlallocation]")
+{
+	// TODO
+}
+#endif
+
+#if 0
+TEST_CASE("Control Allocation with multiple sequential motor failures", "[controlallocation]")
+{
+	// TODO
+}
+#endif
+
+#if 0
+TEST_CASE("Control Allocation with multiple simultaneous motor failures", "[controlallocation]")
+{
+	// TODO
+}
+#endif

test/mavsdk_tests/test_multicopter_failure_injection.cpp

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <thread>
+#include <chrono>
+
+#include "autopilot_tester_failure.h"
+
+TEST_CASE("Failure Injection - Reject mid-air when it is disabled", "[multicopter]")
+{
+	AutopilotTesterFailure tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.arm();
+	tester.takeoff();
+	tester.wait_until_hovering();
+	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, 1,
+			      mavsdk::Failure::Result::Disabled);
+	tester.execute_rtl();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+}
+
+TEST_CASE("Failure Injection - Reject before arming", "[multicopter]")
+{
+	AutopilotTesterFailure tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, 1,
+			      mavsdk::Failure::Result::Disabled);
+}