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

@@ -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();

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
 	 */

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);