mavsdk: Add integration tests for RTL with approaches

test/mavsdk_tests/CMakeLists.txt

@@ -28,6 +28,7 @@ if(MAVSDK_FOUND)
         test_multicopter_offboard.cpp
         test_multicopter_manual.cpp
         test_vtol_mission.cpp
+        test_vtol_rtl.cpp
         # test_multicopter_follow_me.cpp
     )
 

test/mavsdk_tests/autopilot_tester.cpp

@@ -604,6 +604,22 @@ void AutopilotTester::check_tracks_mission_raw(float corridor_radius_m, bool rev
 	});
 }
 
+void AutopilotTester::check_mission_land_within(float acceptance_radius_m)
+{
+	auto mission_raw = _mission_raw->download_mission();
+	CHECK(mission_raw.first == MissionRaw::Result::Success);
+
+	// Get last mission item
+	MissionRaw::MissionItem land_mission_item = mission_raw.second.back();
+	bool is_landing_item = (land_mission_item.command == 85) || (land_mission_item.command == 21);
+	CHECK(is_landing_item);
+	Telemetry::GroundTruth land_coord{};
+	land_coord.latitude_deg = static_cast<double>(land_mission_item.x) / 1E7;
+	land_coord.longitude_deg = static_cast<double>(land_mission_item.y) / 1E7;
+
+	CHECK(ground_truth_horizontal_position_close_to(land_coord, acceptance_radius_m));
+}
+
 void AutopilotTester::check_tracks_mission(float corridor_radius_m)
 {
 	auto mission = _mission->download_mission();

test/mavsdk_tests/autopilot_tester.h

@@ -141,6 +141,7 @@ class AutopilotTester
 	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 check_mission_land_within(float acceptance_radius_m);
 	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);
@@ -164,6 +165,7 @@ class AutopilotTester
 	MavlinkPassthrough *getMavlinkPassthrough() const { return _mavlink_passthrough.get();}
 	std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
 	mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();
+	bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, float acceptance_radius_m);
 
 	const Telemetry::GroundTruth &getHome()
 	{
@@ -205,7 +207,6 @@ class AutopilotTester
 		const MissionOptions &mission_options,
 		const mavsdk::geometry::CoordinateTransformation &ct);
 
-	bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, float acceptance_radius_m);
 	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);

test/mavsdk_tests/autopilot_tester_rtl.cpp

@@ -35,11 +35,14 @@
 #include "autopilot_tester_rtl.h"
 
 #include "math_helpers.h"
+#include <cmath>
 #include <iostream>
 #include <future>
 #include <thread>
 #include <unistd.h>
 
+#include <mavsdk/plugins/telemetry/telemetry.h>
+
 
 void AutopilotTesterRtl::connect(const std::string uri)
 {
@@ -51,7 +54,211 @@ void AutopilotTesterRtl::set_rtl_type(int rtl_type)
 	CHECK(getParams()->set_param_int("RTL_TYPE", rtl_type) == Param::Result::Success);
 }
 
+void AutopilotTesterRtl::set_rtl_appr_force(int rtl_appr_force)
+{
+	CHECK(getParams()->set_param_int("RTL_APPR_FORCE", rtl_appr_force) == Param::Result::Success);
+}
+
 void AutopilotTesterRtl::set_takeoff_land_requirements(int req)
 {
 	CHECK(getParams()->set_param_int("MIS_TKO_LAND_REQ", req) == Param::Result::Success);
 }
+
+void AutopilotTesterRtl::upload_custom_mission(std::chrono::seconds timeout)
+{
+	std::promise<void> prom;
+	auto fut = prom.get_future();
+
+	uint8_t mission_type = _custom_mission[0].mission_type;
+	// Register callback to mission item request.
+	add_mavlink_message_callback(MAVLINK_MSG_ID_MISSION_REQUEST_INT, [this, mission_type,
+	      &prom](const mavlink_message_t &message) {
+
+		mavlink_mission_request_int_t request_int_message;
+		mavlink_msg_mission_request_int_decode(&message, &request_int_message);
+
+		if (request_int_message.mission_type == mission_type) {
+			// send requested element.
+			mavlink_message_t mission_item_int_mavlink_message;
+			mavlink_msg_mission_item_int_encode(getMavlinkPassthrough()->get_our_sysid(), getMavlinkPassthrough()->get_our_compid(),
+							    &mission_item_int_mavlink_message, &(_custom_mission[request_int_message.seq]));
+			send_custom_mavlink_message(mission_item_int_mavlink_message);
+
+			if (request_int_message.seq + 1U == _custom_mission.size()) {
+				add_mavlink_message_callback(MAVLINK_MSG_ID_MISSION_REQUEST_INT, nullptr);
+				prom.set_value();
+			}
+		}
+	});
+
+	// send mission count
+	mavlink_mission_count_t mission_count_message;
+	mission_count_message.count = _custom_mission.size();
+	mission_count_message.target_system = getMavlinkPassthrough()->get_target_sysid();
+	mission_count_message.target_component = getMavlinkPassthrough()->get_target_compid();
+	mission_count_message.mission_type = mission_type;
+
+	mavlink_message_t mission_count_mavlink_message;
+	mavlink_msg_mission_count_encode(getMavlinkPassthrough()->get_our_sysid(), getMavlinkPassthrough()->get_our_compid(),
+					 &mission_count_mavlink_message, &mission_count_message);
+
+	send_custom_mavlink_message(mission_count_mavlink_message);
+
+	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
+}
+
+void AutopilotTesterRtl::add_home_to_rally_point()
+{
+	add_local_rally_point({0., 0.});
+}
+
+void AutopilotTesterRtl::add_home_with_approaches_to_rally_point()
+{
+	add_local_rally_point({0., 0.});
+	add_approaches_to_point({0., 0.});
+}
+
+void AutopilotTesterRtl::add_local_rally_point(mavsdk::geometry::CoordinateTransformation::LocalCoordinate
+		local_coordinate)
+{
+	_rally_points.push_back(local_coordinate);
+
+	mavsdk::geometry::CoordinateTransformation ct(get_coordinate_transformation());
+	mavsdk::geometry::CoordinateTransformation::GlobalCoordinate pos(ct.global_from_local(local_coordinate));
+
+	// Set rally point
+	mavlink_mission_item_int_t tmp_mission_item;
+	tmp_mission_item.param1 = 0.f;
+	tmp_mission_item.param2 = 0.f;
+	tmp_mission_item.param3 = 0.f;
+	tmp_mission_item.param4 = 0.f;
+	tmp_mission_item.x = static_cast<int32_t>(pos.latitude_deg * 1E7);
+	tmp_mission_item.y = static_cast<int32_t>(pos.longitude_deg * 1E7);
+	tmp_mission_item.z = 0.f;
+	tmp_mission_item.seq = static_cast<uint16_t>(_custom_mission.size());
+	tmp_mission_item.command = MAV_CMD_NAV_RALLY_POINT;
+	tmp_mission_item.target_system = getMavlinkPassthrough()->get_target_sysid();
+	tmp_mission_item.target_component = getMavlinkPassthrough()->get_target_compid();
+	tmp_mission_item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+	tmp_mission_item.current = 0;
+	tmp_mission_item.autocontinue = 0;
+	tmp_mission_item.mission_type = MAV_MISSION_TYPE_RALLY;
+
+	_custom_mission.push_back(tmp_mission_item);
+}
+
+void AutopilotTesterRtl::add_local_rally_with_approaches_point(
+	mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate)
+{
+	add_local_rally_point(local_coordinate);
+	add_approaches_to_point(local_coordinate);
+}
+
+void AutopilotTesterRtl::add_approaches_to_point(mavsdk::geometry::CoordinateTransformation::LocalCoordinate
+		local_coordinate)
+{
+
+	mavsdk::geometry::CoordinateTransformation ct(get_coordinate_transformation());
+
+	// Set north loiter to alt
+	mavsdk::geometry::CoordinateTransformation::LocalCoordinate tmp_coordinate{local_coordinate};
+	tmp_coordinate.north_m += 200.;
+	mavsdk::geometry::CoordinateTransformation::GlobalCoordinate pos(ct.global_from_local(tmp_coordinate));
+	mavlink_mission_item_int_t tmp_mission_item;
+	tmp_mission_item.param1 = 0.f;
+	tmp_mission_item.param2 = 80.f;
+	tmp_mission_item.param3 = 0.f;
+	tmp_mission_item.param4 = 0.f;
+	tmp_mission_item.x = static_cast<int32_t>(pos.latitude_deg * 1E7);
+	tmp_mission_item.y = static_cast<int32_t>(pos.longitude_deg * 1E7);
+	tmp_mission_item.z = 15.f;
+	tmp_mission_item.seq = static_cast<uint16_t>(_custom_mission.size());
+	tmp_mission_item.command = MAV_CMD_NAV_LOITER_TO_ALT;
+	tmp_mission_item.target_system = getMavlinkPassthrough()->get_target_sysid();
+	tmp_mission_item.target_component = getMavlinkPassthrough()->get_target_compid();
+	tmp_mission_item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+	tmp_mission_item.current = 0;
+	tmp_mission_item.autocontinue = 0;
+	tmp_mission_item.mission_type = MAV_MISSION_TYPE_RALLY;
+
+	_custom_mission.push_back(tmp_mission_item);
+
+	// Set east loiter to alt
+	tmp_coordinate = local_coordinate;
+	tmp_coordinate.east_m += 200.;
+	pos = ct.global_from_local(tmp_coordinate);
+	tmp_mission_item.x = static_cast<int32_t>(pos.latitude_deg * 1E7);
+	tmp_mission_item.y = static_cast<int32_t>(pos.longitude_deg * 1E7);
+	tmp_mission_item.seq = static_cast<uint16_t>(_custom_mission.size());
+
+	_custom_mission.push_back(tmp_mission_item);
+}
+
+void AutopilotTesterRtl::check_rally_point_within(float acceptance_radius_m)
+{
+	auto old_home(getHome());
+	mavsdk::geometry::CoordinateTransformation ct({old_home.latitude_deg, old_home.longitude_deg});
+	Telemetry::GroundTruth land_coord{};
+	mavsdk::geometry::CoordinateTransformation::GlobalCoordinate pos;
+	bool within_rally_point{false};
+
+	for (const auto &rally_point : _rally_points) {
+		pos = ct.global_from_local(rally_point);
+		land_coord.latitude_deg = pos.latitude_deg;
+		land_coord.longitude_deg = pos.longitude_deg;
+		within_rally_point |= ground_truth_horizontal_position_close_to(land_coord, acceptance_radius_m);
+	}
+
+	CHECK(within_rally_point);
+}
+
+void AutopilotTesterRtl::check_rtl_approaches(float acceptance_radius_m, std::chrono::seconds timeout)
+{
+	auto prom = std::promise<bool> {};
+	auto fut = prom.get_future();
+	auto ct = get_coordinate_transformation();
+	auto return_rtl_alt = getParams()->get_param_float("RTL_RETURN_ALT");
+	auto descend_rtl_alt = getParams()->get_param_float("RTL_DESCEND_ALT");
+	REQUIRE(return_rtl_alt.first == Param::Result::Success);
+	REQUIRE(descend_rtl_alt.first == Param::Result::Success);
+
+	getTelemetry()->subscribe_position_velocity_ned([&prom, acceptance_radius_m, return_rtl_alt, descend_rtl_alt, ct,
+	       this](Telemetry::PositionVelocityNed position_velocity_ned) {
+
+		if ((-position_velocity_ned.position.down_m < return_rtl_alt.second - 3.)
+		    && (position_velocity_ned.velocity.down_m_s > 0.05)) {
+			// We started to loiter down so we should be on the approach loiter
+			bool on_approach_loiter(false);
+
+			for (const auto mission_item : _custom_mission) {
+				if (mission_item.command == MAV_CMD_NAV_LOITER_TO_ALT) {
+					mavsdk::geometry::CoordinateTransformation::LocalCoordinate pos(ct.local_from_global({static_cast<double>(mission_item.x) / 1E7, static_cast<double>(mission_item.y) / 1E7}));
+					double rel_distance_to_center = sqrt(sq(position_velocity_ned.position.north_m - pos.north_m) + sq(
+							position_velocity_ned.position.east_m - pos.east_m));
+
+					if ((rel_distance_to_center > (mission_item.param2 - acceptance_radius_m))
+					    && (rel_distance_to_center > (mission_item.param2 + acceptance_radius_m))) {
+						on_approach_loiter |= true;
+
+						if (-position_velocity_ned.position.down_m < descend_rtl_alt.second + 3.) {
+							// We reached the altitude
+							getTelemetry()->subscribe_position_velocity_ned(nullptr);
+							prom.set_value(true);
+							return;
+
+						}
+					}
+				}
+			}
+
+			if (!on_approach_loiter) {
+				getTelemetry()->subscribe_position_velocity_ned(nullptr);
+				prom.set_value(false);
+
+			}
+		}
+	});
+
+	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
+	REQUIRE(fut.get());
+}

test/mavsdk_tests/autopilot_tester_rtl.h

@@ -35,7 +35,10 @@
 
 #include "autopilot_tester.h"
 
+#include <vector>
+
 #include <mavsdk/mavsdk.h>
+#include <mavsdk/geometry.h>
 #include <mavsdk/plugins/action/action.h>
 
 
@@ -46,10 +49,25 @@ class AutopilotTesterRtl : public AutopilotTester
 	~AutopilotTesterRtl() = default;
 
 	void set_rtl_type(int rtl_type);
+	void set_rtl_appr_force(int rtl_appr_force);
 	void set_takeoff_land_requirements(int req);
+	void add_home_to_rally_point();
+	void add_home_with_approaches_to_rally_point();
+	void add_local_rally_point(mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate);
+	void add_local_rally_with_approaches_point(mavsdk::geometry::CoordinateTransformation::LocalCoordinate
+			local_coordinate);
 	void connect(const std::string uri);
+	void check_rally_point_within(float acceptance_radius_m);
+	void check_rtl_approaches(float acceptance_radius_m, std::chrono::seconds timeout);
+	/* NOTE mavsdk mission upload should be used when possible. Only use this when uploading a mission which is not yet suppported by mavsdk.
+	 * Used here to to test the new way of uploading approaches for rally points. */
+	void upload_custom_mission(std::chrono::seconds timeout);
 
 
 private:
+	void add_approaches_to_point(mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate);
+
 	std::unique_ptr<mavsdk::Failure> _failure{};
+	std::vector<mavlink_mission_item_int_t> _custom_mission{};
+	std::vector<mavsdk::geometry::CoordinateTransformation::LocalCoordinate> _rally_points{};
 };

test/mavsdk_tests/test_vtol_mission.cpp

@@ -31,7 +31,7 @@
  *
  ****************************************************************************/
 
-#include "autopilot_tester_rtl.h"
+#include "autopilot_tester.h"
 
 
 TEST_CASE("Fly VTOL mission", "[vtol]")
@@ -44,47 +44,3 @@ 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(35.0f);
-	tester.wait_until_disarmed(std::chrono::seconds(95));
-}
-
-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 // TODO enable checks again if MAVSDK can handle mission in reverse order
-	tester.set_rtl_type(2);
-	tester.arm();
-	tester.execute_rtl_when_reaching_mission_sequence(6);
-	//tester.check_tracks_mission_raw(35.0f);
-	tester.wait_until_disarmed(std::chrono::seconds(90));
-}