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: fix RTL_TYPE=2 stuck without valid mission

Browse Source 
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan 2026-01-20 15:42:24 +01:00 committed by Silvan Fuhrer
parent 0577a40440
commit fc992385a9
2 changed files with 71 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -310,50 +310,83 @@ bool RTL::isLanding()
void RTL::setRtlTypeAndDestination()
{
init_rtl_mission_type();
uint8_t safe_point_index = UINT8_MAX;
RtlType new_rtl_type{RtlType::RTL_DIRECT};
if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) {
// init destination with Home (used also with Type 2 and 4 as backup)
DestinationType destination_type = DestinationType::DESTINATION_TYPE_HOME;
PositionYawSetpoint destination;
destination.lat = _home_pos_sub.get().lat;
destination.lon = _home_pos_sub.get().lon;
destination.alt = _home_pos_sub.get().alt;
destination.yaw = _home_pos_sub.get().yaw;
loiter_point_s landing_loiter;
landing_loiter.lat = destination.lat;
landing_loiter.lon = destination.lon;
landing_loiter.height_m = NAN;
if (_param_rtl_type.get() == 2) {
if (hasMissionLandStart()) {
new_rtl_type = RtlType::RTL_MISSION_FAST;
} else if (_navigator->get_mission_result()->valid) {
new_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;
} else {
// no valid mission, go direct to home
new_rtl_type = RtlType::RTL_DIRECT;
}
} else if (_param_rtl_type.get() == 4) {
if (hasMissionLandStart() && reverseIsFurther()) {
new_rtl_type = RtlType::RTL_MISSION_FAST;
} else if (_navigator->get_mission_result()->valid) {
new_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;
} else {
// no valid mission, go direct to home
new_rtl_type = RtlType::RTL_DIRECT;
}
} else {
// check the closest allowed destination.
DestinationType destination_type = DestinationType::DESTINATION_TYPE_HOME;
const PositionYawSetpoint destination = findRtlDestination(destination_type, safe_point_index);
float rtl_alt = computeReturnAltitude(destination, (float)_param_rtl_cone_ang.get());
findRtlDestination(destination_type, destination, safe_point_index);
switch (destination_type) {
case DestinationType::DESTINATION_TYPE_MISSION_LAND:
_rtl_type = RtlType::RTL_DIRECT_MISSION_LAND;
_rtl_mission_type_handle->setRtlAlt(rtl_alt);
new_rtl_type = RtlType::RTL_DIRECT_MISSION_LAND;
break;
case DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION:
rtl_alt = destination.alt; // Override return altitude with last known link altitude
// FALLTHROUGH
case DestinationType::DESTINATION_TYPE_SAFE_POINT:
default:
loiter_point_s landing_loiter;
land_approaches_s rtl_land_approaches{readVtolLandApproaches(destination)};
// set loiter position to destination initially, overwrite for VTOL if land approaches exist
landing_loiter.lat = destination.lat;
landing_loiter.lon = destination.lon;
landing_loiter.height_m = NAN;
land_approaches_s rtl_land_approaches{readVtolLandApproaches(destination)};
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(destination, landing_loiter);
new_rtl_type = RtlType::RTL_DIRECT;
break;
}
}
const float rtl_alt = computeReturnAltitude(destination, destination_type, (float)_param_rtl_cone_ang.get());
_rtl_direct.setRtlAlt(rtl_alt);
_rtl_direct.setRtlPosition(destination, landing_loiter);
initRtlMissionType(new_rtl_type, rtl_alt);
_rtl_type = new_rtl_type;
// Publish rtl status
rtl_status_s rtl_status{};
rtl_status.safe_points_id = _safe_points_id;
@ -414,7 +447,7 @@ PositionYawSetpoint RTL::findClosestSafePoint(float min_dist, uint8_t &safe_poin
return safe_point;
}
PositionYawSetpoint RTL::findRtlDestination(DestinationType &destination_type, uint8_t &safe_point_index)
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &destination, uint8_t &safe_point_index)
{
const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
@ -422,16 +455,9 @@ PositionYawSetpoint RTL::findRtlDestination(DestinationType &destination_type, u
const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol
&& (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
PositionYawSetpoint destination{NAN, NAN, NAN, NAN};
float min_dist = FLT_MAX;
if (_param_rtl_type.get() != 5) {
// Home position
destination.lat = _home_pos_sub.get().lat;
destination.lon = _home_pos_sub.get().lon;
destination.alt = _home_pos_sub.get().alt;
destination.yaw = _home_pos_sub.get().yaw;
destination_type = DestinationType::DESTINATION_TYPE_HOME;
_home_has_land_approach = hasVtolLandApproach(destination);
const bool prioritize_safe_points_over_home = ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode);
@ -511,8 +537,6 @@ PositionYawSetpoint RTL::findRtlDestination(DestinationType &destination_type, u
destination_type = DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION;
}
return destination;
}
void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_item_s &land_mission_item) const
@ -548,8 +572,13 @@ void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mis
}
}
float RTL::computeReturnAltitude(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const
float RTL::computeReturnAltitude(const PositionYawSetpoint &rtl_position, DestinationType destination_type, float cone_half_angle_deg) const
{
if (destination_type == DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION) {
// when returning to last known link position, do not modify altitude
return rtl_position.alt;
}
if (_param_rtl_cone_ang.get() > 0 && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
// horizontal distance to destination
const float destination_dist =
@ -590,68 +619,40 @@ float RTL::computeReturnAltitude(const PositionYawSetpoint &rtl_position, float
}
}
void RTL::init_rtl_mission_type()
void RTL::initRtlMissionType(RtlType new_rtl_type, float rtl_alt)
{
RtlType new_rtl_mission_type{RtlType::RTL_DIRECT_MISSION_LAND};
if (_param_rtl_type.get() == 2) {
if (hasMissionLandStart()) {
new_rtl_mission_type = RtlType::RTL_MISSION_FAST;
} else {
new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
}
} else if (_param_rtl_type.get() == 4) {
if (hasMissionLandStart() && reverseIsFurther()) {
new_rtl_mission_type = RtlType::RTL_MISSION_FAST;
} 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) {
if (_rtl_type == new_rtl_type) {
return;
}
if (_rtl_mission_type_handle) {
delete _rtl_mission_type_handle;
_rtl_mission_type_handle = nullptr;
_set_rtl_mission_type = RtlType::NONE;
}
mission_s new_mission = _mission_sub.get();
switch (new_rtl_mission_type) {
switch (new_rtl_type) {
case RtlType::RTL_DIRECT_MISSION_LAND:
_rtl_mission_type_handle = new RtlDirectMissionLand(_navigator);
_set_rtl_mission_type = RtlType::RTL_DIRECT_MISSION_LAND;
_rtl_mission_type_handle->setRtlAlt(rtl_alt);
_rtl_mission_type_handle->initialize();
// RTL type is either direct or mission land have to set it later.
break;
case RtlType::RTL_MISSION_FAST:
_rtl_mission_type_handle = new RtlMissionFast(_navigator, new_mission);
_set_rtl_mission_type = RtlType::RTL_MISSION_FAST;
_rtl_type = RtlType::RTL_MISSION_FAST;
_rtl_mission_type_handle->initialize();
break;
case RtlType::RTL_MISSION_FAST_REVERSE:
_rtl_mission_type_handle = new RtlMissionFastReverse(_navigator, new_mission);
_set_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;
_rtl_mission_type_handle->initialize();
break;
default:
break;
}
if (_rtl_mission_type_handle) {
_rtl_mission_type_handle->initialize();
}
}
void RTL::parameters_update()

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

@ -133,7 +133,7 @@ private:
* @brief Find RTL destination.
*
*/
PositionYawSetpoint findRtlDestination(DestinationType &destination_type, uint8_t &safe_point_index);
void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &destination, uint8_t &safe_point_index);
/**
* @brief Find RTL destination if only safe points are considered
@ -158,16 +158,17 @@ private:
* @brief calculate return altitude from return altitude parameter, current altitude and cone angle
*
* @param[in] rtl_position landing position of the rtl
* param[in] destination_type type of the rtl destination
* @param[in] cone_half_angle_deg half angle of the cone [deg]
* @return return altitude
*/
float computeReturnAltitude(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const;
float computeReturnAltitude(const PositionYawSetpoint &rtl_position, DestinationType destination_type, float cone_half_angle_deg) const;
/**
* @brief initialize RTL mission type
*
*/
void init_rtl_mission_type();
void initRtlMissionType(RtlType new_rtl_type, float rtl_alt);
/**
* @brief Update parameters
@ -213,8 +214,6 @@ private:
hrt_abstime _destination_check_time{0};
RtlBase *_rtl_mission_type_handle{nullptr};
RtlType _set_rtl_mission_type{RtlType::NONE};
RtlType _rtl_type{RtlType::RTL_DIRECT};
bool _home_has_land_approach; ///< Flag if the home position has a land approach defined