diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index fcd6d3a1aa..7cdf88e0d7 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -290,6 +290,7 @@ void AutopilotTester::execute_mission_and_lose_gps() prom.set_value(); }); + // We expect that a blind land is performed. REQUIRE(poll_condition_with_timeout( [this]() { auto flight_mode = _telemetry->flight_mode(); @@ -299,6 +300,37 @@ void AutopilotTester::execute_mission_and_lose_gps() REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); } +void AutopilotTester::execute_mission_and_lose_mag() +{ + CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); + + std::promise prom; + auto fut = prom.get_future(); + + _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::Off) + == Failure::Result::Success); + } + }); + + _mission->start_mission_async([&prom](Mission::Result result) { + REQUIRE(Mission::Result::Success == result); + prom.set_value(); + }); + + // We except the mission to continue without mag just fine. + REQUIRE(poll_condition_with_timeout( + [this]() { + auto progress = _mission->mission_progress(); + return progress.current == progress.total; + }, std::chrono::seconds(60))); + + REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); +} + CoordinateTransformation AutopilotTester::get_coordinate_transformation() { const auto home = _telemetry->home(); diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 24c16544d9..1944a038f5 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -78,6 +78,7 @@ public: void prepare_straight_mission(MissionOptions mission_options); void execute_mission(); void execute_mission_and_lose_gps(); + void execute_mission_and_lose_mag(); void execute_rtl(); void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f, std::chrono::seconds timeout_duration = std::chrono::seconds(60)); diff --git a/test/mavsdk_tests/test_multicopter_failsafe.cpp b/test/mavsdk_tests/test_multicopter_failsafe.cpp index ad4a3c3e60..0850ee1247 100644 --- a/test/mavsdk_tests/test_multicopter_failsafe.cpp +++ b/test/mavsdk_tests/test_multicopter_failsafe.cpp @@ -52,3 +52,17 @@ TEST_CASE("Land on GPS lost during mission", "[multicopter][vtol]") tester.execute_mission_and_lose_gps(); tester.wait_until_disarmed(); } + +TEST_CASE("Continue on mag lost 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_lose_mag(); + tester.wait_until_disarmed(); +}