From fbc80c9bf592d500d5583fdf30278ae7086c6e06 Mon Sep 17 00:00:00 2001
From: Knut Hjorth <108630475+kibidev@users.noreply.github.com>
Date: Wed, 8 Feb 2023 11:07:39 +0100
Subject: [PATCH] RTL fixes and improvements for VTOL vehicles (#21011)

* rtl: remove unconditional transition to land after descent

This was a bug, as it renders the above code lines useless.
This would cause a undesired FW landing for VTOL vehicles if
RTL_LAND_DELAY is above 0.

* rtl: head to center after loiter in VTOL FW

To get the same behavior for RTL with and without loiter before land for
VTOL drones.

* rtl: always go to descend state after return

Previously, the state would change directly to land if in MR and
RTL_LAND_DELAY was 0.0, but we will still wish to descent to
RTL_DESCEND_ALT at descent speed, instead of using landing speeds.

* rtl: mark head to center state as part of vtol transition

The next step in the sequence is transition to MC. By setting
vtol_back_transition we ensure that the acceptance radius is adapted to
the expected transition distance.
---
 src/modules/navigator/rtl.cpp | 14 +++++---------
 1 file changed, 5 insertions(+), 9 deletions(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 17e64090bb..6d882870e7 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -501,6 +501,9 @@ void RTL::set_rtl_item()
 				_mission_item.yaw = _navigator->get_local_position()->heading;
 			}
 
+			_mission_item.vtol_back_transition = true;
+			// acceptance_radius will be overwritten since vtol_back_transition is set,
+			// set as a default value only
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
@@ -613,13 +616,7 @@ void RTL::advance_rtl()
 		break;
 
 	case RTL_STATE_RETURN:
-		if (vtol_in_fw_mode || descend_and_loiter) {
-			_rtl_state = RTL_STATE_DESCEND;
-
-		} else {
-			_rtl_state = RTL_STATE_LAND;
-		}
-
+		_rtl_state = RTL_STATE_DESCEND;
 		break;
 
 	case RTL_STATE_DESCEND:
@@ -639,13 +636,12 @@ void RTL::advance_rtl()
 	case RTL_STATE_LOITER:
 
 		if (vtol_in_fw_mode) {
-			_rtl_state = RTL_STATE_TRANSITION_TO_MC;
+			_rtl_state = RTL_STATE_HEAD_TO_CENTER;
 
 		} else {
 			_rtl_state = RTL_STATE_LAND;
 		}
 
-		_rtl_state = RTL_STATE_LAND;
 		break;
 
 	case RTL_STATE_HEAD_TO_CENTER: