From 654e885003c54ad8f66aee2c0709f3130519491b Mon Sep 17 00:00:00 2001 From: Konrad Date: Mon, 23 Oct 2023 09:34:26 +0200 Subject: [PATCH] mavsdk: Add integration tests for RTL with approaches --- test/mavsdk_tests/CMakeLists.txt | 1 + test/mavsdk_tests/autopilot_tester.cpp | 16 ++ test/mavsdk_tests/autopilot_tester.h | 3 +- test/mavsdk_tests/autopilot_tester_rtl.cpp | 207 ++++++++++++++++ test/mavsdk_tests/autopilot_tester_rtl.h | 18 ++ test/mavsdk_tests/test_vtol_mission.cpp | 46 +--- test/mavsdk_tests/test_vtol_rtl.cpp | 228 ++++++++++++++++++ .../vtol_mission_with_land_start.plan | 203 ++++++++++++++++ 8 files changed, 676 insertions(+), 46 deletions(-) create mode 100644 test/mavsdk_tests/test_vtol_rtl.cpp create mode 100644 test/mavsdk_tests/vtol_mission_with_land_start.plan diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index ab115ce187..f71bd3b24f 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/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 ) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 305842c3b4..f5ec5994ca 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/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(land_mission_item.x) / 1E7; + land_coord.longitude_deg = static_cast(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(); diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 3e2ebaae72..3c4c8ab798 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -141,6 +141,7 @@ public: 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 @@ protected: MavlinkPassthrough *getMavlinkPassthrough() const { return _mavlink_passthrough.get();} std::shared_ptr 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 @@ private: 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); diff --git a/test/mavsdk_tests/autopilot_tester_rtl.cpp b/test/mavsdk_tests/autopilot_tester_rtl.cpp index b3996a76c4..d24b59e90d 100644 --- a/test/mavsdk_tests/autopilot_tester_rtl.cpp +++ b/test/mavsdk_tests/autopilot_tester_rtl.cpp @@ -35,11 +35,14 @@ #include "autopilot_tester_rtl.h" #include "math_helpers.h" +#include #include #include #include #include +#include + 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 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(pos.latitude_deg * 1E7); + tmp_mission_item.y = static_cast(pos.longitude_deg * 1E7); + tmp_mission_item.z = 0.f; + tmp_mission_item.seq = static_cast(_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(pos.latitude_deg * 1E7); + tmp_mission_item.y = static_cast(pos.longitude_deg * 1E7); + tmp_mission_item.z = 15.f; + tmp_mission_item.seq = static_cast(_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(pos.latitude_deg * 1E7); + tmp_mission_item.y = static_cast(pos.longitude_deg * 1E7); + tmp_mission_item.seq = static_cast(_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 {}; + 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(mission_item.x) / 1E7, static_cast(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()); +} diff --git a/test/mavsdk_tests/autopilot_tester_rtl.h b/test/mavsdk_tests/autopilot_tester_rtl.h index 6c59d64462..eaa3c5fd9d 100644 --- a/test/mavsdk_tests/autopilot_tester_rtl.h +++ b/test/mavsdk_tests/autopilot_tester_rtl.h @@ -35,7 +35,10 @@ #include "autopilot_tester.h" +#include + #include +#include #include @@ -46,10 +49,25 @@ public: ~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 _failure{}; + std::vector _custom_mission{}; + std::vector _rally_points{}; }; diff --git a/test/mavsdk_tests/test_vtol_mission.cpp b/test/mavsdk_tests/test_vtol_mission.cpp index e12f583c2c..5417254fce 100644 --- a/test/mavsdk_tests/test_vtol_mission.cpp +++ b/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)); -} diff --git a/test/mavsdk_tests/test_vtol_rtl.cpp b/test/mavsdk_tests/test_vtol_rtl.cpp new file mode 100644 index 0000000000..67238e6214 --- /dev/null +++ b/test/mavsdk_tests/test_vtol_rtl.cpp @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * 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_rtl.h" + +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_with_land_start.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 direct Mission Land", "[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_with_land_start.plan"); + // Do not allow home + tester.set_rtl_type(1); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(2); + tester.wait_until_disarmed(std::chrono::seconds(90)); + tester.check_mission_land_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)); +} + +TEST_CASE("RTL direct home without approaches", "[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_with_land_start.plan"); + // fly directly to home position + tester.set_rtl_type(0); + tester.set_rtl_appr_force(0); + // reupload rally points with approaches + tester.add_home_to_rally_point(); + tester.upload_custom_mission(std::chrono::seconds(10)); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(4); + tester.wait_until_disarmed(std::chrono::seconds(150)); + tester.check_home_within(5.0f); +} + +TEST_CASE("RTL direct home without approaches forced", "[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_with_land_start.plan"); + // fly directly to home position + tester.set_rtl_type(0); + tester.set_rtl_appr_force(1); + // reupload rally points with approaches + tester.add_home_to_rally_point(); + tester.upload_custom_mission(std::chrono::seconds(10)); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(4); + tester.wait_until_disarmed(std::chrono::seconds(150)); + tester.check_mission_land_within(5.f); +} + +TEST_CASE("RTL direct home with approaches", "[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_with_land_start.plan"); + // fly directly to home position + tester.set_rtl_type(0); + // reupload rally points with approaches + tester.add_home_with_approaches_to_rally_point(); + tester.upload_custom_mission(std::chrono::seconds(10)); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(4); + tester.check_rtl_approaches(5., std::chrono::seconds(60)); + tester.wait_until_disarmed(std::chrono::seconds(150)); + tester.check_home_within(5.0f); +} + +TEST_CASE("RTL direct home not as rally point", "[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_with_land_start.plan"); + // fly directly to home position + tester.set_rtl_type(1); + // reupload rally points with approaches + tester.add_home_with_approaches_to_rally_point(); + tester.upload_custom_mission(std::chrono::seconds(10)); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(2); + tester.wait_until_disarmed(std::chrono::seconds(150)); + tester.check_mission_land_within(5.0f); +} + +TEST_CASE("RTL direct rally without approaches", "[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_with_land_start.plan"); + // Do not allow home position + tester.set_rtl_type(1); + tester.set_rtl_appr_force(0); + // reupload rally points with approaches + tester.add_local_rally_point({100., -200.}); + tester.upload_custom_mission(std::chrono::seconds(10)); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(3); + tester.wait_until_disarmed(std::chrono::seconds(150)); + tester.check_rally_point_within(5.0f); + tester.check_home_not_within(20.); +} + +TEST_CASE("RTL direct rally without approaches forced", "[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_with_land_start.plan"); + // Do not allow home position + tester.set_rtl_type(1); + tester.set_rtl_appr_force(1); + // reupload rally points with approaches + tester.add_local_rally_point({100., -2000.}); + tester.upload_custom_mission(std::chrono::seconds(10)); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(3); + tester.wait_until_disarmed(std::chrono::seconds(150)); + tester.check_mission_land_within(5.f); +} + +TEST_CASE("RTL direct rally with approaches", "[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_with_land_start.plan"); + // Do not allow home position + tester.set_rtl_type(1); + tester.set_rtl_appr_force(0); + // reupload rally points with approaches + tester.add_local_rally_with_approaches_point({100., -200.}); + tester.upload_custom_mission(std::chrono::seconds(10)); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(3); + tester.check_rtl_approaches(5., std::chrono::seconds(60)); + tester.wait_until_disarmed(std::chrono::seconds(150)); + tester.check_rally_point_within(5.0f); + tester.check_home_not_within(20.); +} diff --git a/test/mavsdk_tests/vtol_mission_with_land_start.plan b/test/mavsdk_tests/vtol_mission_with_land_start.plan new file mode 100644 index 0000000000..4438190d1e --- /dev/null +++ b/test/mavsdk_tests/vtol_mission_with_land_start.plan @@ -0,0 +1,203 @@ +{ + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 12, + "globalPlanAltitudeMode": 1, + "hoverSpeed": 5, + "items": [ + { + "AMSLAltAboveTerrain": 20, + "Altitude": 20, + "AltitudeMode": 1, + "autoContinue": true, + "command": 84, + "doJumpId": 1, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39833113265167, + 8.545508725338607, + 20 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 20, + "Altitude": 20, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.399332700068925, + 8.54481499384454, + 20 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39908888031702, + 8.54344001880591, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39760160279815, + 8.542394178137585, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.396941861414504, + 8.54282818797708, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.396686401111786, + 8.544419333554089, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 15, + "Altitude": 15, + "AltitudeMode": 1, + "autoContinue": true, + "command": 189, + "doJumpId": 7, + "frame": 2, + "params": [ + 1, + -40, + 1, + null, + 47.39697211349564, + 8.545053363814125, + 15 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 15, + "Altitude": 15, + "AltitudeMode": 1, + "autoContinue": true, + "command": 31, + "doJumpId": 8, + "frame": 3, + "params": [ + 1, + -40, + 1, + null, + 47.39697211349564, + 8.545053363814125, + 15 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 15, + "Altitude": 15, + "AltitudeMode": 1, + "autoContinue": true, + "command": 85, + "doJumpId": 9, + "frame": 3, + "params": [ + 0, + 0, + 15, + null, + 47.3976926, + 8.54600971, + 0 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.39775218584113, + 8.545620889782981, + 489.0021493051957 + ], + "vehicleType": 20, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 +}