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

RTL: if vehicle is a VTOL in hover, always consider all safe landing points

Browse Source 
Unless there is already a mission landing in progress,
in that case stay on the mission landing if RTL_TYPE
isn't set to closest.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-11-05 17:58:21 +03:00
committed by Roman Bapst
parent 1163a52c17
commit 3bd114bf9f
2 changed files with 16 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+15 -4
View File
@@ -132,18 +132,29 @@ void RTL::find_RTL_destination()
mission_landing_alt = _navigator->get_mission_landing_start_alt();
}
// compare home position to landing position to decide which is closer
dlat = mission_landing_lat - global_position.lat;
dlon = mission_landing_lon - global_position.lon;
double dist_squared = coord_dist_sq(dlat, dlon);
// set destination to mission landing if closest or in RTL_TYPE_MISSION_LANDING or RTL_TYPE_MISSION_LANDING_REVERSED (so not in RTL_TYPE_CLOSEST)
if (dist_squared < min_dist_squared || (_param_rtl_type.get() != RTL_TYPE_CLOSEST && !vtol_in_rw_mode)) {
// always find closest destination if in hover and VTOL
if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->getMissionLandingInProgress())) {
// compare home position to landing position to decide which is closer
if (dist_squared < min_dist_squared) {
_destination.type = RTL_DESTINATION_MISSION_LANDING;
min_dist_squared = dist_squared;
_destination.lat = mission_landing_lat;
_destination.lon = mission_landing_lon;
_destination.alt = mission_landing_alt;
}
} else {
// it has to be the mission landing
_destination.type = RTL_DESTINATION_MISSION_LANDING;
min_dist_squared = dist_squared;
_destination.lat = mission_landing_lat;
_destination.lon = mission_landing_lon;
_destination.alt = mission_landing_alt;
_destination.type = RTL_DESTINATION_MISSION_LANDING;;
}
}