mavsdk_tests: add first GPS lost failsafe test

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
     )
 

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

test/mavsdk_tests/autopilot_tester.h

@@ -36,9 +36,11 @@
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/geometry.h>
 #include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/failure/failure.h>
 #include <mavsdk/plugins/mission/mission.h>
 #include <mavsdk/plugins/offboard/offboard.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
+#include <mavsdk/plugins/param/param.h>
 #include "catch2/catch.hpp"
 #include <chrono>
 #include <memory>
@@ -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<mavsdk::Telemetry> _telemetry{};
 	std::unique_ptr<mavsdk::Action> _action{};
+	std::unique_ptr<mavsdk::Failure> _failure{};
 	std::unique_ptr<mavsdk::Mission> _mission{};
 	std::unique_ptr<mavsdk::Offboard> _offboard{};
+	std::unique_ptr<mavsdk::Param> _param{};
+	std::unique_ptr<mavsdk::Telemetry> _telemetry{};
 
 	Telemetry::GroundTruth _home{NAN, NAN, NAN};
 };

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 <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+#include <iostream>
+#include <string>
+#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();
+}