fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 06:50:34 +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-10-25 15:32:28 +02: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;;
}
}
src/modules/navigator/rtl_params.c
+1 -1
View File
@@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f);
* Return mode destination and flight path (home location, rally point, mission landing pattern, reverse mission)
*
* @value 0 Return to closest safe point (home or rally point) via direct path.
* @value 1 Return to closest safe point other than home (mission landing pattern or rally point), via direct path. If no mission landing or rally points are defined return home via direct path.
* @value 1 Return to closest safe point other than home (mission landing pattern or rally point), via direct path. If no mission landing or rally points are defined return home via direct path. Always chose closes safe landing point if vehicle is a VTOL in hover mode.
* @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path. Do not consider rally points.
* @value 3 Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land.
* @group Return Mode