From 8acf273917279dc8756c7dd592fa7688d5f8692e Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 28 Mar 2025 06:29:42 +1300
Subject: [PATCH] Add RTL_TYPE to continue or reverse (#24581)

This adds RTL_TYPE 4 which means continue the mission or reverse back to
the takeoff location, whichever is closer in terms of mission items
in-between.

This would be nicer to have on a distance rather than mission item count
basis but that would require access to the dataman and make it more
complex.
---
 src/modules/navigator/rtl.cpp      | 16 +++++++++++++++-
 src/modules/navigator/rtl.h        |  8 ++++++++
 src/modules/navigator/rtl_params.c |  1 +
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 1b53488d7a..b2898be415 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -318,7 +318,7 @@ void RTL::setRtlTypeAndDestination()
 
 	uint8_t safe_point_index{0U};
 
-	if (_param_rtl_type.get() != 2) {
+	if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) {
 		// check the closest allowed destination.
 		DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
 		PositionYawSetpoint rtl_position;
@@ -566,6 +566,14 @@ void RTL::init_rtl_mission_type()
 		} else {
 			new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
 		}
+
+	} else if (_param_rtl_type.get() == 4) {
+		if (hasMissionLandStart() && reverseIsFurther()) {
+			new_rtl_mission_type = RtlType::RTL_MISSION_FAST;
+
+		} else {
+			new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
+		}
 	}
 
 	if (_set_rtl_mission_type == new_rtl_mission_type) {
@@ -630,6 +638,12 @@ bool RTL::hasMissionLandStart() const
 	       && _navigator->get_mission_result()->valid;
 }
 
+bool RTL::reverseIsFurther() const
+{
+	return (_mission_sub.get().land_start_index - _mission_sub.get().current_seq) < _mission_sub.get().current_seq;
+}
+
+
 bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const
 {
 	return readVtolLandApproaches(rtl_position).isAnyApproachValid();
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 18a7ad3ea1..207af7ceb5 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -105,6 +105,14 @@ private:
 	 */
 	bool hasMissionLandStart() const;
 
+
+	/**
+	 * @brief Check whether there are more waypoints between current waypoint
+	 *        and the takeoff location than the end/land location.
+	 * @return true if the reverse is more items away.
+	 */
+	bool reverseIsFurther() const;
+
 	/**
 	 * @brief function to call regularly to do background work
 	 */
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 951993a724..af02033c3a 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -112,6 +112,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f);
  * @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 closest 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.
+ * @value 4 Return to the planned mission landing, or to home via the reverse mission path, whichever is closer by counting waypoints. Do not consider rally points.
  * @group Return Mode
  */
 PARAM_DEFINE_INT32(RTL_TYPE, 0);