[RTL]: Add CI testing for different RTL modes.

added a vtol mission without a landing at the end (used for reversed RTL) Note: reverse mission tracking check is disabled, due to potential bug in mavsdk Signed-off-by: RomanBapst <[email protected]>
PX4 · Feb 1, 2023 · 4020dd0 · 4020dd0
1 parent 4cf0d2e
commit 4020dd0
Show file tree

Hide file tree

Showing 8 changed files with 437 additions and 10 deletions.
diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt
@@ -17,6 +17,7 @@ if(MAVSDK_FOUND)
         test_main.cpp
         autopilot_tester.cpp
         autopilot_tester_failure.cpp
+        autopilot_tester_rtl.cpp
         # follow-me needs a MAVSDK update:
         # https://github.com/mavlink/MAVSDK/pull/1770
         # autopilot_tester_follow_me.cpp

diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
@@ -268,7 +268,7 @@ void AutopilotTester::execute_mission_and_lose_gps()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorGps, Failure::FailureType::Off, 0) == Failure::Result::Success);
 
@@ -280,7 +280,7 @@ void AutopilotTester::execute_mission_and_lose_mag()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Off, 0) == Failure::Result::Success);
 
@@ -296,7 +296,7 @@ void AutopilotTester::execute_mission_and_lose_baro()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Off, 0) == Failure::Result::Success);
 
@@ -312,7 +312,7 @@ void AutopilotTester::execute_mission_and_get_baro_stuck()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck, 0) == Failure::Result::Success);
 
@@ -328,7 +328,7 @@ void AutopilotTester::execute_mission_and_get_mag_stuck()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck, 0) == Failure::Result::Success);
 
@@ -501,6 +501,49 @@ void AutopilotTester::stop_checking_altitude()
 	_telemetry->subscribe_position(nullptr);
 }
 
+void AutopilotTester::check_tracks_mission_raw(float corridor_radius_m, bool reverse)
+{
+	auto mission_raw = _mission_raw->download_mission();
+	CHECK(mission_raw.first == MissionRaw::Result::Success);
+
+	auto mission_items = mission_raw.second;
+	auto ct = get_coordinate_transformation();
+
+	_telemetry->set_rate_position_velocity_ned(5);
+	_telemetry->subscribe_position_velocity_ned([ct, mission_items, corridor_radius_m, reverse,
+	    this](Telemetry::PositionVelocityNed position_velocity_ned) {
+		auto progress = _mission_raw->mission_progress();
+
+
+		std::function<std::array<float, 3>(std::vector<mavsdk::MissionRaw::MissionItem>, unsigned, mavsdk::geometry::CoordinateTransformation)>
+		get_waypoint_for_sequence = [](std::vector<mavsdk::MissionRaw::MissionItem> mission_items, int sequence, auto ct) {
+			for (auto waypoint : mission_items) {
+
+				if (waypoint.seq == (uint32_t)sequence) {
+					return get_local_mission_item_from_raw_item<float>(waypoint, ct);
+				}
+			}
+
+			return  std::array<float, 3>({0.0f, 0.0f, 0.0f});
+		};
+
+		if (progress.current > 0 && progress.current < progress.total) {
+			// Get shortest distance of current position to 3D line between previous and next waypoint
+
+			std::array<float, 3> current { position_velocity_ned.position.north_m,
+						       position_velocity_ned.position.east_m,
+						       position_velocity_ned.position.down_m };
+			std::array<float, 3> wp_prev = get_waypoint_for_sequence(mission_items,
+						       reverse ? progress.current + 1 : progress.current - 1, ct);
+			std::array<float, 3> wp_next = get_waypoint_for_sequence(mission_items, progress.current, ct);
+
+			float distance_to_trajectory = point_to_line_distance(current, wp_prev, wp_next);
+
+			CHECK(distance_to_trajectory < corridor_radius_m);
+		}
+	});
+}
+
 void AutopilotTester::check_tracks_mission(float corridor_radius_m)
 {
 	auto mission = _mission->download_mission();
@@ -535,6 +578,12 @@ void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float
 	CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m);
 }
 
+void AutopilotTester::execute_rtl_when_reaching_mission_sequence(int sequence_number)
+{
+	start_and_wait_for_mission_sequence_raw(sequence_number);
+	execute_rtl();
+}
+
 std::array<float, 3> AutopilotTester::get_current_position_ned()
 {
 	mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
@@ -642,15 +691,15 @@ bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry:
 	return pass;
 }
 
-void AutopilotTester::start_and_wait_for_first_mission_item()
+void AutopilotTester::start_and_wait_for_mission_sequence(int sequence_number)
 {
 	auto prom = std::promise<void> {};
 	auto fut = prom.get_future();
 
-	_mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) {
+	_mission->subscribe_mission_progress([&prom, this, sequence_number](Mission::MissionProgress progress) {
 		std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
 
-		if (progress.current >= 1) {
+		if (progress.current >= sequence_number) {
 			_mission->subscribe_mission_progress(nullptr);
 			prom.set_value();
 		}
@@ -661,6 +710,25 @@ void AutopilotTester::start_and_wait_for_first_mission_item()
 	REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready);
 }
 
+void AutopilotTester::start_and_wait_for_mission_sequence_raw(int sequence_number)
+{
+	auto prom = std::promise<void> {};
+	auto fut = prom.get_future();
+
+	_mission_raw->subscribe_mission_progress([&prom, this, sequence_number](MissionRaw::MissionProgress progress) {
+		std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
+
+		if (progress.current >= sequence_number) {
+			_mission_raw->subscribe_mission_progress(nullptr);
+			prom.set_value();
+		}
+	});
+
+	REQUIRE(_mission_raw->start_mission() == MissionRaw::Result::Success);
+
+	REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready);
+}
+
 void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout)
 {
 	auto prom = std::promise<void> {};

diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h
@@ -137,9 +137,11 @@ class AutopilotTester
 	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 check_tracks_mission_raw(float corridor_radius_m = 1.f, bool reverse = false);
 	void start_checking_altitude(const float max_deviation_m);
 	void stop_checking_altitude();
 	void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f);
+	void execute_rtl_when_reaching_mission_sequence(int sequence_number);
 
 	// Blocking call to get the drone's current position in NED frame
 	std::array<float, 3> get_current_position_ned();
@@ -199,7 +201,8 @@ class AutopilotTester
 	bool ground_truth_horizontal_position_far_from(const Telemetry::GroundTruth &target_pos, float min_distance_m);
 	bool estimated_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m);
 	bool estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m);
-	void start_and_wait_for_first_mission_item();
+	void start_and_wait_for_mission_sequence(int sequence_number);
+	void start_and_wait_for_mission_sequence_raw(int sequence_number);
 	void wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout);
 	void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout);
 	void wait_for_mission_finished(std::chrono::seconds timeout);

diff --git a/test/mavsdk_tests/autopilot_tester_rtl.cpp b/test/mavsdk_tests/autopilot_tester_rtl.cpp
@@ -0,0 +1,57 @@
+
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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_rtl.h"
+
+#include "math_helpers.h"
+#include <iostream>
+#include <future>
+#include <thread>
+#include <unistd.h>
+
+
+void AutopilotTesterRtl::connect(const std::string uri)
+{
+	AutopilotTester::connect(uri);
+}
+
+void AutopilotTesterRtl::set_rtl_type(int rtl_type)
+{
+	CHECK(getParams()->set_param_int("RTL_TYPE", rtl_type) == Param::Result::Success);
+}
+
+void AutopilotTesterRtl::set_takeoff_land_requirements(int req)
+{
+	CHECK(getParams()->set_param_int("MIS_TKO_LAND_REQ", req) == Param::Result::Success);
+}
diff --git a/test/mavsdk_tests/autopilot_tester_rtl.h b/test/mavsdk_tests/autopilot_tester_rtl.h
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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>
+
+
+class AutopilotTesterRtl : public AutopilotTester
+{
+public:
+	AutopilotTesterRtl() = default;
+	~AutopilotTesterRtl() = default;
+
+	void set_rtl_type(int rtl_type);
+	void set_takeoff_land_requirements(int req);
+	void connect(const std::string uri);
+
+
+private:
+	std::unique_ptr<mavsdk::Failure> _failure{};
+};
diff --git a/test/mavsdk_tests/math_helpers.h b/test/mavsdk_tests/math_helpers.h
@@ -44,6 +44,20 @@ std::array<T, 3> get_local_mission_item(const Mission::MissionItem &item, const
 	return {static_cast<T>(local.north_m), static_cast<T>(local.east_m), -item.relative_altitude_m};
 }
 
+template<typename T>
+std::array<T, 3> get_local_mission_item_from_raw_item(const mavsdk::MissionRaw::MissionItem &item,
+		const CoordinateTransformation &ct)
+{
+	using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate;
+	GlobalCoordinate global;
+	global.latitude_deg = item.x / 1e7;
+	global.longitude_deg = item.y / 1e7;
+
+
+	auto local = ct.local_from_global(global);
+	return {static_cast<T>(local.north_m), static_cast<T>(local.east_m), -item.z};
+}
+
 template<typename T>
 T sq(T x)
 {

diff --git a/test/mavsdk_tests/test_vtol_mission.cpp b/test/mavsdk_tests/test_vtol_mission.cpp
@@ -31,7 +31,7 @@
  *
  ****************************************************************************/
 
-#include "autopilot_tester.h"
+#include "autopilot_tester_rtl.h"
 
 
 TEST_CASE("Fly VTOL mission", "[vtol]")
@@ -44,3 +44,48 @@ TEST_CASE("Fly VTOL mission", "[vtol]")
 	tester.execute_mission_raw();
 	tester.wait_until_disarmed();
 }
+TEST_CASE("RTL direct Home", "[vtol]")
+{
+	AutopilotTesterRtl tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan");
+	// fly directly to home position
+	tester.set_rtl_type(0);
+	tester.arm();
+	tester.execute_rtl_when_reaching_mission_sequence(2);
+	tester.wait_until_disarmed(std::chrono::seconds(90));
+	tester.check_home_within(5.0f);
+}
+
+TEST_CASE("RTL with Mission Landing", "[vtol]")
+{
+	AutopilotTesterRtl tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan");
+	// Vehicle should follow the mission and use the mission landing
+	tester.set_rtl_type(2);
+	tester.arm();
+	tester.execute_rtl_when_reaching_mission_sequence(2);
+	tester.check_tracks_mission_raw(30.0f);
+	tester.wait_until_disarmed(std::chrono::seconds(90));
+}
+
+TEST_CASE("RTL with Reverse Mission", "[vtol]")
+{
+	AutopilotTesterRtl tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.set_takeoff_land_requirements(0);
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_without_landing.plan");
+	// vehicle should follow the mission in reverse and land at the home position
+	tester.set_rtl_type(2);
+	tester.arm();
+	tester.execute_rtl_when_reaching_mission_sequence(6);
+	/* this check is currently disabled since there seems to be a bug in mavsdk which causes the reported
+	mission sequence to be out of order */
+	tester.check_tracks_mission_raw(300.0f);
+	tester.wait_until_disarmed(std::chrono::seconds(90));
+}