fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

RTL: add new RTL_TYPE to only allow returns to safe points or last link position

Browse Source 
Do not allow RTL to Home or mission landings.

Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan 2026-01-06 11:48:06 +01:00 committed by Silvan Fuhrer
parent 5d5e1db97f
commit 024b3d27ac
3 changed files with 149 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -310,17 +310,71 @@ bool RTL::isLanding()
void RTL::setRtlTypeAndDestination()
{
init_rtl_mission_type();
uint8_t safe_point_index{0U};
uint8_t safe_point_index{UINT8_MAX};
if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) {
if (_param_rtl_type.get() == 5) {
PositionYawSetpoint rtl_position;
findClosestSafePoint(rtl_position, safe_point_index);
DestinationType destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
if (safe_point_index == UINT8_MAX) {
// no safe point found, set destination to last position with valid data link
for (auto &telemetry_status : _telemetry_status_subs) {
telemetry_status_s telemetry;
if (telemetry_status.update(&telemetry)) {
if (telemetry.heartbeat_type_gcs) {
_last_position_before_link_loss.alt = _global_pos_sub.get().alt;
_last_position_before_link_loss.lat = _global_pos_sub.get().lat;
_last_position_before_link_loss.lon = _global_pos_sub.get().lon;
break;
}
}
}
if (!PX4_ISFINITE(_last_position_before_link_loss.lat) || !PX4_ISFINITE(_last_position_before_link_loss.lon)) {
// if we never had a valid data link position, fallback to current position
rtl_position.alt = _global_pos_sub.get().alt;
rtl_position.lat = _global_pos_sub.get().lat;
rtl_position.lon = _global_pos_sub.get().lon;
} else {
rtl_position = _last_position_before_link_loss;
}
destination_type = DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION;
}
// set rtl altitude to the destination from the beginning for DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION
const float rtl_alt = destination_type == DestinationType::DESTINATION_TYPE_SAFE_POINT ? computeReturnAltitude(rtl_position,
(float)_param_rtl_cone_half_angle_deg.get()) : rtl_position.alt;
loiter_point_s landing_loiter;
landing_loiter.lat = rtl_position.lat;
landing_loiter.lon = rtl_position.lon;
landing_loiter.height_m = NAN;
land_approaches_s rtl_land_approaches{readVtolLandApproaches(rtl_position)};
if (_vehicle_status_sub.get().is_vtol
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
&& rtl_land_approaches.isAnyApproachValid()) {
landing_loiter = chooseBestLandingApproach(rtl_land_approaches);
}
_rtl_type = RtlType::RTL_DIRECT;
_rtl_direct.setRtlAlt(rtl_alt);
_rtl_direct.setRtlPosition(rtl_position, landing_loiter);
} else if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) {
// check the closest allowed destination.
DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
PositionYawSetpoint rtl_position;
float rtl_alt;
findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index);
findRtlDestination(destination_type, rtl_position, safe_point_index);
const float rtl_alt = computeReturnAltitude(rtl_position, (float)_param_rtl_cone_half_angle_deg.get());
switch (destination_type) {
case DestinationType::DESTINATION_TYPE_MISSION_LAND:
@ -366,7 +420,48 @@ void RTL::setRtlTypeAndDestination()
}
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
void RTL::findClosestSafePoint(PositionYawSetpoint &rtl_position, uint8_t &safe_point_index)
{
float min_dist = FLT_MAX;
if (_safe_points_updated) {
_one_rally_point_has_land_approach = false;
for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
mission_item_s mission_safe_point;
const bool success = _dataman_cache_safepoint.loadWait(static_cast<dm_item_t>(_stats.dataman_id), current_seq,
reinterpret_cast<uint8_t *>(&mission_safe_point),
sizeof(mission_item_s), 500_ms);
if (!success) {
PX4_ERR("dm_read failed");
continue;
}
if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT) {
const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
PositionYawSetpoint safepoint_position;
setSafepointAsDestination(safepoint_position, mission_safe_point);
const bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)};
_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
if (((dist + MIN_DIST_THRESHOLD) < min_dist) && ((_param_rtl_approach_force.get() == 0)
|| current_safe_point_has_approaches)) {
min_dist = dist;
rtl_position = safepoint_position;
safe_point_index = current_seq;
}
}
}
}
}
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position,
uint8_t &safe_point_index)
{
// set destination to home per default, then check if other valid landing spot is closer
@ -448,7 +543,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
const float dist_to_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon,
mission_safe_point.lat, mission_safe_point.lon);
if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { // remove!
float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
PositionYawSetpoint safepoint_position;
@ -468,14 +563,6 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
}
}
}
if (_param_rtl_cone_half_angle_deg.get() > 0
&& _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
rtl_alt = calculate_return_alt_from_cone_half_angle(rtl_position, (float)_param_rtl_cone_half_angle_deg.get());
} else {
rtl_alt = max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get());
}
}
void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_item_s &land_mission_item) const
@ -487,8 +574,7 @@ void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_ite
rtl_position.yaw = _home_pos_sub.get().yaw;
}
void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position,
const mission_item_s &mission_safe_point) const
void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const
{
// There is a safe point closer than home/mission landing
// TODO: handle all possible mission_safe_point.frame cases
@ -515,41 +601,46 @@ void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position,
}
}
float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position,
float cone_half_angle_deg) const
float RTL::computeReturnAltitude(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const
{
// horizontal distance to destination
const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
rtl_position.lat, rtl_position.lon);
if (_param_rtl_cone_half_angle_deg.get() > 0 && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
// horizontal distance to destination
const float destination_dist =
get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon);
// minium rtl altitude to use when outside of horizontal acceptance radius of target position.
// We choose the minimum height to be two times the distance from the land position in order to
// avoid the vehicle touching the ground while still moving horizontally.
const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get();
// minium rtl altitude to use when outside of horizontal acceptance radius of target position.
// We choose the minimum height to be two times the distance from the land position in order to
// avoid the vehicle touching the ground while still moving horizontally.
const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get();
const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get();
const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get();
float return_altitude_amsl = max_return_altitude;
float return_altitude_amsl = max_return_altitude;
if (destination_dist <= _param_nav_acc_rad.get()) {
return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist;
if (destination_dist <= _param_nav_acc_rad.get()) {
return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist;
} else {
if (destination_dist <= _param_rtl_min_dist.get()) {
} else {
if (destination_dist <= _param_rtl_min_dist.get()) {
// constrain cone half angle to meaningful values. All other cases are already handled above.
const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f));
// constrain cone half angle to meaningful values. All other cases are already handled above.
const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f));
// minimum altitude we need in order to be within the user defined cone
const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt;
// minimum altitude we need in order to be within the user defined cone
const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt;
return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl);
return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl);
}
return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl);
}
return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl);
}
return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude);
return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude);
} else {
// standard behaviour: return altitude above rtl destination
return max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get());
}
}
void RTL::init_rtl_mission_type()
@ -571,6 +662,9 @@ void RTL::init_rtl_mission_type()
} else {
new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
}
} else if (_param_rtl_type.get() == 5) {
new_rtl_mission_type = RtlType::RTL_DIRECT;
}
if (_set_rtl_mission_type == new_rtl_mission_type) {

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

@ -55,11 +55,13 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rtl_status.h>
#include <uORB/topics/rtl_time_estimate.h>
#include <uORB/topics/telemetry_status.h>
class Navigator;
@ -95,6 +97,7 @@ private:
DESTINATION_TYPE_HOME,
DESTINATION_TYPE_MISSION_LAND,
DESTINATION_TYPE_SAFE_POINT,
DESTINATION_TYPE_LAST_LINK_POSITION
};
private:
@ -130,8 +133,13 @@ private:
* @brief Find RTL destination.
*
*/
void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
uint8_t &safe_point_index);
void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, uint8_t &safe_point_index);
/**
* @brief Find RTL destination if only safe points are considered
*
*/
void findClosestSafePoint(PositionYawSetpoint &rtl_position, uint8_t &safe_point_index);
/**
* @brief Set the position of the land start marker in the planned mission as destination.
@ -147,14 +155,13 @@ private:
void setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const;
/**
* @brief calculate return altitude from cone half angle
* @brief calculate return altitude from return altitude parameter, current altitude and cone angle
*
* @param[in] rtl_position landing position of the rtl
* @param[in] cone_half_angle_deg half angle of the cone [deg]
* @return return altitude
*/
float calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position,
float cone_half_angle_deg) const;
float computeReturnAltitude(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const;
/**
* @brief initialize RTL mission type
@ -223,6 +230,7 @@ private:
mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
uint32_t _mission_id = 0u;
uint32_t _safe_points_id = 0u;
PositionYawSetpoint _last_position_before_link_loss;
mission_stats_entry_s _stats;
@ -246,6 +254,7 @@ private:
uORB::SubscriptionData<mission_s> _mission_sub{ORB_ID(mission)};
uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};
uORB::SubscriptionData<wind_s> _wind_sub{ORB_ID(wind)};
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
uORB::PublicationData<rtl_status_s> _rtl_status_pub{ORB_ID(rtl_status)};

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

@ -113,6 +113,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f);
* @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path. Do not consider rally points.
* @value 3 Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land.
* @value 4 Return to the planned mission landing, or to home via the reverse mission path, whichever is closer by counting waypoints. Do not consider rally points.
* @value 5 Return directly to safe landing point (do not consider mission landing and Home)
* @group Return Mode
*/
PARAM_DEFINE_INT32(RTL_TYPE, 0);