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

Navigator: fix logic around when to switch to mission landing for RTL (#21390)

Browse Source 
* Navigator: change way of telling logic if RLT was started just now

* Navigator: change logic around when to engage Mission mode for RTL

To find out if we're currently on a mission landing, check if we either are
past the land start marker OR currently land start marker is current WP
and vehicle is already in LOITER mode.

* Navigator: do not engage RTL at all if already on mission landing

* Navigator: consider to be on mission landing if the LOITER_TO_ALT and dist small

To find out if we're currently on a mission landing, check if we either are
past the land start marker OR currently land start marker is current WP,
the type is LOITER_TO_ALT and the vehicle is inside loiter.

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-04-05 10:11:14 +02:00
committed by GitHub
parent fb3b4a3151
commit 94fb334d8f
5 changed files with 38 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+1 -7
View File
@@ -152,7 +152,7 @@ void RTL::find_RTL_destination()
double dist_squared = coord_dist_sq(dlat, dlon);
// always find closest destination if in hover and VTOL
if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->getMissionLandingInProgress())) {
if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->on_mission_landing())) {
// compare home position to landing position to decide which is closer
if (dist_squared < min_dist_squared) {
@@ -282,12 +282,6 @@ void RTL::on_activation()
// For safety reasons don't go into RTL if landed.
_rtl_state = RTL_STATE_LANDED;
} else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) {
// we were just on a mission landing, set _rtl_state past RTL_STATE_LOITER such that navigator will engage mission mode,
// which will continue executing the landing
_rtl_state = RTL_STATE_LAND;
} else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) {
// If lower than return altitude, climb up first.