RTL: add new RTL_TYPE to only allow returns to safe points or last link position

Do not allow RTL to Home or mission landings.

Signed-off-by: Silvan <silvan@auterion.com>

src/modules/navigator/rtl.cpp

@@ -310,17 +310,71 @@ bool RTL::isLanding()
 
 void RTL::setRtlTypeAndDestination()
 {
-
 	init_rtl_mission_type();
 
-	uint8_t safe_point_index{0U};
+	uint8_t safe_point_index{UINT8_MAX};
 
-	if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) {
+	if (_param_rtl_type.get() == 5) {
+		PositionYawSetpoint rtl_position;
+		findClosestSafePoint(rtl_position, safe_point_index);
+		DestinationType destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
+
+		if (safe_point_index == UINT8_MAX) {
+			// no safe point found, set destination to last position with valid data link
+			for (auto &telemetry_status :  _telemetry_status_subs) {
+				telemetry_status_s telemetry;
+
+				if (telemetry_status.update(&telemetry)) {
+
+					if (telemetry.heartbeat_type_gcs) {
+						_last_position_before_link_loss.alt = _global_pos_sub.get().alt;
+						_last_position_before_link_loss.lat = _global_pos_sub.get().lat;
+						_last_position_before_link_loss.lon = _global_pos_sub.get().lon;
+						break;
+					}
+				}
+			}
+
+			if (!PX4_ISFINITE(_last_position_before_link_loss.lat) || !PX4_ISFINITE(_last_position_before_link_loss.lon)) {
+				// if we never had a valid data link position, fallback to current position
+				rtl_position.alt = _global_pos_sub.get().alt;
+				rtl_position.lat = _global_pos_sub.get().lat;
+				rtl_position.lon = _global_pos_sub.get().lon;
+
+			} else {
+				rtl_position = _last_position_before_link_loss;
+			}
+
+			destination_type = DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION;
+		}
+
+		// set rtl altitude to the destination from the beginning for DestinationType::DESTINATION_TYPE_LAST_LINK_POSITION
+		const float rtl_alt = destination_type == DestinationType::DESTINATION_TYPE_SAFE_POINT ? computeReturnAltitude(rtl_position,
+				      (float)_param_rtl_cone_half_angle_deg.get()) : rtl_position.alt;
+
+		loiter_point_s landing_loiter;
+		landing_loiter.lat = rtl_position.lat;
+		landing_loiter.lon = rtl_position.lon;
+		landing_loiter.height_m = NAN;
+
+		land_approaches_s rtl_land_approaches{readVtolLandApproaches(rtl_position)};
+
+		if (_vehicle_status_sub.get().is_vtol
+		    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
+		    && rtl_land_approaches.isAnyApproachValid()) {
+			landing_loiter = chooseBestLandingApproach(rtl_land_approaches);
+		}
+
+		_rtl_type = RtlType::RTL_DIRECT;
+		_rtl_direct.setRtlAlt(rtl_alt);
+		_rtl_direct.setRtlPosition(rtl_position, landing_loiter);
+
+	} else 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;
-		float rtl_alt;
+		findRtlDestination(destination_type, rtl_position, safe_point_index);
-		findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index);
+		const float rtl_alt = computeReturnAltitude(rtl_position, (float)_param_rtl_cone_half_angle_deg.get());
 
 		switch (destination_type) {
 		case DestinationType::DESTINATION_TYPE_MISSION_LAND:
@@ -366,7 +420,48 @@ void RTL::setRtlTypeAndDestination()
 
 }
 
-void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
+void RTL::findClosestSafePoint(PositionYawSetpoint &rtl_position, uint8_t &safe_point_index)
+{
+	float min_dist = FLT_MAX;
+
+	if (_safe_points_updated) {
+
+		_one_rally_point_has_land_approach = false;
+
+		for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
+			mission_item_s mission_safe_point;
+
+			const bool success = _dataman_cache_safepoint.loadWait(static_cast<dm_item_t>(_stats.dataman_id), current_seq,
+					     reinterpret_cast<uint8_t *>(&mission_safe_point),
+					     sizeof(mission_item_s), 500_ms);
+
+			if (!success) {
+				PX4_ERR("dm_read failed");
+				continue;
+			}
+
+			if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT) {
+				const float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
+
+				PositionYawSetpoint safepoint_position;
+				setSafepointAsDestination(safepoint_position, mission_safe_point);
+
+				const bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)};
+
+				_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
+
+				if (((dist + MIN_DIST_THRESHOLD) < min_dist) && ((_param_rtl_approach_force.get() == 0)
+						|| current_safe_point_has_approaches)) {
+					min_dist = dist;
+					rtl_position = safepoint_position;
+					safe_point_index = current_seq;
+				}
+			}
+		}
+	}
+}
+
+void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position,
 			     uint8_t &safe_point_index)
 {
 	// set destination to home per default, then check if other valid landing spot is closer
@@ -448,7 +543,7 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 			const float dist_to_home = get_distance_to_next_waypoint(_home_pos_sub.get().lat, _home_pos_sub.get().lon,
 						   mission_safe_point.lat, mission_safe_point.lon);
 
-			if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
+			if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) { // remove!
 				float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
 
 				PositionYawSetpoint safepoint_position;
@@ -468,14 +563,6 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
 			}
 		}
 	}
-
-	if (_param_rtl_cone_half_angle_deg.get() > 0
-	    && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-		rtl_alt = calculate_return_alt_from_cone_half_angle(rtl_position, (float)_param_rtl_cone_half_angle_deg.get());
-
-	} else {
-		rtl_alt = max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get());
-	}
 }
 
 void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_item_s &land_mission_item) const
@@ -487,8 +574,7 @@ void RTL::setLandPosAsDestination(PositionYawSetpoint &rtl_position, mission_ite
 	rtl_position.yaw = _home_pos_sub.get().yaw;
 }
 
-void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position,
+void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const
-				    const mission_item_s &mission_safe_point) const
 {
 	// There is a safe point closer than home/mission landing
 	// TODO: handle all possible mission_safe_point.frame cases
@@ -515,41 +601,46 @@ void RTL::setSafepointAsDestination(PositionYawSetpoint &rtl_position,
 	}
 }
 
-float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position,
+float RTL::computeReturnAltitude(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const
-		float cone_half_angle_deg) const
 {
-	// horizontal distance to destination
+	if (_param_rtl_cone_half_angle_deg.get() > 0 && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-	const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
+		// horizontal distance to destination
-				       rtl_position.lat, rtl_position.lon);
+		const float destination_dist =
+			get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon);
 
-	// minium rtl altitude to use when outside of horizontal acceptance radius of target position.
+		// minium rtl altitude to use when outside of horizontal acceptance radius of target position.
-	// We choose the minimum height to be two times the distance from the land position in order to
+		// We choose the minimum height to be two times the distance from the land position in order to
-	// avoid the vehicle touching the ground while still moving horizontally.
+		// avoid the vehicle touching the ground while still moving horizontally.
-	const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get();
+		const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get();
 
-	const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get();
+		const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get();
 
-	float return_altitude_amsl = max_return_altitude;
+		float return_altitude_amsl = max_return_altitude;
 
-	if (destination_dist <= _param_nav_acc_rad.get()) {
+		if (destination_dist <= _param_nav_acc_rad.get()) {
-		return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist;
+			return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist;
 
-	} else {
+		} else {
-		if (destination_dist <= _param_rtl_min_dist.get()) {
+			if (destination_dist <= _param_rtl_min_dist.get()) {
 
-			// constrain cone half angle to meaningful values. All other cases are already handled above.
+				// constrain cone half angle to meaningful values. All other cases are already handled above.
-			const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f));
+				const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f));
 
-			// minimum altitude we need in order to be within the user defined cone
+				// minimum altitude we need in order to be within the user defined cone
-			const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt;
+				const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt;
 
-			return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl);
+				return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl);
+			}
+
+			return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl);
 		}
 
-		return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl);
+		return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude);
-	}
 
-	return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude);
+	} else {
+		// standard behaviour: return altitude above rtl destination
+		return max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get());
+	}
 }
 
 void RTL::init_rtl_mission_type()
@@ -571,6 +662,9 @@ void RTL::init_rtl_mission_type()
 		} else {
 			new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
 		}
+
+	} else if (_param_rtl_type.get() == 5) {
+		new_rtl_mission_type = RtlType::RTL_DIRECT;
 	}
 
 	if (_set_rtl_mission_type == new_rtl_mission_type) {

src/modules/navigator/rtl.h

@@ -55,11 +55,13 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/rtl_status.h>
 #include <uORB/topics/rtl_time_estimate.h>
+#include <uORB/topics/telemetry_status.h>
 
 class Navigator;
 
@@ -95,6 +97,7 @@ private:
 		DESTINATION_TYPE_HOME,
 		DESTINATION_TYPE_MISSION_LAND,
 		DESTINATION_TYPE_SAFE_POINT,
+		DESTINATION_TYPE_LAST_LINK_POSITION
 	};
 
 private:
@@ -130,8 +133,13 @@ private:
 	 * @brief Find RTL destination.
 	 *
 	 */
-	void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
+	void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, uint8_t &safe_point_index);
-				uint8_t &safe_point_index);
+
+	/**
+	 * @brief Find RTL destination if only safe points are considered
+	 *
+	 */
+	void findClosestSafePoint(PositionYawSetpoint &rtl_position, uint8_t &safe_point_index);
 
 	/**
 	 * @brief Set the position of the land start marker in the planned mission as destination.
@@ -147,14 +155,13 @@ private:
 	void setSafepointAsDestination(PositionYawSetpoint &rtl_position, const mission_item_s &mission_safe_point) const;
 
 	/**
-	 * @brief calculate return altitude from cone half angle
+	 * @brief calculate return altitude from return altitude parameter, current altitude and cone angle
 	 *
 	 * @param[in] rtl_position landing position of the rtl
 	 * @param[in] cone_half_angle_deg half angle of the cone [deg]
 	 * @return return altitude
 	 */
-	float calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint &rtl_position,
+	float computeReturnAltitude(const PositionYawSetpoint &rtl_position, float cone_half_angle_deg) const;
-			float cone_half_angle_deg) const;
 
 	/**
 	 * @brief initialize RTL mission type
@@ -223,6 +230,7 @@ private:
 	mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
 	uint32_t _mission_id = 0u;
 	uint32_t _safe_points_id = 0u;
+	PositionYawSetpoint _last_position_before_link_loss;
 
 	mission_stats_entry_s _stats;
 
@@ -246,6 +254,7 @@ private:
 	uORB::SubscriptionData<mission_s> _mission_sub{ORB_ID(mission)};
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};
 	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};
+	uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
 
 	uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
 	uORB::PublicationData<rtl_status_s> _rtl_status_pub{ORB_ID(rtl_status)};

src/modules/navigator/rtl_params.c

@@ -113,6 +113,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f);
  * @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.
+ * @value 5 Return directly to safe landing point (do not consider mission landing and Home)
  * @group Return Mode
  */
 PARAM_DEFINE_INT32(RTL_TYPE, 0);