Files
PX4-Autopilot/test/mavsdk_tests/autopilot_tester_rtl.cpp
T

452 lines
16 KiB
C++

/****************************************************************************
*
* 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 <cmath>
#include <iostream>
#include <future>
#include <thread>
#include <unistd.h>
#include <mavsdk/plugins/telemetry/telemetry.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_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_rally_points()
{
REQUIRE(getMissionRaw()->upload_rally_points(_rally_points) == MissionRaw::Result::Success);
}
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)
{
_local_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
mavsdk::MissionRaw::MissionItem 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>(_rally_points.size());
tmp_mission_item.command = MAV_CMD_NAV_RALLY_POINT;
tmp_mission_item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
// FIXME: this is currently required with MAVSDK even though it doesn't make much sense for rally points
tmp_mission_item.current = _rally_points.empty() ? 1 : 0;
tmp_mission_item.autocontinue = 0;
tmp_mission_item.mission_type = MAV_MISSION_TYPE_RALLY;
_rally_points.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));
mavsdk::MissionRaw::MissionItem 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>(_rally_points.size());
tmp_mission_item.command = MAV_CMD_NAV_LOITER_TO_ALT;
tmp_mission_item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
// FIXME: this is currently required with MAVSDK even though it doesn't make much sense for rally points
tmp_mission_item.current = _rally_points.empty() ? 1 : 0;
tmp_mission_item.autocontinue = 0;
tmp_mission_item.mission_type = MAV_MISSION_TYPE_RALLY;
_rally_points.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>(_rally_points.size());
_rally_points.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 : _local_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::wait_until_disarmed_while_in_multicopter_mode(std::chrono::seconds timeout)
{
const uint64_t start_time_us = getTelemetry()->attitude_quaternion().timestamp_us;
const uint64_t timeout_us = std::chrono::duration_cast<std::chrono::microseconds>(timeout).count();
bool remained_in_multicopter_mode = true;
while (getTelemetry()->armed() && remained_in_multicopter_mode
&& getTelemetry()->attitude_quaternion().timestamp_us - start_time_us < timeout_us) {
remained_in_multicopter_mode &= getTelemetry()->vtol_state() == Telemetry::VtolState::Mc;
sleep_for(std::chrono::milliseconds(100));
}
REQUIRE(remained_in_multicopter_mode);
REQUIRE_FALSE(getTelemetry()->armed());
}
void AutopilotTesterRtl::load_qgc_mission_and_geofence_here(const std::string &plan_file)
{
auto import_result = getMissionRaw()->import_qgroundcontrol_mission(plan_file);
REQUIRE(import_result.first == mavsdk::MissionRaw::Result::Success);
auto &data = import_result.second;
REQUIRE(!data.mission_items.empty());
// Same offset semantics as move_mission_raw_here, but applied to mission and geofence
// together so the fences keep their plan-relative positions.
const auto pos = getTelemetry()->position();
REQUIRE(std::isfinite(pos.latitude_deg));
REQUIRE(std::isfinite(pos.longitude_deg));
const int32_t offset_x = data.mission_items[0].x - static_cast<int32_t>(1e7 * pos.latitude_deg);
const int32_t offset_y = data.mission_items[0].y - static_cast<int32_t>(1e7 * pos.longitude_deg);
// Only items in a global position frame carry lat/lon in x/y. For MAV_FRAME_MISSION
// items (e.g. Return, DO_* commands) x/y are generic params p5/p6, so offsetting them
// corrupts the params and the item is rejected by the mission-param validation.
auto is_global_position_frame = [](uint32_t frame) {
switch (frame) {
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_INT:
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
case MAV_FRAME_GLOBAL_TERRAIN_ALT:
case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
return true;
default:
return false;
}
};
auto apply_offset = [&](std::vector<mavsdk::MissionRaw::MissionItem> &items) {
for (auto &item : items) {
if (!is_global_position_frame(item.frame)) {
continue;
}
item.x -= offset_x;
item.y -= offset_y;
}
};
apply_offset(data.mission_items);
apply_offset(data.geofence_items);
REQUIRE(getMissionRaw()->upload_mission(data.mission_items) == mavsdk::MissionRaw::Result::Success);
_geofence_shapes.clear();
if (!data.geofence_items.empty()) {
REQUIRE(getMissionRaw()->upload_geofence(data.geofence_items) == mavsdk::MissionRaw::Result::Success);
// Parse MAVLink fence items into shape records in local NED.
// Polygon vertex items (5001/5002) are emitted as a run with param1 = vertex count;
// circle items (5003/5004) are one-shot with param1 = radius_m.
constexpr uint16_t kCmdPolygonInclusion = 5001;
constexpr uint16_t kCmdPolygonExclusion = 5002;
constexpr uint16_t kCmdCircleInclusion = 5003;
constexpr uint16_t kCmdCircleExclusion = 5004;
const auto ct = get_coordinate_transformation();
auto flush_open_polygon = [this](GeofenceShape & open) {
if (!open.vertices.empty()) {
_geofence_shapes.push_back(std::move(open));
}
open = GeofenceShape{};
};
GeofenceShape open{};
size_t vertex_target = 0;
for (const auto &item : data.geofence_items) {
const double lat = static_cast<double>(item.x) / 1e7;
const double lon = static_cast<double>(item.y) / 1e7;
const auto local = ct.local_from_global({lat, lon});
if (item.command == kCmdPolygonInclusion || item.command == kCmdPolygonExclusion) {
const auto kind = (item.command == kCmdPolygonInclusion)
? GeofenceShape::Kind::PolygonInclusion
: GeofenceShape::Kind::PolygonExclusion;
// Start a new polygon when kind changes or the previous one is full.
if (open.vertices.empty() || open.kind != kind
|| open.vertices.size() >= vertex_target) {
flush_open_polygon(open);
open.kind = kind;
vertex_target = static_cast<size_t>(item.param1);
}
open.vertices.push_back(local);
} else if (item.command == kCmdCircleInclusion || item.command == kCmdCircleExclusion) {
flush_open_polygon(open);
GeofenceShape circle{};
circle.kind = (item.command == kCmdCircleInclusion)
? GeofenceShape::Kind::CircleInclusion
: GeofenceShape::Kind::CircleExclusion;
circle.center = local;
circle.radius_m = static_cast<double>(item.param1);
_geofence_shapes.push_back(std::move(circle));
}
}
flush_open_polygon(open);
}
sleep_for(std::chrono::seconds(1));
}
void AutopilotTesterRtl::start_monitoring_geofence_breach()
{
REQUIRE(!_geofence_shapes.empty()); // load_qgc_mission_and_geofence_here() must run first
REQUIRE(!_geofence_monitor_active);
auto ct = get_coordinate_transformation();
_geofence_breached.store(false);
_geofence_monitor_active.store(true);
_geofence_monitor_handle = getTelemetry()->subscribe_ground_truth(
[this, ct](Telemetry::GroundTruth gt) {
if (!_geofence_monitor_active.load()) {
return;
}
if (std::isnan(gt.latitude_deg) || std::isnan(gt.longitude_deg)) {
return;
}
const auto local = ct.local_from_global({gt.latitude_deg, gt.longitude_deg});
for (size_t i = 0; i < _geofence_shapes.size(); ++i) {
if (point_breaches_shape(local.north_m, local.east_m, _geofence_shapes[i])) {
if (!_geofence_breached.exchange(true)) {
std::cout << time_str() << "GEOFENCE BREACH (shape #" << i
<< ") at NED (" << local.north_m << ", "
<< local.east_m << ")\n";
}
break;
}
}
});
}
void AutopilotTesterRtl::check_no_geofence_breach()
{
if (_geofence_monitor_active.exchange(false)) {
getTelemetry()->unsubscribe_ground_truth(_geofence_monitor_handle);
}
CHECK(!_geofence_breached.load());
}
bool AutopilotTesterRtl::point_in_polygon_local(
double north_m, double east_m,
const std::vector<mavsdk::geometry::CoordinateTransformation::LocalCoordinate> &poly)
{
bool inside = false;
const size_t n = poly.size();
for (size_t i = 0, j = n - 1; i < n; j = i++) {
const double ni = poly[i].north_m;
const double ei = poly[i].east_m;
const double nj = poly[j].north_m;
const double ej = poly[j].east_m;
const bool crosses = ((ei > east_m) != (ej > east_m))
&& (north_m < (nj - ni) * (east_m - ei) / (ej - ei) + ni);
if (crosses) {
inside = !inside;
}
}
return inside;
}
bool AutopilotTesterRtl::point_breaches_shape(double north_m, double east_m, const GeofenceShape &shape)
{
switch (shape.kind) {
case GeofenceShape::Kind::PolygonExclusion:
return shape.vertices.size() >= 3
&& point_in_polygon_local(north_m, east_m, shape.vertices);
case GeofenceShape::Kind::PolygonInclusion:
return shape.vertices.size() >= 3
&& !point_in_polygon_local(north_m, east_m, shape.vertices);
case GeofenceShape::Kind::CircleExclusion: {
const double dn = north_m - shape.center.north_m;
const double de = east_m - shape.center.east_m;
return std::sqrt(dn * dn + de * de) < shape.radius_m;
}
case GeofenceShape::Kind::CircleInclusion: {
const double dn = north_m - shape.center.north_m;
const double de = east_m - shape.center.east_m;
return std::sqrt(dn * dn + de * de) > shape.radius_m;
}
}
return false;
}
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);
Telemetry::PositionVelocityNedHandle handle = getTelemetry()->subscribe_position_velocity_ned(
[&prom, &handle, 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 : _rally_points) {
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()->unsubscribe_position_velocity_ned(handle);
prom.set_value(true);
return;
}
}
}
}
if (!on_approach_loiter) {
getTelemetry()->unsubscribe_position_velocity_ned(handle);
prom.set_value(false);
}
}
});
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
REQUIRE(fut.get());
}