[RTL] Add VTOL land approach for home with designated loiter points in the RTL mode.

src/modules/navigator/mission_base.h

@@ -119,10 +119,10 @@ protected:
 	/**
 	 * @brief Has Mission a Land Start or Land Item
 	 *
-	 * @return true If mission has a land start of land item
+	 * @return true If mission has a land start of land item and a land item
 	 * @return false otherwise
 	 */
-	bool hasMissionLandStart() const { return _mission.land_start_index > 0;};
+	bool hasMissionLandStart() const { return _mission.land_start_index > 0 && _mission.land_index > 0;};
 	/**
 	 * @brief Go to next Mission Item
 	 * Go to next non jump mission item

src/modules/navigator/mission_block.cpp

@@ -961,3 +961,101 @@ MissionBlock::initialize()
 	_mission_item.autocontinue = true;
 	_mission_item.origin = ORIGIN_ONBOARD;
 }
+
+void MissionBlock::setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius,
+		HeadingMode heading_mode) const
+{
+	item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
+	item.lat = dest.lat;
+	item.lon = dest.lon;
+	item.altitude = dest.alt;
+	item.altitude_is_relative = false;
+
+	item. yaw = setYawFromHeadingMode(dest, heading_mode);
+
+	item.acceptance_radius = _navigator->get_acceptance_radius();
+	item.time_inside = 0.0f;
+	item.autocontinue = true;
+	item.origin = ORIGIN_ONBOARD;
+	item.loiter_radius = loiter_radius;
+}
+
+void MissionBlock::setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time,
+		float loiter_radius, HeadingMode heading_mode) const
+{
+	const bool autocontinue = (loiter_time > -FLT_EPSILON);
+
+	if (autocontinue) {
+		item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+
+	} else {
+		item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+	}
+
+	item.lat = dest.lat;
+	item.lon = dest.lon;
+	item.altitude = dest.alt;
+	item.altitude_is_relative = false;
+
+	item. yaw = setYawFromHeadingMode(dest, heading_mode);
+
+	item.acceptance_radius = _navigator->get_acceptance_radius();
+	item.time_inside = math::max(loiter_time, 0.0f);
+	item.autocontinue = autocontinue;
+	item.origin = ORIGIN_ONBOARD;
+	item.loiter_radius = loiter_radius;
+}
+
+void MissionBlock::setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest,
+		HeadingMode heading_mode) const
+{
+	item.nav_cmd = NAV_CMD_WAYPOINT;
+	item.lat = dest.lat;
+	item.lon = dest.lon;
+	item.altitude = dest.alt;
+	item.altitude_is_relative = false;
+
+	item.autocontinue = true;
+	item.acceptance_radius = _navigator->get_acceptance_radius();
+	item.time_inside = 0.f;
+	item.origin = ORIGIN_ONBOARD;
+
+	item. yaw = setYawFromHeadingMode(dest, heading_mode);
+}
+
+void MissionBlock::setLandMissionItem(mission_item_s &item, const DestinationPosition &dest,
+				      HeadingMode heading_mode) const
+{
+	item.nav_cmd = NAV_CMD_LAND;
+	item.lat = dest.lat;
+	item.lon = dest.lon;
+	item.altitude = dest.alt;
+
+	if (heading_mode == HeadingMode::CURRENT_HEADING) {
+		item.yaw = _navigator->get_local_position()->heading;
+
+	} else {
+		item.yaw = dest.yaw;
+	}
+
+	item.acceptance_radius = _navigator->get_acceptance_radius();
+	item.time_inside = 0.0f;
+	item.autocontinue = true;
+	item.origin = ORIGIN_ONBOARD;
+}
+
+float MissionBlock::setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const
+{
+	float desired_yaw(_navigator->get_local_position()->heading);
+
+	if (heading_mode == HeadingMode::NAVIGATION_HEADING) {
+		desired_yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
+				_navigator->get_global_position()->lon, dest.lat, dest.lon);
+
+	} else if (heading_mode == HeadingMode::DESTINATION_HEADING) {
+		desired_yaw = dest.yaw;
+
+	}
+
+	return desired_yaw;
+}

src/modules/navigator/mission_block.h

@@ -146,6 +146,15 @@ public:
 				    const struct mission_item_s *const mission_item_next) const;
 
 protected:
+	/**
+	 * @brief heading mode for setting navigation items
+	 *
+	 */
+	enum class HeadingMode {
+		NAVIGATION_HEADING = 0,
+		DESTINATION_HEADING,
+		CURRENT_HEADING,
+	};
 	/**
 	 * Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items)
 	 *
@@ -201,6 +210,19 @@ protected:
 	 */
 	void mission_apply_limitation(mission_item_s &item);
 
+	void setLoiterToAltMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_radius,
+				       HeadingMode heading_mode) const;
+
+	void setLoiterHoldMissionItem(mission_item_s &item, const DestinationPosition &dest, float loiter_time,
+				      float loiter_radius, HeadingMode heading_mode) const;
+
+	void setMoveToPositionMissionItem(mission_item_s &item, const DestinationPosition &dest,
+					  HeadingMode heading_mode) const;
+
+	void setLandMissionItem(mission_item_s &item, const DestinationPosition &dest, HeadingMode heading_mode) const;
+
+	float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const;
+
 	/**
 	 * @brief Issue a command for mission items with a nav_cmd that specifies an action
 	 *

src/modules/navigator/navigation.h

@@ -227,6 +227,18 @@ struct mission_fence_point_s {
 	uint8_t _padding0[5];				/**< padding struct size to alignment boundary  */
 };
 
+/**
+ * @brief Return to launch position.
+ * Defines the position and landing yaw for the return to launch destination.
+ *
+ */
+struct DestinationPosition {
+	double lat;	/**< latitude in WGS84 [rad].*/
+	double lon;	/**< longitude in WGS84 [rad].*/
+	float alt;	/**< altitude in MSL [m].*/
+	float yaw;	/**< final yaw when landed [rad].*/
+};
+
 #if (__GNUC__ >= 5) || __clang__
 #pragma GCC diagnostic pop
 #endif // GCC >= 5 || Clang

src/modules/navigator/navigator_main.cpp

@@ -720,6 +720,17 @@ void Navigator::run()
 
 			} else {
 				_pos_sp_triplet_published_invalid_once = false;
+			}
+
+#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
+
+			// If we are in VTOL takeoff, do not switch until it is finished.
+			if (_navigation_mode == &_vtol_takeoff && !get_mission_result()->finished) {
+				navigation_mode_new = &_vtol_takeoff;
+
+			} else
+#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
+			{
 				navigation_mode_new = &_rtl;
 			}
 

src/modules/navigator/rtl.cpp

@@ -42,13 +42,16 @@
 
 #include "rtl.h"
 #include "navigator.h"
+#include "mission_block.h"
 
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/events.h>
 
 using namespace time_literals;
 using namespace math;
+using matrix::wrap_pi;
 
+static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We don't consider safe points valid if the distance from the current home to the safe point is smaller than this distance
 static constexpr float MIN_DIST_THRESHOLD = 2.f;
 
 RTL::RTL(Navigator *navigator) :
@@ -76,7 +79,7 @@ void RTL::updateDatamanCache()
 	case DatamanState::Read:
 
 		_dataman_state	= DatamanState::ReadWait;
-		success = _dataman_client_geofence.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&_stats),
+		success = _dataman_client_safepoint.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&_stats),
 				sizeof(mission_stats_entry_s));
 
 		if (!success) {
@@ -88,9 +91,9 @@ void RTL::updateDatamanCache()
 
 	case DatamanState::ReadWait:
 
-		_dataman_client_geofence.update();
+		_dataman_client_safepoint.update();
 
-		if (_dataman_client_geofence.lastOperationCompleted(success)) {
+		if (_dataman_client_safepoint.lastOperationCompleted(success)) {
 
 			if (!success) {
 				_error_state = DatamanState::ReadWait;
@@ -101,14 +104,14 @@ void RTL::updateDatamanCache()
 				_update_counter = _stats.update_counter;
 				_safe_points_updated = false;
 
-				_dataman_cache_geofence.invalidate();
+				_dataman_cache_safepoint.invalidate();
 
-				if (_dataman_cache_geofence.size() != _stats.num_items) {
-					_dataman_cache_geofence.resize(_stats.num_items);
+				if (_dataman_cache_safepoint.size() != _stats.num_items) {
+					_dataman_cache_safepoint.resize(_stats.num_items);
 				}
 
-				for (int index = 1; index <= _dataman_cache_geofence.size(); ++index) {
-					_dataman_cache_geofence.load(DM_KEY_SAFE_POINTS, index);
+				for (int index = 1; index <= _dataman_cache_safepoint.size(); ++index) {
+					_dataman_cache_safepoint.load(DM_KEY_SAFE_POINTS, index);
 				}
 
 				_dataman_state = DatamanState::Load;
@@ -122,9 +125,9 @@ void RTL::updateDatamanCache()
 
 	case DatamanState::Load:
 
-		_dataman_cache_geofence.update();
+		_dataman_cache_safepoint.update();
 
-		if (!_dataman_cache_geofence.isLoading()) {
+		if (!_dataman_cache_safepoint.isLoading()) {
 			_dataman_state = DatamanState::UpdateRequestWait;
 			_safe_points_updated = true;
 		}
@@ -146,10 +149,6 @@ void RTL::updateDatamanCache()
 		const dm_item_t dm_item = static_cast<dm_item_t>(_mission_sub.get().dataman_id);
 		_dataman_cache_landItem.invalidate();
 
-		if (_mission_sub.get().land_start_index > 0) {
-			_dataman_cache_landItem.load(dm_item, _mission_sub.get().land_start_index);
-		}
-
 		if (_mission_sub.get().land_index > 0) {
 			_dataman_cache_landItem.load(dm_item, _mission_sub.get().land_index);
 		}
@@ -182,6 +181,7 @@ void RTL::on_inactive()
 	_vehicle_status_sub.update();
 	_mission_sub.update();
 	_home_pos_sub.update();
+	_wind_sub.update();
 
 	updateDatamanCache();
 
@@ -227,7 +227,8 @@ void RTL::on_inactive()
 				estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate();
 				break;
 
-			default: break;
+			default:
+				break;
 			}
 		}
 
@@ -252,7 +253,6 @@ void RTL::on_activation()
 		_rtl_direct.on_activation();
 		break;
 
-
 	default:
 		break;
 	}
@@ -264,6 +264,7 @@ void RTL::on_active()
 	_vehicle_status_sub.update();
 	_mission_sub.update();
 	_home_pos_sub.update();
+	_wind_sub.update();
 
 	updateDatamanCache();
 
@@ -290,31 +291,49 @@ void RTL::setRtlTypeAndDestination()
 
 	if (_param_rtl_type.get() != 2) {
 		// check the closest allowed destination.
-		bool isMissionLanding{false};
-		RtlDirect::RtlPosition rtl_position;
+		DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
+		DestinationPosition rtl_position;
 		float rtl_alt;
-		findRtlDestination(isMissionLanding, rtl_position, rtl_alt);
+		findRtlDestination(destination_type, rtl_position, rtl_alt);
 
-		if (isMissionLanding) {
+		switch (destination_type) {
+		case DestinationType::DESTINATION_TYPE_MISSION_LAND:
 			_rtl_type = RtlType::RTL_DIRECT_MISSION_LAND;
 			_rtl_mission_type_handle->setRtlAlt(rtl_alt);
+			break;
+
+		case DestinationType::DESTINATION_TYPE_SAFE_POINT: // Fallthrough
+		case DestinationType::DESTINATION_TYPE_HOME: // Fallthrough
+		default:
+
+			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 (rtl_land_approaches.isAnyApproachValid()) {
+				landing_loiter = chooseBestLandingApproach(rtl_land_approaches);
+			}
 
-		} else {
 			_rtl_type = RtlType::RTL_DIRECT;
 			_rtl_direct.setRtlAlt(rtl_alt);
-			_rtl_direct.setRtlPosition(rtl_position);
+			_rtl_direct.setRtlPosition(rtl_position, landing_loiter);
+
+			break;
 		}
 	}
 }
 
-void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt)
+void RTL::findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt)
 {
 	// set destination to home per default, then check if other valid landing spot is closer
 	rtl_position.alt = _home_pos_sub.get().alt;
 	rtl_position.lat = _home_pos_sub.get().lat;
 	rtl_position.lon = _home_pos_sub.get().lon;
 	rtl_position.yaw = _home_pos_sub.get().yaw;
-	isMissionLanding = false;
+	destination_type = DestinationType::DESTINATION_TYPE_HOME;
 
 	const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol
 				     && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
@@ -323,8 +342,9 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
 	float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)};
 	float min_dist;
 
-	if ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) {
-		// Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode.
+	if (((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) || ((_param_rtl_approach_force.get() == 1)
+			&& !hasVtolLandApproach(rtl_position))) {
+		// Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we forces approach landing and it is not defined for home.
 		min_dist = FLT_MAX;
 
 	} else {
@@ -332,7 +352,8 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
 	}
 
 	// consider the mission landing if available and allowed
-	if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3)) && hasMissionLandStart()) {
+	if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3) || (fabsf(FLT_MAX - min_dist) < FLT_EPSILON))
+	    && hasMissionLandStart()) {
 		mission_item_s land_mission_item;
 		const dm_item_t dm_item = static_cast<dm_item_t>(_mission_sub.get().dataman_id);
 		bool success = _dataman_cache_landItem.loadWait(dm_item, _mission_sub.get().land_index,
@@ -348,18 +369,25 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
 		float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)};
 
 		if ((dist + MIN_DIST_THRESHOLD) < min_dist) {
-			min_dist = dist;
+			if (_param_rtl_type.get() != 0) {
+				min_dist = dist;
+
+			} else {
+				// Mission landing is not allowed, but home has no approaches. Still use mission landing.
+				min_dist = FLT_MAX;
+			}
+
 			setLandPosAsDestination(rtl_position, land_mission_item);
-			isMissionLanding = true;
+			destination_type = DestinationType::DESTINATION_TYPE_MISSION_LAND;
 		}
 	}
 
 	if (_safe_points_updated) {
 
-		for (int current_seq = 1; current_seq <= _dataman_cache_geofence.size(); ++current_seq) {
+		for (int current_seq = 1; current_seq <= _dataman_cache_safepoint.size(); ++current_seq) {
 			mission_item_s mission_safe_point;
 
-			bool success = _dataman_cache_geofence.loadWait(DM_KEY_SAFE_POINTS, current_seq,
+			bool success = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS, current_seq,
 					reinterpret_cast<uint8_t *>(&mission_safe_point),
 					sizeof(mission_item_s), 500_ms);
 
@@ -375,10 +403,15 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
 			if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && dist_to_home > MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
 				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)};
 
-			if ((dist + MIN_DIST_THRESHOLD) < min_dist) {
-				min_dist = dist;
-				setSafepointAsDestination(rtl_position, mission_safe_point);
-				isMissionLanding = false;
+				DestinationPosition safepoint_position;
+				setSafepointAsDestination(safepoint_position, mission_safe_point);
+
+				if (((dist + MIN_DIST_THRESHOLD) < min_dist) && ((_param_rtl_approach_force.get() == 0)
+						|| hasVtolLandApproach(safepoint_position))) {
+					min_dist = dist;
+					rtl_position = safepoint_position;
+					destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
+				}
 			}
 		}
 	}
@@ -392,7 +425,7 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
 	}
 }
 
-void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item)
+void RTL::setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const
 {
 	rtl_position.alt = land_mission_item.altitude_is_relative ?	land_mission_item.altitude +
 			   _home_pos_sub.get().alt : land_mission_item.altitude;
@@ -401,8 +434,8 @@ void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_
 	rtl_position.yaw = _home_pos_sub.get().yaw;
 }
 
-void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position,
-				    const mission_item_s &mission_safe_point)
+void RTL::setSafepointAsDestination(DestinationPosition &rtl_position,
+				    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
@@ -429,8 +462,8 @@ void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position,
 	}
 }
 
-float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position,
-		float cone_half_angle_deg)
+float RTL::calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position,
+		float cone_half_angle_deg) const
 {
 	// horizontal distance to destination
 	const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
@@ -504,7 +537,7 @@ void RTL::init_rtl_mission_type()
 	case RtlType::RTL_MISSION_FAST_REVERSE:
 		_rtl_mission_type_handle = new RtlMissionFastReverse(_navigator);
 		_set_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
-		_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;;
+		_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;
 		break;
 
 	default:
@@ -528,7 +561,99 @@ void RTL::parameters_update()
 	}
 }
 
-bool RTL::hasMissionLandStart()
+bool RTL::hasMissionLandStart() const
 {
 	return _mission_sub.get().land_start_index > 0;
 }
+
+bool RTL::hasVtolLandApproach(const DestinationPosition &rtl_position) const
+{
+	return readVtolLandApproaches(rtl_position).isAnyApproachValid();
+}
+
+loiter_point_s RTL::chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches)
+{
+	const float wind_direction = atan2f(_wind_sub.get().windspeed_east, _wind_sub.get().windspeed_north);
+	int8_t min_index = -1;
+	float wind_angle_prev = INFINITY;
+
+	for (int i = 0; i < vtol_land_approaches.num_approaches_max; i++) {
+
+		if (vtol_land_approaches.approaches[i].isValid()) {
+			const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(_home_pos_sub.get().lat,
+							 _home_pos_sub.get().lon, vtol_land_approaches.approaches[i].lat,
+							 vtol_land_approaches.approaches[i].lon) - wind_direction);
+
+			if (fabsf(wind_angle) < wind_angle_prev) {
+				min_index = i;
+				wind_angle_prev = fabsf(wind_angle);
+			}
+
+		}
+	}
+
+	if (min_index >= 0) {
+		return vtol_land_approaches.approaches[min_index];
+
+	} else {
+
+		return loiter_point_s();
+	}
+}
+
+land_approaches_s RTL::readVtolLandApproaches(DestinationPosition rtl_position) const
+{
+
+	// go through all mission items in the rally point storage. If we find a mission item of type NAV_CMD_RALLY_POINT
+	// which is within MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES of our current home position then treat ALL following mission items of type NAV_CMD_LOITER_TO_ALT which come
+	// BEFORE the next mission item of type NAV_CMD_RALLY_POINT as land approaches for the home position
+	land_approaches_s vtol_land_approaches{};
+
+	if (!_safe_points_updated) {
+		return vtol_land_approaches;
+	}
+
+	bool foundHomeLandApproaches = false;
+	uint8_t sector_counter = 0;
+
+	for (int current_seq = 1; current_seq <= _stats.num_items; ++current_seq) {
+		mission_item_s mission_item{};
+
+		bool success_mission_item = _dataman_cache_safepoint.loadWait(DM_KEY_SAFE_POINTS, current_seq,
+					    reinterpret_cast<uint8_t *>(&mission_item),
+					    sizeof(mission_item_s));
+
+		if (!success_mission_item) {
+			PX4_ERR("dm_read failed");
+			break;
+		}
+
+		if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) {
+
+			if (foundHomeLandApproaches) {
+				break;
+			}
+
+			const float dist_to_safepoint = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, rtl_position.lat,
+							rtl_position.lon);
+
+			if (dist_to_safepoint < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
+				foundHomeLandApproaches = true;
+				vtol_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.lat, mission_item.lon);
+			}
+
+			sector_counter = 0;
+		}
+
+		if (foundHomeLandApproaches && mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
+			vtol_land_approaches.approaches[sector_counter].lat = mission_item.lat;
+			vtol_land_approaches.approaches[sector_counter].lon = mission_item.lon;
+			vtol_land_approaches.approaches[sector_counter].height_m = MissionBlock::get_absolute_altitude_for_item(mission_item,
+					_home_pos_sub.get().alt);
+			vtol_land_approaches.approaches[sector_counter].loiter_radius_m = mission_item.loiter_radius;
+			sector_counter++;
+		}
+	}
+
+	return vtol_land_approaches;
+}

src/modules/navigator/rtl.h

@@ -44,6 +44,7 @@
 #include <px4_platform_common/module_params.h>
 
 #include "navigator_mode.h"
+#include "navigation.h"
 #include <dataman_client/DatamanClient.hpp>
 #include "rtl_base.h"
 #include "rtl_direct.h"
@@ -88,7 +89,14 @@ public:
 	void updateSafePoints() { _initiate_safe_points_updated = true; }
 
 private:
-	bool hasMissionLandStart();
+	enum class DestinationType {
+		DESTINATION_TYPE_HOME,
+		DESTINATION_TYPE_MISSION_LAND,
+		DESTINATION_TYPE_SAFE_POINT,
+	};
+
+private:
+	bool hasMissionLandStart() const;
 
 	/**
 	 * @brief function to call regularly to do background work
@@ -101,20 +109,20 @@ private:
 	 * @brief Find RTL destination.
 	 *
 	 */
-	void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt);
+	void findRtlDestination(DestinationType &destination_type, DestinationPosition &rtl_position, float &rtl_alt);
 
 	/**
 	 * @brief Set the position of the land start marker in the planned mission as destination.
 	 *
 	 */
-	void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item);
+	void setLandPosAsDestination(DestinationPosition &rtl_position, mission_item_s &land_mission_item) const;
 
 	/**
 	 * @brief Set the safepoint as destination.
 	 *
 	 * @param mission_safe_point is the mission safe point/rally point to set as destination.
 	 */
-	void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_item_s &mission_safe_point);
+	void setSafepointAsDestination(DestinationPosition &rtl_position, const mission_item_s &mission_safe_point) const;
 
 	/**
 	 * @brief calculate return altitude from cone half angle
@@ -123,7 +131,8 @@ private:
 	 * @param[in] cone_half_angle_deg half angle of the cone [deg]
 	 * @return return altitude
 	 */
-	float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg);
+	float calculate_return_alt_from_cone_half_angle(const DestinationPosition &rtl_position,
+			float cone_half_angle_deg) const;
 
 	/**
 	 * @brief initialize RTL mission type
@@ -137,6 +146,33 @@ private:
 	 */
 	void parameters_update();
 
+	/**
+	 * @brief read VTOL land approaches
+	 *
+	 * @param[in] rtl_position landing position of the rtl
+	 *
+	 */
+	land_approaches_s readVtolLandApproaches(DestinationPosition rtl_position) const;
+
+	/**
+	 * @brief Has VTOL land approach
+	 *
+	 * @param[in] rtl_position landing position of the rtl
+	 *
+	 * @return true if home land approaches are defined for home position
+	 * @return false otherwise
+	 */
+	bool hasVtolLandApproach(const DestinationPosition &rtl_position) const;
+
+	/**
+	 * @brief Choose best landing approach
+	 *
+	 * Choose best landing approach for home considering wind
+	 *
+	 * @return loiter_point_s best landing approach
+	 */
+	loiter_point_s chooseBestLandingApproach(const land_approaches_s &vtol_land_approaches);
+
 	enum class DatamanState {
 		UpdateRequestWait,
 		Read,
@@ -156,10 +192,10 @@ private:
 	DatamanState _error_state{DatamanState::UpdateRequestWait};
 	uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated
 	bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache
-	DatamanCache _dataman_cache_geofence{"rtl_dm_cache_miss_geo", 4};
-	DatamanClient	&_dataman_client_geofence = _dataman_cache_geofence.client();
+	mutable DatamanCache _dataman_cache_safepoint{"rtl_dm_cache_miss_geo", 4};
+	DatamanClient	&_dataman_client_safepoint = _dataman_cache_safepoint.client();
 	bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed
-	DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
+	mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
 	int16_t _mission_counter = -1;
 
 	mission_stats_entry_s _stats;
@@ -173,7 +209,8 @@ private:
 		(ParamInt<px4::params::RTL_CONE_ANG>)      _param_rtl_cone_half_angle_deg,
 		(ParamFloat<px4::params::RTL_RETURN_ALT>)  _param_rtl_return_alt,
 		(ParamFloat<px4::params::RTL_MIN_DIST>)    _param_rtl_min_dist,
-		(ParamFloat<px4::params::NAV_ACC_RAD>)      _param_nav_acc_rad
+		(ParamFloat<px4::params::NAV_ACC_RAD>)     _param_nav_acc_rad,
+		(ParamInt<px4::params::RTL_APPR_FORCE>)    _param_rtl_approach_force
 	)
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -182,6 +219,7 @@ private:
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};	/**< vehicle status subscription */
 	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::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
 };

src/modules/navigator/rtl_direct.cpp

@@ -40,23 +40,26 @@
  * @author Julian Kent <julian@auterion.com>
  */
 
+#include <float.h>
+
 #include "rtl_direct.h"
 #include "navigator.h"
-#include <dataman/dataman.h>
 #include <px4_platform_common/events.h>
 
 #include <lib/geo/geo.h>
 
-
-static constexpr float DELAY_SIGMA = 0.01f;
-
-using namespace time_literals;
 using namespace math;
 
 RtlDirect::RtlDirect(Navigator *navigator) :
 	MissionBlock(navigator),
 	ModuleParams(navigator)
 {
+	_destination.lat = static_cast<double>(NAN);
+	_destination.lon = static_cast<double>(NAN);
+	_land_approach.lat = static_cast<double>(NAN);
+	_land_approach.lon = static_cast<double>(NAN);
+	_land_approach.height_m = NAN;
+
 	_param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP");
 	_param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN");
 	_param_mpc_land_speed = param_find("MPC_LAND_SPEED");
@@ -72,54 +75,45 @@ void RtlDirect::on_inactivation()
 	if (_navigator->get_precland()->is_activated()) {
 		_navigator->get_precland()->on_inactivation();
 	}
+
+	_rtl_state = RTLState::IDLE;
 }
 
 void RtlDirect::on_activation()
 {
 	_global_pos_sub.update();
-	_local_pos_sub.update();
 	_land_detected_sub.update();
 	_vehicle_status_sub.update();
 
 	parameters_update();
 
-	if (_land_detected_sub.get().landed) {
-		// For safety reasons don't go into RTL if landed.
-		_rtl_state = RTL_STATE_LANDED;
-
-	} else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || _enforce_rtl_alt) {
-
-		// If lower than return altitude, climb up first.
-		// If rtl_alt_min is true then forcing altitude change even if above.
-		_rtl_state = RTL_STATE_CLIMB;
-
-	} else {
-		// Otherwise go start with climb
-		_rtl_state = RTL_STATE_RETURN;
-	}
+	_rtl_state = getActivationLandState();
 
 	// reset cruising speed and throttle to default for RTL
 	_navigator->reset_cruising_speed();
 	_navigator->set_cruising_throttle();
 
 	set_rtl_item();
+
+	mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: start return at %d m (%d m above destination)\t",
+			 (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt));
+	events::send<int32_t, int32_t>(events::ID("vrtl_return_at"), events::Log::Info,
+				       "RTL: start return at {1m_v} ({2m_v} above destination)",
+				       (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt));
 }
 
 void RtlDirect::on_active()
 {
 	_global_pos_sub.update();
-	_local_pos_sub.update();
-	_land_detected_sub.update();
 	_vehicle_status_sub.update();
 
 	parameters_update();
 
-	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) {
-		advance_rtl();
+	if (_rtl_state != RTLState::IDLE && is_mission_item_reached_or_completed()) {
 		set_rtl_item();
 	}
 
-	if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) {
+	if (_rtl_state == RTLState::LAND && _param_rtl_pld_md.get() > 0) {
 		// Need to update the position and type on the current setpoint triplet.
 		_navigator->get_precland()->on_active();
 
@@ -128,249 +122,204 @@ void RtlDirect::on_active()
 	}
 }
 
+void RtlDirect::setRtlPosition(DestinationPosition rtl_position, loiter_point_s loiter_pos)
+{
+	_home_pos_sub.update();
+
+	parameters_update();
+
+	// Only allow to set a new approach if the mode is not activated yet.
+	if (!isActive()) {
+		_land_approach = loiter_pos;
+		_destination = rtl_position;
+		_force_heading = false;
+
+		// Input sanitation
+		if (!PX4_ISFINITE(_destination.lat) || !PX4_ISFINITE(_destination.lon)) {
+			// We don't have a valid rtl position, use the home position instead.
+			_destination.lat = _home_pos_sub.get().lat;
+			_destination.lon = _home_pos_sub.get().lon;
+			_destination.alt = _home_pos_sub.get().alt;
+			_destination.yaw = _home_pos_sub.get().yaw;
+		}
+
+		if (!PX4_ISFINITE(_destination.alt)) {
+			// Not a valid rtl land altitude. Assume same altitude as home position.
+			_destination.alt = _home_pos_sub.get().alt;
+		}
+
+		if (!PX4_ISFINITE(_land_approach.lat) || !PX4_ISFINITE(_land_approach.lon)) {
+			_land_approach.lat = _destination.lat;
+			_land_approach.lon = _destination.lon;
+
+		} else {
+			const float dist_to_destination{get_distance_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon)};
+
+			if (dist_to_destination > _navigator->get_acceptance_radius()) {
+				_force_heading = true;
+			}
+		}
+
+		if (!PX4_ISFINITE(_land_approach.height_m)) {
+			_land_approach.height_m = _destination.alt + _param_rtl_descend_alt.get();
+		}
+
+		if (!PX4_ISFINITE(_land_approach.loiter_radius_m) || fabsf(_land_approach.loiter_radius_m) <= FLT_EPSILON) {
+			_land_approach.loiter_radius_m = _param_rtl_loiter_rad.get();
+		}
+	}
+}
+
 void RtlDirect::set_rtl_item()
 {
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
 	const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon,
 				       _global_pos_sub.get().lat, _global_pos_sub.get().lon);
-	const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
+	const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt);
 
-	const RTLHeadingMode rtl_heading_mode = static_cast<RTLHeadingMode>(_param_rtl_hdg_md.get());
+	HeadingMode rtl_heading_mode = static_cast<HeadingMode>(_param_rtl_hdg_md.get());
+
+	if ((rtl_heading_mode == HeadingMode::NAVIGATION_HEADING) && (destination_dist < _param_rtl_min_dist.get())) {
+		rtl_heading_mode = HeadingMode::DESTINATION_HEADING;
+	}
 
 	switch (_rtl_state) {
-	case RTL_STATE_CLIMB: {
+	case RTLState::CLIMBING: {
+			DestinationPosition dest {
+				.lat = _global_pos_sub.get().lat,
+				.lon = _global_pos_sub.get().lon,
+				.alt = _rtl_alt,
+			};
 
-			_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
+			setLoiterToAltMissionItem(_mission_item, dest, _navigator->get_loiter_radius(), HeadingMode::CURRENT_HEADING);
 
-			_mission_item.lat = _global_pos_sub.get().lat;
-			_mission_item.lon = _global_pos_sub.get().lon;
-			_mission_item.altitude = _rtl_alt;
-			_mission_item.altitude_is_relative = false;
-
-			if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) {
-				_mission_item.yaw = _local_pos_sub.get().heading;
-
-			} else {
-				_mission_item.yaw = _destination.yaw;
-			}
-
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-			_mission_item.time_inside = 0.0f;
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
-			_mission_item.loiter_radius = _navigator->get_loiter_radius();
-
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t",
-					 (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt));
-			events::send<int32_t, int32_t>(events::ID("rtl_climb_to"), events::Log::Info,
-						       "RTL: climb to {1m_v} ({2m_v} above destination)",
-						       (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt));
+			_rtl_state = RTLState::MOVE_TO_LOITER;
 			break;
 		}
 
-	case RTL_STATE_RETURN: {
+	case RTLState::MOVE_TO_LOITER: {
+			DestinationPosition dest {
+				.lat = _land_approach.lat,
+				.lon = _land_approach.lon,
+				.alt = _rtl_alt,
+				.yaw = _destination.yaw,
+			};
 
 			// For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status
 			// can be displayed on groundstation and the WP is accepted once within loiter radius
 			if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-				_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-
+				setLoiterHoldMissionItem(_mission_item, dest, 0.f, _land_approach.loiter_radius_m, rtl_heading_mode);
 
 			} else {
-				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+				setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode);
 			}
 
-			_mission_item.lat = _destination.lat;
-			_mission_item.lon = _destination.lon;
-			_mission_item.altitude = _rtl_alt; // Don't change altitude
-			_mission_item.altitude_is_relative = false;
-
-			if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING &&
-			    destination_dist > _param_rtl_min_dist.get()) {
-				_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat,
-						    _destination.lon);
-
-			} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING ||
-				   destination_dist < _param_rtl_min_dist.get()) {
-				// Use destination yaw if close to _destination.
-				_mission_item.yaw = _destination.yaw;
-
-			} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
-				_mission_item.yaw = _local_pos_sub.get().heading;
-			}
-
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-			_mission_item.time_inside = 0.0f;
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
-			_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
-
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t",
-					 (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
-			events::send<int32_t, int32_t>(events::ID("rtl_return_at"), events::Log::Info,
-						       "RTL: return at {1m_v} ({2m_v} above destination)",
-						       (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt));
+			_rtl_state = RTLState::LOITER_DOWN;
 
 			break;
 		}
 
-	case RTL_STATE_DESCEND: {
-			_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
-			_mission_item.lat = _destination.lat;
-			_mission_item.lon = _destination.lon;
-			_mission_item.altitude = loiter_altitude;
-			_mission_item.altitude_is_relative = false;
+	case RTLState::LOITER_DOWN: {
+			DestinationPosition dest{
+				.lat = _land_approach.lat,
+				.lon = _land_approach.lon,
+				.alt = loiter_altitude,
+				.yaw = _destination.yaw,
+			};
 
-			// Except for vtol which might be still off here and should point towards this location.
-			const float d_current = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
-						_mission_item.lat, _mission_item.lon);
+			setLoiterToAltMissionItem(_mission_item, dest, _land_approach.loiter_radius_m, rtl_heading_mode);
 
-			if (_vehicle_status_sub.get().is_vtol && (d_current > _navigator->get_acceptance_radius())) {
-				_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
-						    _mission_item.lat, _mission_item.lon);
+			pos_sp_triplet->next.valid = true;
+			pos_sp_triplet->next.lat = _destination.lat;
+			pos_sp_triplet->next.lon = _destination.lon;
+			pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_LAND;
 
-			} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
-				_mission_item.yaw = _local_pos_sub.get().heading;
-
-			} else {
-				_mission_item.yaw = _destination.yaw;
+			if (_force_heading) {
+				_mission_item.force_heading = true;
 			}
 
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-			_mission_item.time_inside = 0.0f;
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
-			_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
-
 			// Disable previous setpoint to prevent drift.
 			pos_sp_triplet->previous.valid = false;
 
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t",
-					 (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
-			events::send<int32_t, int32_t>(events::ID("rtl_descend_to"), events::Log::Info,
-						       "RTL: descend to {1m_v} ({2m_v} above destination)",
-						       (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt));
+			_rtl_state = RTLState::LOITER_HOLD;
+
 			break;
 		}
 
-	case RTL_STATE_LOITER: {
-			const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON);
+	case RTLState::LOITER_HOLD: {
+			DestinationPosition dest {
+				.lat = _land_approach.lat,
+				.lon = _land_approach.lon,
+				.alt = loiter_altitude,
+				.yaw = _destination.yaw,
+			};
 
-			if (autocontinue) {
-				_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t",
-						 (double)_param_rtl_land_delay.get());
-				events::send<float>(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get());
+			setLoiterHoldMissionItem(_mission_item, dest, _param_rtl_land_delay.get(), _land_approach.loiter_radius_m,
+						 rtl_heading_mode);
 
-			} else {
-				_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+			if (_param_rtl_land_delay.get() < -FLT_EPSILON) {
 				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t");
 				events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering");
 			}
 
-			_mission_item.lat = _destination.lat;
-			_mission_item.lon = _destination.lon;
-			_mission_item.altitude = loiter_altitude;    // Don't change altitude.
-			_mission_item.altitude_is_relative = false;
-
-			if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
-				_mission_item.yaw = _local_pos_sub.get().heading;
+			if (_vehicle_status_sub.get().is_vtol
+			    && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+				_rtl_state = RTLState::MOVE_TO_LAND;
 
 			} else {
-				_mission_item.yaw = _destination.yaw;
+				_rtl_state = RTLState::MOVE_TO_LAND_HOVER;
 			}
 
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-			_mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f);
-			_mission_item.autocontinue = autocontinue;
-			_mission_item.origin = ORIGIN_ONBOARD;
-			_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
-
 			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;
+	case RTLState::MOVE_TO_LAND: {
 
-			if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
-				_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat,
-						    _destination.lon);
+			DestinationPosition dest{_destination};
+			dest.alt = loiter_altitude;
 
-			} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) {
-				_mission_item.yaw = _destination.yaw;
-
-			} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
-				_mission_item.yaw = _local_pos_sub.get().heading;
-			}
+			setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode);
 
+			// Prepare for transition
 			_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;
-			_mission_item.origin = ORIGIN_ONBOARD;
+			_mission_item.force_heading = false;
+
+			// set previous item location to loiter location such that vehicle tracks line between loiter
+			// location and land location after exiting the loiter circle
+			pos_sp_triplet->previous.lat = _land_approach.lat;
+			pos_sp_triplet->previous.lon = _land_approach.lon;
+			pos_sp_triplet->previous.alt = _mission_item.altitude;
+			pos_sp_triplet->previous.valid = true;
+
+			_rtl_state = RTLState::TRANSITION_TO_MC;
 
-			// Disable previous setpoint to prevent drift.
-			pos_sp_triplet->previous.valid = false;
 			break;
 		}
 
-	case RTL_STATE_TRANSITION_TO_MC: {
+	case RTLState::TRANSITION_TO_MC: {
 			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
+
+			_rtl_state = RTLState::MOVE_TO_LAND_HOVER;
+
 			break;
 		}
 
-	case RTL_MOVE_TO_LAND_HOVER_VTOL: {
-			_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;
+	case RTLState::MOVE_TO_LAND_HOVER: {
+			DestinationPosition dest{_destination};
+			dest.alt = loiter_altitude;
 
-			// have to reset here because these field were used in set_vtol_transition_item
-			_mission_item.time_inside = 0.f;
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			setMoveToPositionMissionItem(_mission_item, dest, rtl_heading_mode);
 
-			if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) {
-				_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat,
-						    _destination.lon);
+			_rtl_state = RTLState::LAND;
 
-			} else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) {
-				_mission_item.yaw = _destination.yaw;
-
-			} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
-				_mission_item.yaw = _local_pos_sub.get().heading;
-			}
-
-			_mission_item.origin = ORIGIN_ONBOARD;
 			break;
 		}
 
-	case RTL_STATE_LAND: {
-			// Land at destination.
-			_mission_item.nav_cmd = NAV_CMD_LAND;
-			_mission_item.lat = _destination.lat;
-			_mission_item.lon = _destination.lon;
-			_mission_item.altitude = _destination.alt;
-			_mission_item.altitude_is_relative = false;
+	case RTLState::LAND: {
 
-			if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) {
-				_mission_item.yaw = _local_pos_sub.get().heading;
+			setLandMissionItem(_mission_item, _destination, rtl_heading_mode);
 
-			} else {
-				_mission_item.yaw = _destination.yaw;
-			}
-
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-			_mission_item.time_inside = 0.0f;
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
 			_mission_item.land_precision = _param_rtl_pld_md.get();
 
 			if (_mission_item.land_precision == 1) {
@@ -382,13 +331,16 @@ void RtlDirect::set_rtl_item()
 				_navigator->get_precland()->on_activation();
 			}
 
+			_rtl_state = RTLState::IDLE;
+
 			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t");
 			events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination");
 			break;
 		}
 
-	case RTL_STATE_LANDED: {
+	case RTLState::IDLE: {
 			set_idle_item(&_mission_item);
+			_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
 			break;
 		}
 
@@ -401,85 +353,35 @@ void RtlDirect::set_rtl_item()
 	// Execute command if set. This is required for commands like VTOL transition.
 	if (!MissionBlock::item_contains_position(_mission_item)) {
 		issue_command(_mission_item);
-	}
 
-	// Convert mission item to current position setpoint and make it valid.
-	mission_apply_limitation(_mission_item);
+	} else {
+		// Convert mission item to current position setpoint and make it valid.
+		mission_apply_limitation(_mission_item);
 
-	if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
-		_navigator->set_position_setpoint_triplet_updated();
+		if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
+			_navigator->set_position_setpoint_triplet_updated();
+		}
 	}
 }
 
-void RtlDirect::advance_rtl()
+RtlDirect::RTLState RtlDirect::getActivationLandState()
 {
-	// determines if the vehicle should loiter above land
-	const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA;
+	_land_detected_sub.update();
 
-	// vehicle is a vtol and currently in fixed wing mode
-	const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol
-				     && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+	RTLState land_state;
 
-	switch (_rtl_state) {
-	case RTL_STATE_CLIMB:
-		_rtl_state = RTL_STATE_RETURN;
-		break;
+	if (_land_detected_sub.get().landed) {
+		// For safety reasons don't go into RTL if landed.
+		land_state = RTLState::IDLE;
 
-	case RTL_STATE_RETURN:
-		_rtl_state = RTL_STATE_DESCEND;
-		break;
+	} else if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) {
+		land_state = RTLState::CLIMBING;
 
-	case RTL_STATE_DESCEND:
-
-		if (descend_and_loiter) {
-			_rtl_state = RTL_STATE_LOITER;
-
-		} else if (vtol_in_fw_mode) {
-			_rtl_state = RTL_STATE_HEAD_TO_CENTER;
-
-		} else {
-			_rtl_state = RTL_STATE_LAND;
-		}
-
-		break;
-
-	case RTL_STATE_LOITER:
-
-		if (vtol_in_fw_mode) {
-			_rtl_state = RTL_STATE_HEAD_TO_CENTER;
-
-		} else {
-			_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;
-
-		break;
-
-	case RTL_MOVE_TO_LAND_HOVER_VTOL:
-
-		_rtl_state = RTL_STATE_LAND;
-
-		break;
-
-	case RTL_STATE_LAND:
-		_rtl_state = RTL_STATE_LANDED;
-		_navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
-		break;
-
-	default:
-		break;
+	} else {
+		land_state = RTLState::MOVE_TO_LOITER;
 	}
+
+	return land_state;
 }
 
 rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
@@ -488,10 +390,13 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 
 	rtl_time_estimate_s rtl_time_estimate{};
 
-	RTLState start_state_for_estimate{RTL_STATE_NONE};
+	RTLState start_state_for_estimate;
 
 	if (isActive()) {
 		start_state_for_estimate = _rtl_state;
+
+	} else {
+		start_state_for_estimate = getActivationLandState();
 	}
 
 	// Calculate RTL time estimate only when there is a valid home position
@@ -501,11 +406,13 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 
 	} else {
 		rtl_time_estimate.valid = true;
+		rtl_time_estimate.time_estimate = 0.f;
+
+		const float loiter_altitude = min(_land_approach.height_m, _rtl_alt);
 
 		// Sum up time estimate for various segments of the landing procedure
 		switch (start_state_for_estimate) {
-		case RTL_STATE_NONE:
-		case RTL_STATE_CLIMB: {
+		case RTLState::CLIMBING: {
 				// Climb segment is only relevant if the drone is below return altitude
 				const float climb_dist = _global_pos_sub.get().alt < _rtl_alt ? (_rtl_alt - _global_pos_sub.get().alt) : 0;
 
@@ -515,30 +422,24 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 			}
 
 		// FALLTHROUGH
-		case RTL_STATE_RETURN:
+		case RTLState::MOVE_TO_LOITER:
 
 			// Add cruise segment to home
 			rtl_time_estimate.time_estimate += get_distance_to_next_waypoint(
-					_destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed();
+					_land_approach.lat, _land_approach.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed();
 
 		// FALLTHROUGH
-		case RTL_STATE_HEAD_TO_CENTER:
-		case RTL_STATE_TRANSITION_TO_MC:
-		case RTL_STATE_DESCEND: {
+		case RTLState::LOITER_DOWN: {
 				// when descending, the target altitude is stored in the current mission item
 				float initial_altitude = 0.f;
-				float loiter_altitude = 0.f;
 
-				if (start_state_for_estimate == RTL_STATE_DESCEND) {
+				if (start_state_for_estimate == RTLState::LOITER_DOWN) {
 					// Take current vehicle altitude as the starting point for calculation
 					initial_altitude = _global_pos_sub.get().alt;  // TODO: Check if this is in the right frame
-					loiter_altitude = _mission_item.altitude;  // Next waypoint = loiter
-
 
 				} else {
 					// Take the return altitude as the starting point for the calculation
 					initial_altitude = _rtl_alt; // CLIMB and RETURN
-					loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
 				}
 
 				// Add descend segment (first landing phase: return alt to loiter alt)
@@ -546,18 +447,38 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 			}
 
 		// FALLTHROUGH
-		case RTL_STATE_LOITER:
+		case RTLState::LOITER_HOLD:
 			// Add land delay (the short pause for deploying landing gear)
 			// TODO: Check if landing gear is deployed or not
 			rtl_time_estimate.time_estimate += _param_rtl_land_delay.get();
 
 		// FALLTHROUGH
-		case RTL_MOVE_TO_LAND_HOVER_VTOL:
-		case RTL_STATE_LAND: {
+		case RTLState::MOVE_TO_LAND:
+		case RTLState::TRANSITION_TO_MC:
+		case RTLState::MOVE_TO_LAND_HOVER: {
+				// Add cruise segment to home
+				float move_to_land_dist{0.f};
+
+				if (start_state_for_estimate >= RTLState::MOVE_TO_LAND) {
+					move_to_land_dist = get_distance_to_next_waypoint(
+								    _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon);
+
+				} else {
+					move_to_land_dist = get_distance_to_next_waypoint(
+								    _destination.lat, _destination.lon, _land_approach.lat, _land_approach.lon);
+				}
+
+				if (move_to_land_dist > FLT_EPSILON) {
+					rtl_time_estimate.time_estimate += move_to_land_dist / getCruiseGroundSpeed();
+				}
+			}
+
+		// FALLTHROUGH
+		case RTLState::LAND: {
 				float initial_altitude;
 
 				// Add land segment (second landing phase) which comes after LOITER
-				if (start_state_for_estimate == RTL_STATE_LAND) {
+				if (start_state_for_estimate == RTLState::LAND) {
 					// If we are in this phase, use the current vehicle altitude  instead
 					// of the altitude paramteter to get a continous time estimate
 					initial_altitude = _global_pos_sub.get().alt;
@@ -566,7 +487,6 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 				} else {
 					// If this phase is not active yet, simply use the loiter altitude,
 					// which is where the LAND phase will start
-					const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
 					initial_altitude = loiter_altitude;
 				}
 
@@ -578,7 +498,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 
 			break;
 
-		case RTL_STATE_LANDED:
+		case RTLState::IDLE:
 			// Remaining time is 0
 			break;
 		}

src/modules/navigator/rtl_direct.h

@@ -41,11 +41,9 @@
 
 #pragma once
 
-#include <px4_platform_common/module_params.h>
-
-#include "mission_block.h"
-
 #include <drivers/drv_hrt.h>
+#include <matrix/Vector2.hpp>
+#include <px4_platform_common/module_params.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/home_position.h>
@@ -53,10 +51,12 @@
 #include <uORB/topics/rtl_time_estimate.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
-#include <matrix/Vector2.hpp>
+
+#include "mission_block.h"
+#include "navigation.h"
+#include "safe_point_land.hpp"
 
 using namespace time_literals;
 
@@ -65,18 +65,6 @@ class Navigator;
 class RtlDirect : public MissionBlock, public ModuleParams
 {
 public:
-	/**
-	 * @brief Return to launch position.
-	 * Defines the position and landing yaw for the return to launch destination.
-	 *
-	 */
-	struct RtlPosition {
-		double lat;	/**< latitude in WGS84 [rad].*/
-		double lon;	/**< longitude in WGS84 [rad].*/
-		float alt;	/**< altitude in MSL [m].*/
-		float yaw;	/**< final yaw when landed [rad].*/
-	};
-
 	RtlDirect(Navigator *navigator);
 
 	~RtlDirect() = default;
@@ -111,35 +99,24 @@ public:
 	void setReturnAltMin(bool min) { _enforce_rtl_alt = min; }
 	void setRtlAlt(float alt) {_rtl_alt = alt;};
 
-	void setRtlPosition(RtlPosition position) {_destination = position;};
+	void setRtlPosition(DestinationPosition position, loiter_point_s loiter_pos);
 
 private:
-	/**
-	 * @brief Return to launch heading mode.
-	 *
-	 */
-	enum RTLHeadingMode {
-		RTL_NAVIGATION_HEADING = 0,
-		RTL_DESTINATION_HEADING,
-		RTL_CURRENT_HEADING,
-	};
-
 	/**
 	 * @brief Return to launch state machine.
 	 *
 	 */
-	enum RTLState {
-		RTL_STATE_NONE = 0,
-		RTL_STATE_CLIMB,
-		RTL_STATE_RETURN,
-		RTL_STATE_DESCEND,
-		RTL_STATE_LOITER,
-		RTL_STATE_TRANSITION_TO_MC,
-		RTL_MOVE_TO_LAND_HOVER_VTOL,
-		RTL_STATE_LAND,
-		RTL_STATE_LANDED,
-		RTL_STATE_HEAD_TO_CENTER,
-	};
+	enum class RTLState {
+		CLIMBING,
+		MOVE_TO_LOITER,
+		LOITER_DOWN,
+		LOITER_HOLD,
+		MOVE_TO_LAND,
+		TRANSITION_TO_MC,
+		MOVE_TO_LAND_HOVER,
+		LAND,
+		IDLE
+	} _rtl_state{RTLState::IDLE}; /*< Current state in the state machine.*/
 
 private:
 	/**
@@ -155,12 +132,6 @@ private:
 	 */
 	void set_rtl_item();
 
-	/**
-	 * @brief Advance the return to launch state machine.
-	 *
-	 */
-	void advance_rtl();
-
 	/**
 	 * @brief Get the Cruise Ground Speed
 	 *
@@ -201,17 +172,19 @@ private:
 	 */
 	void parameters_update();
 
-	/** Current state in the state machine.*/
-	RTLState _rtl_state{RTL_STATE_NONE};
+	RTLState getActivationLandState();
+
+	void setLoiterPosition();
 
 	bool _enforce_rtl_alt{false};
+	bool _force_heading{false};
 
-	RtlPosition _destination{}; ///< the RTL position to fly to
+	DestinationPosition _destination; ///< the RTL position to fly to
+	loiter_point_s _land_approach;
 
 	float _rtl_alt{0.0f};	///< AMSL altitude at which the vehicle should return to the home position
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RTL_RETURN_ALT>)  _param_rtl_return_alt,
 		(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt,
 		(ParamFloat<px4::params::RTL_LAND_DELAY>)  _param_rtl_land_delay,
 		(ParamFloat<px4::params::RTL_MIN_DIST>)    _param_rtl_min_dist,
@@ -234,8 +207,8 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)};	/**< global position subscription */
+	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 	uORB::SubscriptionData<vehicle_land_detected_s> _land_detected_sub{ORB_ID(vehicle_land_detected)};	/**< vehicle land detected subscription */
 	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};	/**< vehicle status subscription */
-	uORB::SubscriptionData<vehicle_local_position_s> _local_pos_sub{ORB_ID(vehicle_local_position)};	/**< vehicle status subscription */
 	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};
 };

src/modules/navigator/rtl_params.c

@@ -200,3 +200,13 @@ PARAM_DEFINE_FLOAT(RTL_TIME_FACTOR, 1.1f);
  * @group Return To Land
  */
 PARAM_DEFINE_INT32(RTL_TIME_MARGIN, 100);
+
+/**
+ * RTL force approach landing
+ *
+ * Only consider RTL point, if it has an approach defined.
+ *
+ * @boolean
+ * @group Return To Land
+ */
+PARAM_DEFINE_INT32(RTL_APPR_FORCE, 0);

src/modules/navigator/safe_point_land.hpp

@@ -0,0 +1,110 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file safe_point_land.hpp
+ * This file defines helper structs that are used to define land approaches which consists of a land location and a number of
+ * loiter circles. Each loiter circle defines a possible approach for landing at the land location.
+ *
+ */
+
+#pragma once
+
+#include <lib/mathlib/mathlib.h>
+#include <geo/geo.h>
+
+struct loiter_point_s {
+	loiter_point_s() { reset(); }
+	double lat;
+	double lon;
+	float height_m;
+	float loiter_radius_m;
+
+	void reset()
+	{
+		lat = lon = static_cast<double>(NAN);
+		height_m = NAN;
+		loiter_radius_m = NAN;
+	}
+
+	bool isValid() const { return PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(height_m); }
+};
+
+// defines one land location and a maximum of num_approaches_max loiter points
+struct land_approaches_s {
+
+	static constexpr uint8_t num_approaches_max = 8;
+	loiter_point_s approaches[num_approaches_max];
+	matrix::Vector2d land_location_lat_lon;
+
+	land_approaches_s()
+	{
+		resetAllApproaches();
+	}
+
+	void resetAllApproaches()
+	{
+		for (uint8_t i = 0; i < num_approaches_max; i++) {
+			approaches[i].reset();
+		}
+	}
+
+	bool isAnyApproachValid() const
+	{
+		for (uint8_t i = 0; i < num_approaches_max; i++) {
+			if (approaches[i].isValid()) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	float getMaxDistLandToLoiterCircle() const
+	{
+		// returns negative infinity if there is no valid approach
+		float dist_max = -INFINITY;
+
+		for (uint8_t i = 0; i < num_approaches_max; i++) {
+			if (approaches[i].isValid()) {
+				float dist = get_distance_to_next_waypoint(land_location_lat_lon(0), land_location_lat_lon(1), approaches[i].lat,
+						approaches[i].lon) + approaches[i].loiter_radius_m;
+
+				if (dist > dist_max) {
+					dist_max = dist;
+				}
+			}
+		}
+
+		return dist_max;
+	}
+};