mavsdk_tests: add test for stuck mag

test/mavsdk_tests/autopilot_tester.cpp

@@ -386,6 +386,35 @@ void AutopilotTester::execute_mission_and_get_baro_stuck()
 	}, std::chrono::seconds(60)));
 }
 
+void AutopilotTester::execute_mission_and_get_mag_stuck()
+{
+	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
+
+	_mission->subscribe_mission_progress([this](Mission::MissionProgress progress) {
+		std::cout << "Progress: " << progress.current << "/" << progress.total;
+
+		if (progress.current == 1) {
+			CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck)
+			      == Failure::Result::Success);
+		}
+	});
+
+	std::promise<void> prom;
+	auto fut = prom.get_future();
+	_mission->start_mission_async([&prom](Mission::Result result) {
+		REQUIRE(Mission::Result::Success == result);
+		prom.set_value();
+	});
+	REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready);
+
+	// We except the mission to continue with a stuck mag just fine.
+	REQUIRE(poll_condition_with_timeout(
+	[this]() {
+		auto progress = _mission->mission_progress();
+		return progress.current == progress.total;
+	}, std::chrono::seconds(60)));
+}
+
 CoordinateTransformation AutopilotTester::get_coordinate_transformation()
 {
 	const auto home = _telemetry->home();

test/mavsdk_tests/autopilot_tester.h

@@ -79,6 +79,7 @@ public:
 	void execute_mission();
 	void execute_mission_and_lose_gps();
 	void execute_mission_and_lose_mag();
+	void execute_mission_and_get_mag_stuck();
 	void execute_mission_and_lose_baro();
 	void execute_mission_and_get_baro_stuck();
 	void execute_rtl();

test/mavsdk_tests/test_multicopter_failsafe.cpp

@@ -67,6 +67,20 @@ TEST_CASE("Continue on mag lost during mission", "[multicopter][vtol]")
 	tester.wait_until_disarmed();
 }
 
+TEST_CASE("Continue on mag stuck during mission", "[multicopter][vtol]")
+{
+	AutopilotTester tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+
+	AutopilotTester::MissionOptions mission_options;
+	mission_options.rtl_at_end = true;
+	tester.prepare_square_mission(mission_options);
+	tester.arm();
+	tester.execute_mission_and_get_mag_stuck();
+	tester.wait_until_disarmed();
+}
+
 TEST_CASE("Continue on baro lost during mission", "[multicopter][vtol]")
 {
 	AutopilotTester tester;