From ed7a5314d85c76bd922bad559e74f0ceaabbb840 Mon Sep 17 00:00:00 2001
From: David Jablonski <dayjaby@gmail.com>
Date: Mon, 1 Feb 2021 23:57:47 +0100
Subject: [PATCH] rtl: head towards home location after descend

---
 src/modules/navigator/rtl.cpp | 27 ++++++++++++++++++++++++++-
 src/modules/navigator/rtl.h   |  1 +
 2 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 1c558dfa27..f10dd8f951 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -258,6 +258,7 @@ void RTL::on_activation()
 	const vehicle_global_position_s &global_position = *_navigator->get_global_position();
 
 	_rtl_loiter_rad = _param_rtl_loiter_rad.get();
+
 	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 		_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
 
@@ -458,6 +459,24 @@ void RTL::set_rtl_item()
 			break;
 		}
 
+	case RTL_STATE_HEAD_TO_CENTER: {
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+			_mission_item.lat = _destination.lat;
+			_mission_item.lon = _destination.lon;
+			_mission_item.altitude = loiter_altitude;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
+			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.time_inside = 0.0f;
+			_mission_item.autocontinue = true;
+			_mission_item.origin = ORIGIN_ONBOARD;
+
+			// Disable previous setpoint to prevent drift.
+			pos_sp_triplet->previous.valid = false;
+			break;
+		}
+
 	case RTL_STATE_TRANSITION_TO_MC: {
 			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
 			break;
@@ -559,7 +578,7 @@ void RTL::advance_rtl()
 			_rtl_state = RTL_STATE_LOITER;
 
 		} else 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;
@@ -578,6 +597,12 @@ void RTL::advance_rtl()
 		_rtl_state = RTL_STATE_LAND;
 		break;
 
+	case RTL_STATE_HEAD_TO_CENTER:
+
+		_rtl_state = RTL_STATE_TRANSITION_TO_MC;
+
+		break;
+
 	case RTL_STATE_TRANSITION_TO_MC:
 
 		_rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL;
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index fcdc970ab8..d7cb01028a 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -121,6 +121,7 @@ private:
 		RTL_MOVE_TO_LAND_HOVER_VTOL,
 		RTL_STATE_LAND,
 		RTL_STATE_LANDED,
+		RTL_STATE_HEAD_TO_CENTER,
 	} _rtl_state{RTL_STATE_NONE};
 
 	struct RTLPosition {