From 4020dd004b7e9148131e5a2fb78ae9ff5fee8a2e Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 18 Jan 2023 11:34:56 +0300 Subject: [PATCH] [RTL]: Add CI testing for different RTL modes. added a vtol mission without a landing at the end (used for reversed RTL) Note: reverse mission tracking check is disabled, due to potential bug in mavsdk Signed-off-by: RomanBapst --- test/mavsdk_tests/CMakeLists.txt | 1 + test/mavsdk_tests/autopilot_tester.cpp | 84 +++++++- test/mavsdk_tests/autopilot_tester.h | 5 +- test/mavsdk_tests/autopilot_tester_rtl.cpp | 57 ++++++ test/mavsdk_tests/autopilot_tester_rtl.h | 55 ++++++ test/mavsdk_tests/math_helpers.h | 14 ++ test/mavsdk_tests/test_vtol_mission.cpp | 47 ++++- .../vtol_mission_without_landing.plan | 184 ++++++++++++++++++ 8 files changed, 437 insertions(+), 10 deletions(-) create mode 100644 test/mavsdk_tests/autopilot_tester_rtl.cpp create mode 100644 test/mavsdk_tests/autopilot_tester_rtl.h create mode 100644 test/mavsdk_tests/vtol_mission_without_landing.plan diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 90b27f943aac..3b4be475e3b7 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -17,6 +17,7 @@ if(MAVSDK_FOUND) test_main.cpp autopilot_tester.cpp autopilot_tester_failure.cpp + autopilot_tester_rtl.cpp # follow-me needs a MAVSDK update: # https://github.com/mavlink/MAVSDK/pull/1770 # autopilot_tester_follow_me.cpp diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index c58dd28bf63d..104eaa9e0569 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -268,7 +268,7 @@ void AutopilotTester::execute_mission_and_lose_gps() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorGps, Failure::FailureType::Off, 0) == Failure::Result::Success); @@ -280,7 +280,7 @@ void AutopilotTester::execute_mission_and_lose_mag() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Off, 0) == Failure::Result::Success); @@ -296,7 +296,7 @@ void AutopilotTester::execute_mission_and_lose_baro() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Off, 0) == Failure::Result::Success); @@ -312,7 +312,7 @@ void AutopilotTester::execute_mission_and_get_baro_stuck() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck, 0) == Failure::Result::Success); @@ -328,7 +328,7 @@ void AutopilotTester::execute_mission_and_get_mag_stuck() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck, 0) == Failure::Result::Success); @@ -501,6 +501,49 @@ void AutopilotTester::stop_checking_altitude() _telemetry->subscribe_position(nullptr); } +void AutopilotTester::check_tracks_mission_raw(float corridor_radius_m, bool reverse) +{ + auto mission_raw = _mission_raw->download_mission(); + CHECK(mission_raw.first == MissionRaw::Result::Success); + + auto mission_items = mission_raw.second; + auto ct = get_coordinate_transformation(); + + _telemetry->set_rate_position_velocity_ned(5); + _telemetry->subscribe_position_velocity_ned([ct, mission_items, corridor_radius_m, reverse, + this](Telemetry::PositionVelocityNed position_velocity_ned) { + auto progress = _mission_raw->mission_progress(); + + + std::function(std::vector, unsigned, mavsdk::geometry::CoordinateTransformation)> + get_waypoint_for_sequence = [](std::vector mission_items, int sequence, auto ct) { + for (auto waypoint : mission_items) { + + if (waypoint.seq == (uint32_t)sequence) { + return get_local_mission_item_from_raw_item(waypoint, ct); + } + } + + return std::array({0.0f, 0.0f, 0.0f}); + }; + + if (progress.current > 0 && progress.current < progress.total) { + // Get shortest distance of current position to 3D line between previous and next waypoint + + std::array current { position_velocity_ned.position.north_m, + position_velocity_ned.position.east_m, + position_velocity_ned.position.down_m }; + std::array wp_prev = get_waypoint_for_sequence(mission_items, + reverse ? progress.current + 1 : progress.current - 1, ct); + std::array wp_next = get_waypoint_for_sequence(mission_items, progress.current, ct); + + float distance_to_trajectory = point_to_line_distance(current, wp_prev, wp_next); + + CHECK(distance_to_trajectory < corridor_radius_m); + } + }); +} + void AutopilotTester::check_tracks_mission(float corridor_radius_m) { auto mission = _mission->download_mission(); @@ -535,6 +578,12 @@ void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m); } +void AutopilotTester::execute_rtl_when_reaching_mission_sequence(int sequence_number) +{ + start_and_wait_for_mission_sequence_raw(sequence_number); + execute_rtl(); +} + std::array AutopilotTester::get_current_position_ned() { mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned(); @@ -642,15 +691,15 @@ bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry: return pass; } -void AutopilotTester::start_and_wait_for_first_mission_item() +void AutopilotTester::start_and_wait_for_mission_sequence(int sequence_number) { auto prom = std::promise {}; auto fut = prom.get_future(); - _mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) { + _mission->subscribe_mission_progress([&prom, this, sequence_number](Mission::MissionProgress progress) { std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; - if (progress.current >= 1) { + if (progress.current >= sequence_number) { _mission->subscribe_mission_progress(nullptr); prom.set_value(); } @@ -661,6 +710,25 @@ void AutopilotTester::start_and_wait_for_first_mission_item() REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready); } +void AutopilotTester::start_and_wait_for_mission_sequence_raw(int sequence_number) +{ + auto prom = std::promise {}; + auto fut = prom.get_future(); + + _mission_raw->subscribe_mission_progress([&prom, this, sequence_number](MissionRaw::MissionProgress progress) { + std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; + + if (progress.current >= sequence_number) { + _mission_raw->subscribe_mission_progress(nullptr); + prom.set_value(); + } + }); + + REQUIRE(_mission_raw->start_mission() == MissionRaw::Result::Success); + + REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready); +} + void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout) { auto prom = std::promise {}; diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index bb4b826c0ee2..dbc77b8c2b43 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -137,9 +137,11 @@ class AutopilotTester void request_ground_truth(); 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 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); + void execute_rtl_when_reaching_mission_sequence(int sequence_number); // Blocking call to get the drone's current position in NED frame std::array get_current_position_ned(); @@ -199,7 +201,8 @@ class AutopilotTester 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); - void start_and_wait_for_first_mission_item(); + void start_and_wait_for_mission_sequence(int sequence_number); + void start_and_wait_for_mission_sequence_raw(int sequence_number); void wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout); void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout); void wait_for_mission_finished(std::chrono::seconds timeout); diff --git a/test/mavsdk_tests/autopilot_tester_rtl.cpp b/test/mavsdk_tests/autopilot_tester_rtl.cpp new file mode 100644 index 000000000000..b3996a76c437 --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_rtl.cpp @@ -0,0 +1,57 @@ + +/**************************************************************************** + * + * 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" + +#include "math_helpers.h" +#include +#include +#include +#include + + +void AutopilotTesterRtl::connect(const std::string uri) +{ + AutopilotTester::connect(uri); +} + +void AutopilotTesterRtl::set_rtl_type(int rtl_type) +{ + CHECK(getParams()->set_param_int("RTL_TYPE", rtl_type) == Param::Result::Success); +} + +void AutopilotTesterRtl::set_takeoff_land_requirements(int req) +{ + CHECK(getParams()->set_param_int("MIS_TKO_LAND_REQ", req) == Param::Result::Success); +} diff --git a/test/mavsdk_tests/autopilot_tester_rtl.h b/test/mavsdk_tests/autopilot_tester_rtl.h new file mode 100644 index 000000000000..6c59d6446281 --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_rtl.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 +#include + + +class AutopilotTesterRtl : public AutopilotTester +{ +public: + AutopilotTesterRtl() = default; + ~AutopilotTesterRtl() = default; + + void set_rtl_type(int rtl_type); + void set_takeoff_land_requirements(int req); + void connect(const std::string uri); + + +private: + std::unique_ptr _failure{}; +}; diff --git a/test/mavsdk_tests/math_helpers.h b/test/mavsdk_tests/math_helpers.h index 34817381efef..3f26d3aa1877 100644 --- a/test/mavsdk_tests/math_helpers.h +++ b/test/mavsdk_tests/math_helpers.h @@ -44,6 +44,20 @@ std::array get_local_mission_item(const Mission::MissionItem &item, const return {static_cast(local.north_m), static_cast(local.east_m), -item.relative_altitude_m}; } +template +std::array get_local_mission_item_from_raw_item(const mavsdk::MissionRaw::MissionItem &item, + const CoordinateTransformation &ct) +{ + using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate; + GlobalCoordinate global; + global.latitude_deg = item.x / 1e7; + global.longitude_deg = item.y / 1e7; + + + auto local = ct.local_from_global(global); + return {static_cast(local.north_m), static_cast(local.east_m), -item.z}; +} + template T sq(T x) { diff --git a/test/mavsdk_tests/test_vtol_mission.cpp b/test/mavsdk_tests/test_vtol_mission.cpp index 5417254fce8b..20386bfdd123 100644 --- a/test/mavsdk_tests/test_vtol_mission.cpp +++ b/test/mavsdk_tests/test_vtol_mission.cpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -#include "autopilot_tester.h" +#include "autopilot_tester_rtl.h" TEST_CASE("Fly VTOL mission", "[vtol]") @@ -44,3 +44,48 @@ 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(30.0f); + tester.wait_until_disarmed(std::chrono::seconds(90)); +} + +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 + tester.set_rtl_type(2); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(6); + /* this check is currently disabled since there seems to be a bug in mavsdk which causes the reported + mission sequence to be out of order */ + tester.check_tracks_mission_raw(300.0f); + tester.wait_until_disarmed(std::chrono::seconds(90)); +} diff --git a/test/mavsdk_tests/vtol_mission_without_landing.plan b/test/mavsdk_tests/vtol_mission_without_landing.plan new file mode 100644 index 000000000000..e52e3f6fa4a8 --- /dev/null +++ b/test/mavsdk_tests/vtol_mission_without_landing.plan @@ -0,0 +1,184 @@ +{ + "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": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.397202447861446, + 8.545440338322464, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 20, + "Altitude": 20, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39766309343905, + 8.545713820298545, + 20 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.39775218584113, + 8.545620889782981, + 489.0021493051957 + ], + "vehicleType": 20, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 +}