From cd66a7974704a1df02b6c6e2ad43bd2c536e63cb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Mar 2021 11:06:31 +0100 Subject: [PATCH] mavsdk_tests: move waypoints to vehicle position This way it shouldn't matter where the test is run. --- test/mavsdk_tests/autopilot_tester.cpp | 24 ++++++++++++++++++++++-- test/mavsdk_tests/autopilot_tester.h | 3 ++- test/mavsdk_tests/test_vtol_mission.cpp | 2 +- 3 files changed, 25 insertions(+), 4 deletions(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 82b6a27d27..f52b21ca7f 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/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 &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(1e7 * position.latitude_deg); + auto offset_y = mission_items[1].y - static_cast(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. diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 2f5d1a8832..6da29a3944 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/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 &mission_items); void report_speed_factor(); diff --git a/test/mavsdk_tests/test_vtol_mission.cpp b/test/mavsdk_tests/test_vtol_mission.cpp index 53a15ceb17..2729e8f47a 100644 --- a/test/mavsdk_tests/test_vtol_mission.cpp +++ b/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();