mavsdk_tests: move waypoints to vehicle position

This way it shouldn't matter where the test is run.

test/mavsdk_tests/autopilot_tester.cpp

@@ -326,11 +326,13 @@ Mission::MissionItem  AutopilotTester::create_mission_item(
 	return mission_item;
 }
 
-void AutopilotTester::load_qgc_mission_raw(const std::string &plan_file)
+void AutopilotTester::load_qgc_mission_raw_and_move_here(const std::string &plan_file)
 {
-	const auto import_result = _mission_raw->import_qgroundcontrol_mission(plan_file);
+	auto import_result = _mission_raw->import_qgroundcontrol_mission(plan_file);
 	REQUIRE(import_result.first == MissionRaw::Result::Success);
 
+	move_mission_raw_here(import_result.second.mission_items);
+
 	REQUIRE(_mission_raw->upload_mission(import_result.second.mission_items) == MissionRaw::Result::Success);
 }
 
@@ -654,6 +656,24 @@ void AutopilotTester::wait_for_mission_raw_finished(std::chrono::seconds timeout
 	REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
 }
 
+void AutopilotTester::move_mission_raw_here(std::vector<MissionRaw::MissionItem> &mission_items)
+{
+	const auto position = _telemetry->position();
+	REQUIRE(std::isfinite(position.latitude_deg));
+	REQUIRE(std::isfinite(position.longitude_deg));
+
+	auto offset_x = mission_items[0].x - static_cast<int32_t>(1e7 * position.latitude_deg);
+	auto offset_y = mission_items[1].y - static_cast<int32_t>(1e7 * position.longitude_deg);
+
+	for (auto &item : mission_items) {
+		if (item.frame == 3) { // MAV_FRAME_GLOBAL_RELATIVE_ALT
+			item.x -= offset_x;
+		}
+
+		item.y -= offset_y;
+	}
+}
+
 void AutopilotTester::report_speed_factor()
 {
 	// We check the exit flag more often than the speed factor.

test/mavsdk_tests/autopilot_tester.h

@@ -111,7 +111,7 @@ public:
 	void execute_mission_and_get_mag_stuck();
 	void execute_mission_and_lose_baro();
 	void execute_mission_and_get_baro_stuck();
-	void load_qgc_mission_raw(const std::string &plan_file);
+	void load_qgc_mission_raw_and_move_here(const std::string &plan_file);
 	void execute_mission_raw();
 	void execute_rtl();
 	void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f,
@@ -140,6 +140,7 @@ private:
 	void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout);
 	void wait_for_mission_finished(std::chrono::seconds timeout);
 	void wait_for_mission_raw_finished(std::chrono::seconds timeout);
+	void move_mission_raw_here(std::vector<mavsdk::MissionRaw::MissionItem> &mission_items);
 
 	void report_speed_factor();
 

test/mavsdk_tests/test_vtol_mission.cpp

@@ -38,8 +38,8 @@ TEST_CASE("Fly VTOL mission", "[vtol]")
 {
 	AutopilotTester tester;
 	tester.connect(connection_url);
-	tester.load_qgc_mission_raw("test/mavsdk_tests/vtol_mission_allmend.plan");
 	tester.wait_until_ready();
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_allmend.plan");
 	tester.arm();
 	tester.execute_mission_raw();
 	tester.wait_until_disarmed();