From db361d7a599cd7051b2b9081d214765d3b617298 Mon Sep 17 00:00:00 2001
From: Julian Kent <julian@auterion.com>
Date: Thu, 9 Jul 2020 17:36:24 +0200
Subject: [PATCH] Add straight-line MAVSDK test

---
 test/mavsdk_tests/autopilot_tester.cpp        | 37 +++++++++++++++++++
 test/mavsdk_tests/autopilot_tester.h          |  3 ++
 .../mavsdk_tests/test_multicopter_mission.cpp | 19 ++++++++++
 3 files changed, 59 insertions(+)

diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index cb4b9716a7..109daa3745 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -164,6 +164,30 @@ void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
 	REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready);
 }
 
+void AutopilotTester::prepare_straight_mission(MissionOptions mission_options)
+{
+	const auto ct = get_coordinate_transformation();
+
+	Mission::MissionPlan mission_plan {};
+	mission_plan.mission_items.push_back(create_mission_item({0, 0.}, mission_options, ct));
+	mission_plan.mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0}, mission_options, ct));
+	mission_plan.mission_items.push_back(create_mission_item({2 * mission_options.leg_length_m, 0}, mission_options, ct));
+	mission_plan.mission_items.push_back(create_mission_item({3 * mission_options.leg_length_m, 0}, mission_options, ct));
+	mission_plan.mission_items.push_back(create_mission_item({4 * mission_options.leg_length_m, 0}, mission_options, ct));
+
+	_mission->set_return_to_launch_after_mission(mission_options.rtl_at_end);
+
+	std::promise<void> prom;
+	auto fut = prom.get_future();
+
+	_mission->upload_mission_async(mission_plan, [&prom](Mission::Result result) {
+		REQUIRE(Mission::Result::Success == result);
+		prom.set_value();
+	});
+
+	REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready);
+}
+
 void AutopilotTester::execute_mission()
 {
 	std::promise<void> prom;
@@ -203,6 +227,7 @@ Mission::MissionItem  AutopilotTester::create_mission_item(
 	mission_item.latitude_deg = pos_north.latitude_deg;
 	mission_item.longitude_deg = pos_north.longitude_deg;
 	mission_item.relative_altitude_m = mission_options.relative_altitude_m;
+	mission_item.is_fly_through = mission_options.fly_through;
 	return mission_item;
 }
 
@@ -221,6 +246,18 @@ void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, floa
 	std::cout << "Target position reached" << std::endl;
 }
 
+void AutopilotTester::check_mission_item_speed_above(int item_index, float min_speed)
+{
+	_telemetry->subscribe_velocity_ned([item_index, min_speed, this](Telemetry::VelocityNed velocity) {
+		float horizontal = std::hypot(velocity.north_m_s, velocity.east_m_s);
+		auto progress = _mission->mission_progress();
+
+		if (progress.current == item_index) {
+			CHECK(horizontal > min_speed);
+		}
+	});
+}
+
 void AutopilotTester::offboard_land()
 {
 	Offboard::VelocityNedYaw land_velocity;
diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h
index 05f6c50f0c..ca3651228f 100644
--- a/test/mavsdk_tests/autopilot_tester.h
+++ b/test/mavsdk_tests/autopilot_tester.h
@@ -56,6 +56,7 @@ public:
 		double leg_length_m {20.0};
 		double relative_altitude_m {10.0};
 		bool rtl_at_end {false};
+		bool fly_through {false};
 	};
 
 	void connect(const std::string uri);
@@ -72,12 +73,14 @@ public:
 	void wait_until_disarmed(std::chrono::seconds timeout_duration = std::chrono::seconds(90));
 	void wait_until_hovering();
 	void prepare_square_mission(MissionOptions mission_options);
+	void prepare_straight_mission(MissionOptions mission_options);
 	void execute_mission();
 	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));
 	void offboard_land();
 	void request_ground_truth();
+	void check_mission_item_speed_above(int item_index, float min_speed);
 
 
 private:
diff --git a/test/mavsdk_tests/test_multicopter_mission.cpp b/test/mavsdk_tests/test_multicopter_mission.cpp
index b8258e14b3..c3e86139c0 100644
--- a/test/mavsdk_tests/test_multicopter_mission.cpp
+++ b/test/mavsdk_tests/test_multicopter_mission.cpp
@@ -78,3 +78,22 @@ TEST_CASE("Fly square Multicopter Missions", "[multicopter][vtol]")
 		tester.wait_until_disarmed();
 	}
 }
+
+TEST_CASE("Fly straight Multicopter Mission", "[multicopter]")
+{
+	AutopilotTester tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+
+	AutopilotTester::MissionOptions mission_options;
+	mission_options.rtl_at_end = false;
+	mission_options.fly_through = true;
+	tester.prepare_straight_mission(mission_options);
+	tester.check_mission_item_speed_above(2, 4.5);
+	tester.arm();
+	tester.execute_mission();
+	tester.wait_until_hovering();
+	tester.execute_rtl();
+	tester.wait_until_disarmed();
+
+}