Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

rtl: remove duplication for safe landing only in setRtlTypeAndDestination()

Browse Source
This commit is contained in:
Matthias Grob
2026-01-14 19:54:47 +01:00
committed by Silvan Fuhrer
parent f685df32bc
commit deb9a1ad4e
1 changed files with 38 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+15 -29
View File
@@ -314,10 +314,15 @@ void RTL::setRtlTypeAndDestination()
uint8_t safe_point_index{UINT8_MAX};
if (_param_rtl_type.get() == 5) {
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;
if (_param_rtl_type.get() == 5) {
destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
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
@@ -348,33 +353,15 @@ void RTL::setRtlTypeAndDestination()
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_ang.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);
} else {
findRtlDestination(destination_type, rtl_position, safe_point_index);
}
_rtl_type = RtlType::RTL_DIRECT;
_rtl_direct.setRtlAlt(rtl_alt);
_rtl_direct.setRtlPosition(rtl_position, landing_loiter);
float rtl_alt = computeReturnAltitude(rtl_position, (float)_param_rtl_cone_ang.get());
} 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;
findRtlDestination(destination_type, rtl_position, safe_point_index);
const float rtl_alt = computeReturnAltitude(rtl_position, (float)_param_rtl_cone_ang.get());
if (destination_type == DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION) {
rtl_alt = rtl_position.alt;
}
switch (destination_type) {
case DestinationType::DESTINATION_TYPE_MISSION_LAND:
@@ -382,10 +369,9 @@ void RTL::setRtlTypeAndDestination()
_rtl_mission_type_handle->setRtlAlt(rtl_alt);
break;
case DestinationType::DESTINATION_TYPE_SAFE_POINT: // Fallthrough
case DestinationType::DESTINATION_TYPE_HOME: // Fallthrough
case DestinationType::DESTINATION_TYPE_SAFE_POINT:
case DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION:
default:
loiter_point_s landing_loiter;
landing_loiter.lat = rtl_position.lat;
landing_loiter.lon = rtl_position.lon;