diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index fcdb3b1925f..483f17afb6a 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -14,14 +14,17 @@ if(MAVSDK_FOUND) autopilot_tester.cpp test_multicopter_offboard.cpp test_multicopter_mission.cpp + test_multicopter_failsafe.cpp test_vtol_mission.cpp ) target_link_libraries(mavsdk_tests MAVSDK::mavsdk MAVSDK::mavsdk_action + MAVSDK::mavsdk_failure MAVSDK::mavsdk_mission MAVSDK::mavsdk_offboard + MAVSDK::mavsdk_param MAVSDK::mavsdk_telemetry ) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index e032df2a1e5..fcd6d3a1aa4 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -101,10 +101,12 @@ void AutopilotTester::connect(const std::string uri) auto &system = _mavsdk.system(); - _telemetry.reset(new Telemetry(system)); _action.reset(new Action(system)); + _failure.reset(new Failure(system)); _mission.reset(new Mission(system)); _offboard.reset(new Offboard(system)); + _param.reset(new Param(system)); + _telemetry.reset(new Telemetry(system)); } void AutopilotTester::wait_until_ready() @@ -267,6 +269,36 @@ void AutopilotTester::execute_mission() REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); } +void AutopilotTester::execute_mission_and_lose_gps() +{ + 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::SensorGps, Failure::FailureType::Off) + == Failure::Result::Success); + } + }); + + _mission->start_mission_async([&prom](Mission::Result result) { + REQUIRE(Mission::Result::Success == result); + prom.set_value(); + }); + + REQUIRE(poll_condition_with_timeout( + [this]() { + auto flight_mode = _telemetry->flight_mode(); + return flight_mode == Telemetry::FlightMode::Land; + }, 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 0318852c44f..24c16544d9f 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -36,9 +36,11 @@ #include #include #include +#include #include #include #include +#include #include "catch2/catch.hpp" #include #include @@ -75,6 +77,7 @@ public: void prepare_square_mission(MissionOptions mission_options); void prepare_straight_mission(MissionOptions mission_options); void execute_mission(); + void execute_mission_and_lose_gps(); 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)); @@ -96,10 +99,12 @@ private: bool estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m); mavsdk::Mavsdk _mavsdk{}; - std::unique_ptr _telemetry{}; std::unique_ptr _action{}; + std::unique_ptr _failure{}; std::unique_ptr _mission{}; std::unique_ptr _offboard{}; + std::unique_ptr _param{}; + std::unique_ptr _telemetry{}; Telemetry::GroundTruth _home{NAN, NAN, NAN}; }; diff --git a/test/mavsdk_tests/test_multicopter_failsafe.cpp b/test/mavsdk_tests/test_multicopter_failsafe.cpp new file mode 100644 index 00000000000..db1f1fa6a38 --- /dev/null +++ b/test/mavsdk_tests/test_multicopter_failsafe.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 +#include +#include +#include +#include +#include "autopilot_tester.h" + + +TEST_CASE("Land on GPS lost during mission", "[multicopter]") +{ + 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_gps(); + tester.wait_until_disarmed(); +}