Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 13:15:32 +08:00
Code Issues Actions 9 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
+38 -52
View File
@@ -314,78 +314,64 @@ void RTL::setRtlTypeAndDestination()
uint8_t safe_point_index{UINT8_MAX}; 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; PositionYawSetpoint rtl_position;
findClosestSafePoint(rtl_position, safe_point_index);
DestinationType destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
if (safe_point_index == UINT8_MAX) { if (_param_rtl_type.get() == 5) {
// no safe point found, set destination to last position with valid data link destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
for (auto &telemetry_status : _telemetry_status_subs) { findClosestSafePoint(rtl_position, safe_point_index);
telemetry_status_s telemetry;
if (telemetry_status.update(&telemetry)) { 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.heartbeat_type_gcs) { if (telemetry_status.update(&telemetry)) {
_last_position_before_link_loss.alt = _global_pos_sub.get().alt;
_last_position_before_link_loss.lat = _global_pos_sub.get().lat; if (telemetry.heartbeat_type_gcs) {
_last_position_before_link_loss.lon = _global_pos_sub.get().lon; _last_position_before_link_loss.alt = _global_pos_sub.get().alt;
break; _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;
} }
if (!PX4_ISFINITE(_last_position_before_link_loss.lat) || !PX4_ISFINITE(_last_position_before_link_loss.lon)) { } else {
// if we never had a valid data link position, fallback to current position findRtlDestination(destination_type, rtl_position, safe_point_index);
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 float rtl_alt = computeReturnAltitude(rtl_position, (float)_param_rtl_cone_ang.get());
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; if (destination_type == DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION) {
landing_loiter.lat = rtl_position.lat; rtl_alt = rtl_position.alt;
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;
findRtlDestination(destination_type, rtl_position, safe_point_index);
const float rtl_alt = computeReturnAltitude(rtl_position, (float)_param_rtl_cone_ang.get());
switch (destination_type) { switch (destination_type) {
case DestinationType::DESTINATION_TYPE_MISSION_LAND: case DestinationType::DESTINATION_TYPE_MISSION_LAND:
_rtl_type = RtlType::RTL_DIRECT_MISSION_LAND; _rtl_type = RtlType::RTL_DIRECT_MISSION_LAND;
_rtl_mission_type_handle->setRtlAlt(rtl_alt); _rtl_mission_type_handle->setRtlAlt(rtl_alt);
break; break;
case DestinationType::DESTINATION_TYPE_SAFE_POINT: // Fallthrough case DestinationType::DESTINATION_TYPE_SAFE_POINT:
case DestinationType::DESTINATION_TYPE_HOME: // Fallthrough case DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION:
default: default:
loiter_point_s landing_loiter; loiter_point_s landing_loiter;
landing_loiter.lat = rtl_position.lat; landing_loiter.lat = rtl_position.lat;
landing_loiter.lon = rtl_position.lon; landing_loiter.lon = rtl_position.lon;