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

RTL: When RTL_TYPE is set to 1 make sure to always use a mission landing/safepoint if available and not in RW mode.

Browse Source
This commit is contained in:
Konrad
2023-08-09 08:47:39 +02:00
committed by Silvan Fuhrer
parent 6fca984c3f
commit d4ea106f9e
1 changed files with 13 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+13 -11
View File
@@ -341,12 +341,21 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
rtl_position.yaw = _home_pos_sub.get().yaw; rtl_position.yaw = _home_pos_sub.get().yaw;
isMissionLanding = false; isMissionLanding = false;
// get distance to home position
float min_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)};
const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol 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); && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
// get distance to home position
float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)};
float min_dist;
if ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) {
// Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode.
min_dist = FLT_MAX;
} else {
min_dist = home_dist;
}
// consider the mission landing if available and allowed // consider the mission landing if available and allowed
if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3)) && hasMissionLandStart()) { if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3)) && hasMissionLandStart()) {
mission_item_s land_mission_item; mission_item_s land_mission_item;
@@ -363,14 +372,7 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)}; float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)};
// always find closest destination if in hover and VTOL if (dist < min_dist) {
if ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) {
// Use the mission landing destination.
min_dist = dist;
setLandPosAsDestination(rtl_position, land_mission_item);
isMissionLanding = true;
} else if (dist < min_dist) {
min_dist = dist; min_dist = dist;
setLandPosAsDestination(rtl_position, land_mission_item); setLandPosAsDestination(rtl_position, land_mission_item);
isMissionLanding = true; isMissionLanding = true;