dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavsdk: Add integration tests for RTL with approaches

This commit is contained in:
Konrad 2023-10-23 09:34:26 +02:00 committed by Roman Bapst
parent 24f59dd465
commit 654e885003
8 changed files with 676 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
test/mavsdk_tests/CMakeLists.txt
View File

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

16
test/mavsdk_tests/autopilot_tester.cpp
View File

@ -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<double>(land_mission_item.x) / 1E7;
land_coord.longitude_deg = static_cast<double>(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();

3
test/mavsdk_tests/autopilot_tester.h
View File

@ -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<System> 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);

207
test/mavsdk_tests/autopilot_tester_rtl.cpp
View File

@ -35,11 +35,14 @@
#include "autopilot_tester_rtl.h"
#include "math_helpers.h"
#include <cmath>
#include <iostream>
#include <future>
#include <thread>
#include <unistd.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
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<void> 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<int32_t>(pos.latitude_deg * 1E7);
tmp_mission_item.y = static_cast<int32_t>(pos.longitude_deg * 1E7);
tmp_mission_item.z = 0.f;
tmp_mission_item.seq = static_cast<uint16_t>(_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<int32_t>(pos.latitude_deg * 1E7);
tmp_mission_item.y = static_cast<int32_t>(pos.longitude_deg * 1E7);
tmp_mission_item.z = 15.f;
tmp_mission_item.seq = static_cast<uint16_t>(_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<int32_t>(pos.latitude_deg * 1E7);
tmp_mission_item.y = static_cast<int32_t>(pos.longitude_deg * 1E7);
tmp_mission_item.seq = static_cast<uint16_t>(_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<bool> {};
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<double>(mission_item.x) / 1E7, static_cast<double>(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());
}

18
test/mavsdk_tests/autopilot_tester_rtl.h
View File

@ -35,7 +35,10 @@
#include "autopilot_tester.h"
#include <vector>
#include <mavsdk/mavsdk.h>
#include <mavsdk/geometry.h>
#include <mavsdk/plugins/action/action.h>
@ -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<mavsdk::Failure> _failure{};
std::vector<mavlink_mission_item_int_t> _custom_mission{};
std::vector<mavsdk::geometry::CoordinateTransformation::LocalCoordinate> _rally_points{};
};

46
test/mavsdk_tests/test_vtol_mission.cpp
View File

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

228
test/mavsdk_tests/test_vtol_rtl.cpp Normal file
View File

@ -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.);
}

203
test/mavsdk_tests/vtol_mission_with_land_start.plan Normal file
View File

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