mavsdk_tests: Add integration tests for figure of 8

PX4 · Oct 31, 2023 · e3473a0 · e3473a0
1 parent 8edd7ce
commit e3473a0
Show file tree

Hide file tree

Showing 6 changed files with 329 additions and 0 deletions.
diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt
@@ -18,6 +18,7 @@ if(MAVSDK_FOUND)
         autopilot_tester.cpp
         autopilot_tester_failure.cpp
         autopilot_tester_rtl.cpp
+        autopilot_tester_figure_eight.cpp
         # follow-me needs a MAVSDK update:
         # https://github.com/mavlink/MAVSDK/pull/1770
         # autopilot_tester_follow_me.cpp
@@ -28,6 +29,7 @@ if(MAVSDK_FOUND)
         test_multicopter_offboard.cpp
         test_multicopter_manual.cpp
         test_vtol_mission.cpp
+        test_vtol_figure_eight.cpp
         test_vtol_rtl.cpp
         # test_multicopter_follow_me.cpp
     )

diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
@@ -220,6 +220,12 @@ void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::sec
 	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
 }
 
+void AutopilotTester::wait_until_fixedwing(std::chrono::seconds timeout)
+{
+	REQUIRE(poll_condition_with_timeout(
+	[this]() { return _telemetry->vtol_state() == Telemetry::VtolState::Fw; }, timeout));
+}
+
 void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
 {
 	const auto ct = get_coordinate_transformation();

diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h
@@ -118,6 +118,7 @@ class AutopilotTester
 	void wait_until_disarmed(std::chrono::seconds timeout_duration = std::chrono::seconds(60));
 	void wait_until_hovering(); // TODO: name suggests, that function waits for drone velocity to be zero and not just drone in the air
 	void wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout);
+	void wait_until_fixedwing(std::chrono::seconds timeout);
 	void wait_until_speed_lower_than(float speed, std::chrono::seconds timeout);
 	void prepare_square_mission(MissionOptions mission_options);
 	void prepare_straight_mission(MissionOptions mission_options);

diff --git a/test/mavsdk_tests/autopilot_tester_figure_eight.cpp b/test/mavsdk_tests/autopilot_tester_figure_eight.cpp
@@ -0,0 +1,175 @@
+
+/****************************************************************************
+ *
+ *   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_figure_eight.h"
+
+#include <cmath>
+#include <float.h>
+#include <future>
+#include <vector>
+
+#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
+#include <mavlink/development/mavlink_msg_figure_eight_execution_status.h>
+
+using namespace mavsdk::geometry;
+
+void AutopilotTesterFigureEight::execute_figure_eight()
+{
+
+	MavlinkPassthrough::CommandInt figure_eight_command;
+
+	auto ct = get_coordinate_transformation();
+	const auto global_center = ct.global_from_local(_figure_eight.center);
+
+	figure_eight_command.target_sysid = getMavlinkPassthrough()->get_target_sysid();
+	figure_eight_command.target_compid = getMavlinkPassthrough()->get_target_compid();
+	figure_eight_command.command = 35; // Figure eight command
+	figure_eight_command.frame = MAV_FRAME_GLOBAL_INT;
+	figure_eight_command.param1 = _figure_eight.major_axis;
+	figure_eight_command.param2 = _figure_eight.minor_axis;
+	figure_eight_command.param3 = NAN;
+	figure_eight_command.param4 = _figure_eight.orientation;
+	figure_eight_command.x = static_cast<int32_t>(global_center.latitude_deg * 1E7);
+	figure_eight_command.y = static_cast<int32_t>(global_center.longitude_deg * 1E7);
+	figure_eight_command.z = _figure_eight.alt;
+
+	send_custom_mavlink_command(figure_eight_command);
+}
+
+void AutopilotTesterFigureEight::set_figure_eight(const double major_axis, const double minor_axis,
+		const double orientation, const double home_offset_N, const double home_offset_E, const double rel_alt)
+{
+	_figure_eight.major_axis = major_axis;
+	_figure_eight.minor_axis = minor_axis;
+	_figure_eight.orientation = orientation;
+	_figure_eight.alt = getHome().absolute_altitude_m + rel_alt;
+	_figure_eight.center = {home_offset_N, home_offset_E};
+}
+
+void AutopilotTesterFigureEight::check_tracks_figure_eight(std::chrono::seconds timeout, double corridor_radius_m)
+{
+	auto prom = std::promise<bool> {};
+	auto fut = prom.get_future();
+
+	const double cos_or = cos(_figure_eight.orientation);
+	const double sin_or = sin(_figure_eight.orientation);
+
+	std::vector<CoordinateTransformation::LocalCoordinate> figure_eight_point_of_interest;
+	figure_eight_point_of_interest.push_back(_figure_eight.center);
+	figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (abs(_figure_eight.major_axis) - _figure_eight.minor_axis) - sin_or * (- _figure_eight.minor_axis), _figure_eight.center.east_m + sin_or * (abs(_figure_eight.major_axis) - _figure_eight.minor_axis) + cos_or * (- _figure_eight.minor_axis)});
+	figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (abs(_figure_eight.major_axis)) - sin_or * 0., _figure_eight.center.east_m + sin_or * (abs(_figure_eight.major_axis)) + cos_or * 0.});
+	figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (abs(_figure_eight.major_axis) - _figure_eight.minor_axis) - sin_or * (_figure_eight.minor_axis), _figure_eight.center.east_m + sin_or * (abs(_figure_eight.major_axis) - _figure_eight.minor_axis) + cos_or * (_figure_eight.minor_axis)});
+	figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (-abs(_figure_eight.major_axis) + _figure_eight.minor_axis) - sin_or * (- _figure_eight.minor_axis), _figure_eight.center.east_m + sin_or * (-abs(_figure_eight.major_axis) + _figure_eight.minor_axis) + cos_or * (- _figure_eight.minor_axis)});
+	figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (-abs(_figure_eight.major_axis)) - sin_or * 0., _figure_eight.center.east_m + sin_or * (-abs(_figure_eight.major_axis)) + cos_or * (0.)});
+	figure_eight_point_of_interest.push_back({_figure_eight.center.north_m + cos_or * (-abs(_figure_eight.major_axis) + _figure_eight.minor_axis) - sin_or * (+ _figure_eight.minor_axis), _figure_eight.center.east_m + sin_or * (-abs(_figure_eight.major_axis) + _figure_eight.minor_axis) + cos_or * (_figure_eight.minor_axis)});
+
+	std::vector<int32_t> order_to_fly;
+
+	if (_figure_eight.major_axis > 0) {
+		order_to_fly = std::vector<int32_t> {0, 1, 2, 3, 0, 4, 5, 6, 0};
+
+	} else {
+		order_to_fly = std::vector<int32_t> {0, 3, 2, 1, 0, 6, 5, 4, 0};
+	}
+
+	getTelemetry()->subscribe_position_velocity_ned([&figure_eight_point_of_interest, &prom, corridor_radius_m,
+					 &order_to_fly, this](Telemetry::PositionVelocityNed position_velocity_ned) {
+		static size_t index{0};
+		int32_t close_index{-1};
+
+		for (size_t interest_point_index{0}; interest_point_index < figure_eight_point_of_interest.size();
+		     interest_point_index++) {
+			if ((abs(position_velocity_ned.position.north_m - figure_eight_point_of_interest[interest_point_index].north_m) <
+			     corridor_radius_m) &&
+			    (abs(position_velocity_ned.position.east_m - figure_eight_point_of_interest[interest_point_index].east_m) <
+			     corridor_radius_m)) {
+				close_index = static_cast<int32_t>(interest_point_index);
+				break;
+			}
+		}
+
+		if (close_index >= 0) {
+			if (close_index == order_to_fly[index]) { // Still at the same point already found
+				// Do nothing
+
+			} else if (close_index == order_to_fly[index + 1]) { // reached the next expected point
+				index++;
+
+			} else { // reached an out of order point
+
+				if (index > 0U) { // only set to false if we already hve passed the first center point
+					getTelemetry()->subscribe_position_velocity_ned(nullptr);
+					prom.set_value(false);
+				}
+			}
+		}
+
+		if (index + 1 == order_to_fly.size()) {
+			getTelemetry()->subscribe_position_velocity_ned(nullptr);
+			prom.set_value(true);
+		}
+	});
+
+	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
+	CHECK(fut.get() == true);
+}
+
+void AutopilotTesterFigureEight::check_receive_execution_status(std::chrono::seconds timeout)
+{
+	auto prom = std::promise<void> {};
+	auto fut = prom.get_future();
+
+	auto ct = get_coordinate_transformation();
+	const auto global_center = ct.global_from_local(_figure_eight.center);
+
+	add_mavlink_message_callback(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS, [&prom, global_center,
+	       this](const mavlink_message_t &message) {
+		add_mavlink_message_callback(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS, nullptr);
+		mavlink_figure_eight_execution_status_t status_message;
+
+		mavlink_msg_figure_eight_execution_status_decode(&message, &status_message);
+		CHECK(abs(status_message.major_radius - _figure_eight.major_axis) < 1E-4);
+		CHECK(abs(status_message.minor_radius - _figure_eight.minor_axis) < 1E-4);
+		CHECK(abs(status_message.orientation - _figure_eight.orientation) < 1E-7);
+		CHECK(status_message.x == static_cast<int32_t>(global_center.latitude_deg * 1E7));
+		CHECK(status_message.y == static_cast<int32_t>(global_center.longitude_deg * 1E7));
+		CHECK(abs(status_message.z - _figure_eight.alt) < 1E-4);
+
+		prom.set_value();
+
+		return true;
+	});
+
+	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
+}
diff --git a/test/mavsdk_tests/autopilot_tester_figure_eight.h b/test/mavsdk_tests/autopilot_tester_figure_eight.h
@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   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/geometry.h>
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+
+class AutopilotTesterFigureEight : public AutopilotTester
+{
+public:
+	AutopilotTesterFigureEight() = default;
+	~AutopilotTesterFigureEight() = default;
+
+	void set_figure_eight(double major_axis, double minor_axis, double orientation, double home_offset_N,
+			      double home_offset_E, double rel_alt);
+
+	void execute_figure_eight();
+	void check_tracks_figure_eight(std::chrono::seconds timeout, double corridor_radius_m = 5.f);
+	void check_receive_execution_status(std::chrono::seconds timeout);
+
+private:
+
+	struct {
+		double major_axis;
+		double minor_axis;
+		double orientation;
+		CoordinateTransformation::LocalCoordinate center;
+		double alt;
+	} _figure_eight;
+};
diff --git a/test/mavsdk_tests/test_vtol_figure_eight.cpp b/test/mavsdk_tests/test_vtol_figure_eight.cpp
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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_figure_eight.h"
+
+#include <chrono>
+#include <thread>
+
+TEST_CASE("Figure eight execution clockwise", "[vtol]")
+{
+	AutopilotTesterFigureEight tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	const float takeoff_altitude = 20.f;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.arm();
+	tester.takeoff();
+	tester.wait_until_hovering();
+	tester.wait_until_altitude(takeoff_altitude - 1.f, std::chrono::seconds(30));
+	tester.transition_to_fixedwing();
+	tester.wait_until_fixedwing(std::chrono::seconds(5));
+	std::this_thread::sleep_for(std::chrono::seconds(1));
+	tester.set_figure_eight(150., 50., 0., 200., 0., 20.);
+	tester.execute_figure_eight();
+	tester.check_tracks_figure_eight(std::chrono::seconds(60), 10.);
+	// tester.check_receive_execution_status(std::chrono::seconds(
+	// 		5)); //TODO With mavsdk we can't subscribe to custom messages. Need to wait until messages are recognised by mavsdk
+}
+
+TEST_CASE("Figure eight execution counterclockwise", "[vtol]")
+{
+	AutopilotTesterFigureEight tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	const float takeoff_altitude = 20.f;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.arm();
+	tester.takeoff();
+	tester.wait_until_hovering();
+	tester.wait_until_altitude(takeoff_altitude - 1.f, std::chrono::seconds(30));
+	tester.transition_to_fixedwing();
+	tester.wait_until_fixedwing(std::chrono::seconds(5));
+	std::this_thread::sleep_for(std::chrono::seconds(1));
+	tester.set_figure_eight(-150., 50., 30.*M_PI / 180., 200., 0., 20.);
+	tester.execute_figure_eight();
+	tester.check_tracks_figure_eight(std::chrono::seconds(60), 10.);
+	// tester.check_receive_execution_status(std::chrono::seconds(
+	// 		5)); //TODO With mavsdk we can't subscribe to custom messages. Need to wait until messages are recognised by mavsdk.
+}