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

Rally Points in RTL: fly to closest Rally Point (safe point), or to home if that's closer 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2019-08-14 10:07:09 +02:00 committed by Beat Küng
parent 99dd229d71
commit efafd91095
3 changed files with 139 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

163
src/modules/navigator/rtl.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2019 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
@ -41,6 +41,7 @@
#include "rtl.h"
#include "navigator.h"
#include <dataman/dataman.h>
static constexpr float DELAY_SIGMA = 0.01f;
@ -68,6 +69,79 @@ void
RTL::on_activation()
{
// find the RTL destination: go through the safe points & home position
const home_position_s &home_position = *_navigator->get_home_position();
const vehicle_global_position_s &global_position = *_navigator->get_global_position();
mission_stats_entry_s stats;
int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
int num_safe_points = 0;
if (ret == sizeof(mission_stats_entry_s)) {
num_safe_points = stats.num_items;
}
int closest_index = 0;
mission_safe_point_s closest_safe_point;
// take home position into account
double dlat = home_position.lat - global_position.lat;
double dlon = home_position.lon - global_position.lon;
double min_dist_squared = dlat * dlat + dlon * dlon;
// find the closest point
for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) {
mission_safe_point_s mission_safe_point;
if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) !=
sizeof(mission_safe_point_s)) {
PX4_ERR("dm_read failed");
continue;
}
// TODO: take altitude into account for distance measurement
dlat = mission_safe_point.lat - global_position.lat;
dlon = mission_safe_point.lon - global_position.lon;
double dist_squared = dlat * dlat + dlon * dlon;
if (dist_squared < min_dist_squared) {
closest_index = current_seq;
min_dist_squared = dist_squared;
closest_safe_point = mission_safe_point;
}
}
if (closest_index == 0) {
_destination.set(home_position);
} else {
// TODO: handle all possible mission_safe_point.frame cases
switch (closest_safe_point.frame) {
case 0: // MAV_FRAME_GLOBAL
_destination.safe_point_index = closest_index;
_destination.lat = closest_safe_point.lat;
_destination.lon = closest_safe_point.lon;
_destination.alt = closest_safe_point.alt;
_destination.yaw = home_position.yaw;
break;
case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT
_destination.safe_point_index = closest_index;
_destination.lat = closest_safe_point.lat;
_destination.lon = closest_safe_point.lon;
_destination.alt = closest_safe_point.alt + home_position.alt; // alt of safe point is rel to home
_destination.yaw = home_position.yaw;
break;
default:
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME. Landing at HOME.");
_destination.set(home_position);
break;
}
}
_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
if (_navigator->get_land_detected()->landed) {
@ -77,8 +151,7 @@ RTL::on_activation()
} else if ((rtl_type() == RTL_LAND) && _navigator->on_mission_landing()) {
// RTL straight to RETURN state, but mission will takeover for landing.
} else if ((_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_rtl_return_alt.get())
|| _rtl_alt_min) {
} else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) {
// If lower than return altitude, climb up first.
// If rtl_alt_min is true then forcing altitude change even if above.
@ -111,7 +184,7 @@ void
RTL::set_rtl_item()
{
// RTL_TYPE: mission landing.
// Landing using planned mission landing, fly to DO_LAND_START instead of returning HOME.
// Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination.
// Do nothing, let navigator takeover with mission landing.
if (rtl_type() == RTL_LAND) {
if (_rtl_state > RTL_STATE_CLIMB) {
@ -128,16 +201,15 @@ RTL::set_rtl_item()
_navigator->set_can_loiter_at_sp(false);
const home_position_s &home = *_navigator->get_home_position();
const vehicle_global_position_s &gpos = *_navigator->get_global_position();
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// Check if we are pretty close to home already.
const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
// Check if we are pretty close to the destination already.
const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
// Compute the loiter altitude.
const float loiter_altitude = math::min(home.alt + _param_rtl_descend_alt.get(), gpos.alt);
const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
@ -153,8 +225,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
(int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _navigator->get_home_position()->alt));
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)",
(int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt));
break;
}
@ -162,19 +234,19 @@ RTL::set_rtl_item()
// Don't change altitude.
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = _rtl_alt;
_mission_item.altitude_is_relative = false;
// Use home yaw if close to home.
// Check if we are pretty close to home already.
if (home_dist < _param_rtl_min_dist.get()) {
_mission_item.yaw = home.yaw;
// Use destination yaw if close to _destination.
// Check if we are pretty close to the destination already.
if (destination_dist < _param_rtl_min_dist.get()) {
_mission_item.yaw = _destination.yaw;
} else {
// Use current heading to home.
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, home.lat, home.lon);
// Use current heading to _destination.
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
@ -182,8 +254,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
(int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - home.alt));
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)",
(int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
break;
}
@ -195,8 +267,8 @@ RTL::set_rtl_item()
case RTL_STATE_DESCEND: {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
@ -207,7 +279,7 @@ RTL::set_rtl_item()
_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
} else {
_mission_item.yaw = home.yaw;
_mission_item.yaw = _destination.yaw;
}
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
@ -218,8 +290,8 @@ RTL::set_rtl_item()
// Disable previous setpoint to prevent drift.
pos_sp_triplet->previous.valid = false;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
(int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - home.alt));
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)",
(int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
break;
}
@ -227,11 +299,11 @@ RTL::set_rtl_item()
const bool autoland = (_param_rtl_land_delay.get() > FLT_EPSILON);
// Don't change altitude.
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.altitude = loiter_altitude;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = home.yaw;
_mission_item.yaw = _destination.yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = math::max(_param_rtl_land_delay.get(), 0.0f);
@ -254,19 +326,19 @@ RTL::set_rtl_item()
}
case RTL_STATE_LAND: {
// Land at home position.
// Land at destination.
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.lat = home.lat;
_mission_item.lon = home.lon;
_mission_item.yaw = home.yaw;
_mission_item.altitude = home.alt;
_mission_item.lat = _destination.lat;
_mission_item.lon = _destination.lon;
_mission_item.yaw = _destination.yaw;
_mission_item.altitude = _destination.alt;
_mission_item.altitude_is_relative = false;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination");
break;
}
@ -352,34 +424,33 @@ RTL::advance_rtl()
float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
{
const home_position_s &home = *_navigator->get_home_position();
const vehicle_global_position_s &gpos = *_navigator->get_global_position();
// horizontal distance to home position
const float home_dist = get_distance_to_next_waypoint(home.lat, home.lon, gpos.lat, gpos.lon);
// horizontal distance to destination
const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
float rtl_altitude;
if (home_dist <= _param_rtl_min_dist.get()) {
rtl_altitude = home.alt + _param_rtl_descend_alt.get();
if (destination_dist <= _param_rtl_min_dist.get()) {
rtl_altitude = _destination.alt + _param_rtl_descend_alt.get();
} else if (gpos.alt > home.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) {
} else if (gpos.alt > _destination.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) {
rtl_altitude = gpos.alt;
} else if (cone_half_angle_deg <= 0) {
rtl_altitude = home.alt + _param_rtl_return_alt.get();
rtl_altitude = _destination.alt + _param_rtl_return_alt.get();
} else {
// constrain cone half angle to meaningful values. All other cases are already handled above.
const float cone_half_angle_rad = math::radians(math::constrain(cone_half_angle_deg, 1.0f, 89.0f));
// minimum height above home position required
float height_above_home_min = home_dist / tanf(cone_half_angle_rad);
// minimum height above destination required
float height_above_destination_min = destination_dist / tanf(cone_half_angle_rad);
// minimum altitude we need in order to be within the user defined cone
const float altitude_min = math::constrain(height_above_home_min + home.alt, home.alt,
home.alt + _param_rtl_return_alt.get());
const float altitude_min = math::constrain(height_above_destination_min + _destination.alt, _destination.alt,
_destination.alt + _param_rtl_return_alt.get());
if (gpos.alt < altitude_min) {
rtl_altitude = altitude_min;
@ -390,7 +461,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
}
// always demand altitude which is higher or equal the RTL descend altitude
rtl_altitude = math::max(rtl_altitude, home.alt + _param_rtl_descend_alt.get());
rtl_altitude = math::max(rtl_altitude, _destination.alt + _param_rtl_descend_alt.get());
return rtl_altitude;
}

21
src/modules/navigator/rtl.h
View File

@ -46,6 +46,8 @@
#include "navigator_mode.h"
#include "mission_block.h"
#include <uORB/topics/home_position.h>
class Navigator;
class RTL : public MissionBlock, public ModuleParams
@ -94,6 +96,25 @@ private:
RTL_STATE_LANDED,
} _rtl_state{RTL_STATE_NONE};
struct RTLPosition {
double lat;
double lon;
float alt;
float yaw;
uint8_t safe_point_index; ///< 0 = home position
void set(const home_position_s &home_position)
{
lat = home_position.lat;
lon = home_position.lon;
alt = home_position.alt;
yaw = home_position.yaw;
safe_point_index = 0;
}
};
RTLPosition _destination; ///< the RTL position to fly to (typically the home position or a safe point)
float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position
bool _rtl_alt_min{false};

2
src/modules/navigator/rtl_params.c
View File

@ -97,7 +97,7 @@ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
*
* @unit m
* @min 0.5
* @max 20
* @max 100
* @decimal 1
* @increment 0.5
* @group Return Mode