Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2025-12-07 18:59:29 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

test_vtol_mission: Add additional testing for VTOL RTL and reverse mission RTL

Browse Source 
mavsdk-test: added a vtol mission without a landing at the end (used for reversed RTL)
math_helpers: added function to compute local position from a raw mission item

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2023-01-18 11:34:56 +03:00
committed by Silvan Fuhrer
parent 9a48d375ce
commit 6fca984c3f
8 changed files with 436 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
test/mavsdk_tests/CMakeLists.txt
View File

@@ -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

84
test/mavsdk_tests/autopilot_tester.cpp
View File

@@ -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);
@@ -560,6 +560,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::array<float, 3>(std::vector<mavsdk::MissionRaw::MissionItem>, unsigned, mavsdk::geometry::CoordinateTransformation)>
get_waypoint_for_sequence = [](std::vector<mavsdk::MissionRaw::MissionItem> 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<float>(waypoint, ct);
}
}
return std::array<float, 3>({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<float, 3> current { position_velocity_ned.position.north_m,
position_velocity_ned.position.east_m,
position_velocity_ned.position.down_m };
std::array<float, 3> wp_prev = get_waypoint_for_sequence(mission_items,
reverse ? progress.current + 1 : progress.current - 1, ct);
std::array<float, 3> 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();
@@ -594,6 +637,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<float, 3> AutopilotTester::get_current_position_ned()
{
mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
@@ -701,15 +750,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<void> {};
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();
}
@@ -720,6 +769,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<void> {};
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<void> {};

5
test/mavsdk_tests/autopilot_tester.h
View File

@@ -138,9 +138,11 @@ public:
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<float, 3> get_current_position_ned();
@@ -200,7 +202,8 @@ private:
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);

57
test/mavsdk_tests/autopilot_tester_rtl.cpp Normal file
View File

@@ -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 <iostream>
#include <future>
#include <thread>
#include <unistd.h>
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);
}

55
test/mavsdk_tests/autopilot_tester_rtl.h Normal file
View File

@@ -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 <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
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<mavsdk::Failure> _failure{};
};

14
test/mavsdk_tests/math_helpers.h
View File

@@ -44,6 +44,20 @@ std::array<T, 3> get_local_mission_item(const Mission::MissionItem &item, const
return {static_cast<T>(local.north_m), static_cast<T>(local.east_m), -item.relative_altitude_m};
}
template<typename T>
std::array<T, 3> 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<T>(local.north_m), static_cast<T>(local.east_m), -item.z};
}
template<typename T>
T sq(T x)
{

46
test/mavsdk_tests/test_vtol_mission.cpp
View File

@@ -31,7 +31,7 @@
*
****************************************************************************/
#include "autopilot_tester.h"
#include "autopilot_tester_rtl.h"
TEST_CASE("Fly VTOL mission", "[vtol]")
@@ -44,3 +44,47 @@ 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);
tester.check_tracks_mission_raw(30.0f);
tester.wait_until_disarmed(std::chrono::seconds(90));
}

184
test/mavsdk_tests/vtol_mission_without_landing.plan Normal file
View File

@@ -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
}