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

src/modules/navigator/CMakeLists.txt

@@ -51,6 +51,7 @@ px4_add_module(
 		precland.cpp
 		mission_feasibility_checker.cpp
 		geofence.cpp
+		vtol_land.cpp
 		vtol_takeoff.cpp
 	DEPENDS
 		geo

src/modules/navigator/navigator_main.cpp

@@ -666,7 +666,15 @@ void Navigator::run()
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 			_pos_sp_triplet_published_invalid_once = false;
-			navigation_mode_new = &_rtl;
+
+			// 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 {
+				navigation_mode_new = &_rtl;
+			}
+
 			break;
 
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:

src/modules/navigator/rtl.cpp

@@ -54,7 +54,8 @@ RTL::RTL(Navigator *navigator) :
 	ModuleParams(navigator),
 	_rtl_direct(navigator),
 	_rtl_mission(navigator),
-	_rtl_mission_reverse(navigator)
+	_rtl_mission_reverse(navigator),
+	_rtl_vtol_land(navigator)
 {
 	initMission();
 }
@@ -74,6 +75,10 @@ void RTL::on_inactivation()
 		_rtl_direct.on_inactivation();
 		break;
 
+	case RtlType::RTL_DIRECT_SAFE_VTOL:
+		_rtl_vtol_land.on_inactivation();
+		break;
+
 	default:
 		break;
 	}
@@ -88,6 +93,7 @@ void RTL::on_inactive()
 	_rtl_mission.on_inactive();
 	_rtl_mission_reverse.on_inactive();
 	_rtl_direct.on_inactive();
+	_rtl_vtol_land.on_inactive();
 
 	// Limit inactive calculation to 1Hz
 	hrt_abstime now{hrt_absolute_time()};
@@ -111,6 +117,10 @@ void RTL::on_inactive()
 			estimated_time = _rtl_mission_reverse.calc_rtl_time_estimate();
 			break;
 
+		case RtlType::RTL_DIRECT_SAFE_VTOL:
+			_rtl_vtol_land.calc_rtl_time_estimate();
+			break;
+
 		default:
 			break;
 		}
@@ -136,6 +146,10 @@ void RTL::on_activation()
 		_rtl_direct.on_activation(_enforce_rtl_alt);
 		break;
 
+	case RtlType::RTL_DIRECT_SAFE_VTOL:
+		_rtl_vtol_land.on_activation();
+		break;
+
 	default:
 		break;
 	}
@@ -152,18 +166,28 @@ void RTL::on_active()
 		_rtl_mission.on_active();
 		_rtl_mission_reverse.on_inactive();
 		_rtl_direct.on_inactive();
+		_rtl_vtol_land.on_inactive();
 		break;
 
 	case RtlType::RTL_MISSION_FAST_REVERSE:
 		_rtl_mission_reverse.on_active();
 		_rtl_mission.on_inactive();
 		_rtl_direct.on_inactive();
+		_rtl_vtol_land.on_inactive();
 		break;
 
 	case RtlType::RTL_DIRECT:
 		_rtl_direct.on_active();
 		_rtl_mission_reverse.on_inactive();
 		_rtl_mission.on_inactive();
+		_rtl_vtol_land.on_inactive();
+		break;
+
+	case RtlType::RTL_DIRECT_SAFE_VTOL:
+		_rtl_vtol_land.on_active();
+		_rtl_mission_reverse.on_inactive();
+		_rtl_mission.on_inactive();
+		_rtl_direct.on_inactive();
 		break;
 
 	default:
@@ -185,31 +209,58 @@ void RTL::setRtlTypeAndDestination()
 
 	} else {
 		// check the closest allowed destination.
-		bool isMissionLanding{false};
+		DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
 		RtlDirect::RtlPosition 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_mission.setInitialMissionIndex(_land_start_index);
 			_rtl_type = RtlType::RTL_MISSION_FAST;
+			break;
 
-		} else {
+		case DestinationType::DESTINATION_TYPE_SAFE_POINT:
 			_rtl_direct.setRtlAlt(rtl_alt);
 			_rtl_direct.setRtlPosition(rtl_position);
 			_rtl_type = RtlType::RTL_DIRECT;
+			break;
+
+		case DestinationType::DESTINATION_TYPE_HOME: {
+				// check if we can apply vtol land.
+				bool vtol_land{_vehicle_status_sub.get().is_vtol &&
+					       _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
+					       _rtl_vtol_land.hasVtolHomeLandApproach()};
+
+				if (vtol_land) {
+					_rtl_type = RtlType::RTL_DIRECT_SAFE_VTOL;
+
+				} else {
+					_rtl_direct.setRtlAlt(rtl_alt);
+					_rtl_direct.setRtlPosition(rtl_position);
+					_rtl_type = RtlType::RTL_DIRECT;
+				}
+
+				break;
+			}
+
+		default:
+			_rtl_direct.setRtlAlt(rtl_alt);
+			_rtl_direct.setRtlPosition(rtl_position);
+			_rtl_type = RtlType::RTL_DIRECT;
+			break;
 		}
 	}
 }
 
-void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt)
+void RTL::findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPosition &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;
 
 	// get distance to home position
 	float min_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)};
@@ -226,12 +277,12 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
 			// Use the mission landing destination.
 			min_dist = dist;
 			setLandPosAsDestination(rtl_position);
-			isMissionLanding = true;
+			destination_type = DestinationType::DESTINATION_TYPE_MISSION_LAND;
 
 		} else if (dist < min_dist) {
 			min_dist = dist;
 			setLandPosAsDestination(rtl_position);
-			isMissionLanding = true;
+			destination_type = DestinationType::DESTINATION_TYPE_MISSION_LAND;
 		}
 	}
 
@@ -263,7 +314,7 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
 		if (dist < min_dist) {
 			min_dist = dist;
 			setSafepointAsDestination(rtl_position, mission_safe_point);
-			isMissionLanding = false;
+			destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
 		}
 	}
 

src/modules/navigator/rtl.h

@@ -49,6 +49,7 @@
 #include "rtl_direct.h"
 #include "rtl_mission_fast.h"
 #include "rtl_mission_fast_reverse.h"
+#include "vtol_land.h"
 
 #include <uORB/uORB.h>
 #include <uORB/Publication.hpp>
@@ -65,8 +66,10 @@ public:
 
 	enum class RtlType {
 		RTL_DIRECT,
+		RTL_DIRECT_SAFE_VTOL,
 		RTL_MISSION_FAST,
 		RTL_MISSION_FAST_REVERSE,
+		VTOL_LAND
 	};
 
 	void on_inactivation() override;
@@ -78,6 +81,13 @@ public:
 
 	void set_return_alt_min(bool min) { _enforce_rtl_alt = min; }
 
+private:
+	enum class DestinationType {
+		DESTINATION_TYPE_HOME,
+		DESTINATION_TYPE_MISSION_LAND,
+		DESTINATION_TYPE_SAFE_POINT,
+	};
+
 private:
 	void onMissionUpdate(bool has_mission_items_changed) override {};
 
@@ -87,7 +97,7 @@ private:
 	 * @brief Find RTL destination.
 	 *
 	 */
-	void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt);
+	void findRtlDestination(DestinationType &destination_type, RtlDirect::RtlPosition &rtl_position, float &rtl_alt);
 
 	/**
 	 * @brief Set the position of the land start marker in the planned mission as destination.
@@ -120,6 +130,8 @@ private:
 
 	RtlMissionFastReverse _rtl_mission_reverse;
 
+	VtolLand _rtl_vtol_land;
+
 	bool _enforce_rtl_alt{false};
 
 	DEFINE_PARAMETERS(

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;
+	}
+};

src/modules/navigator/vtol_land.cpp

@@ -0,0 +1,329 @@
+/****************************************************************************
+ *
+ *   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 vtol_land.cpp
+ *
+ * Helper class to do a VTOL landing using a loiter down to altitude landing pattern.
+ *
+ */
+
+#include "vtol_land.h"
+#include "navigator.h"
+#include "navigation.h"
+
+#include "modules/dataman/dataman.h"
+
+using matrix::wrap_pi;
+
+VtolLand::VtolLand(Navigator *navigator) :
+	MissionBlock(navigator),
+	ModuleParams(navigator)
+{
+}
+
+void
+VtolLand::on_activation()
+{
+	_global_pos_sub.update();
+	_home_pos_sub.update();
+	_wind_sub.update();
+
+	_land_pos_lat_lon = matrix::Vector2<double>(_home_pos_sub.get().lat, _home_pos_sub.get().lon);
+	readVtolHomeLandApproachesFromStorage();
+	set_loiter_position();
+	_land_state = vtol_land_state::MOVE_TO_LOITER;
+
+}
+
+void VtolLand::on_inactive()
+{
+	_global_pos_sub.update();
+	_home_pos_sub.update();
+	_wind_sub.update();
+}
+
+void
+VtolLand::on_active()
+{
+	_global_pos_sub.update();
+	_home_pos_sub.update();
+	_wind_sub.update();
+
+	if (is_mission_item_reached_or_completed()) {
+		switch	(_land_state) {
+		case vtol_land_state::MOVE_TO_LOITER: {
+				_mission_item.altitude = _home_pos_sub.get().alt + _land_approach.height_m;
+				_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
+				_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
+
+				_navigator->get_mission_result()->finished = false;
+				_navigator->set_mission_result_updated();
+				reset_mission_item_reached();
+
+				// convert mission item to current setpoint
+				struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+				mission_apply_limitation(_mission_item);
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+				pos_sp_triplet->next.valid = true;
+				pos_sp_triplet->next.lat = _land_pos_lat_lon(0);
+				pos_sp_triplet->next.lon = _land_pos_lat_lon(1);
+				pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_LAND;
+
+				pos_sp_triplet->previous.valid = false;
+				pos_sp_triplet->current.yaw_valid = true;
+
+				_land_state = vtol_land_state::LOITER_DOWN;
+				break;
+			}
+
+		case vtol_land_state::LOITER_DOWN: {
+				_mission_item.lat = _land_pos_lat_lon(0);
+				_mission_item.lon = _land_pos_lat_lon(1);
+
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+				_mission_item.vtol_back_transition = true;
+
+				struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+				// 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 = _loiter_pos_lat_lon(0);
+				pos_sp_triplet->previous.lon = _loiter_pos_lat_lon(1);
+				pos_sp_triplet->previous.alt = _mission_item.altitude;
+				pos_sp_triplet->previous.valid = true;
+
+				//publish_navigator_mission_item(); // for logging
+				_navigator->set_position_setpoint_triplet_updated();
+
+				// issue_command(_mission_item);
+				reset_mission_item_reached();
+
+				_land_state = vtol_land_state::TRANSITION_TO_MC;
+
+				break;
+			}
+
+		case vtol_land_state::TRANSITION_TO_MC: {
+				set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
+				_mission_item.vtol_back_transition = true;
+
+				issue_command(_mission_item);
+
+				struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+				//publish_navigator_mission_item(); // for logging
+				_navigator->set_position_setpoint_triplet_updated();
+
+				// issue_command(_mission_item);
+				reset_mission_item_reached();
+
+				_land_state = vtol_land_state::LAND;
+
+				break;
+			}
+
+		case vtol_land_state::LAND: {
+				_mission_item.nav_cmd = NAV_CMD_LAND;
+				struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+				//publish_navigator_mission_item(); // for logging
+				_navigator->set_position_setpoint_triplet_updated();
+
+				// issue_command(_mission_item);
+				reset_mission_item_reached();
+
+				_land_state = vtol_land_state::IDLE;
+
+				break;
+			}
+
+		default: {
+
+				break;
+			}
+		}
+	}
+}
+
+bool VtolLand::hasVtolHomeLandApproach()
+{
+	readVtolHomeLandApproachesFromStorage();
+	return _vtol_home_land_approaches.isAnyApproachValid();
+}
+
+void
+VtolLand::set_loiter_position()
+{
+
+	_land_approach = chooseBestLandingApproach();
+
+	if (PX4_ISFINITE(_land_approach.lat) && PX4_ISFINITE(_land_approach.lon)) {
+		_loiter_pos_lat_lon(0) = _land_approach.lat;
+		_loiter_pos_lat_lon(1) = _land_approach.lon;
+
+	} else {
+		_loiter_pos_lat_lon(0) = _home_pos_sub.get().lat;
+		_loiter_pos_lat_lon(1) = _home_pos_sub.get().lon;
+	}
+
+
+	_mission_item.lat  = _loiter_pos_lat_lon(0);
+	_mission_item.lon = _loiter_pos_lat_lon(1);
+	_mission_item.altitude = math::max(_home_pos_sub.get().alt + _param_return_alt_rel_m.get(),
+					   _global_pos_sub.get().alt);
+	_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+	_mission_item.force_heading = true;
+	_mission_item.autocontinue = false;
+	_mission_item.time_inside = _min_loiter_time_before_land;
+
+	_mission_item.altitude_is_relative = false;
+
+	_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
+	_mission_item.origin = ORIGIN_ONBOARD;
+
+	_navigator->get_mission_result()->finished = false;
+	_navigator->set_mission_result_updated();
+	reset_mission_item_reached();
+
+	// convert mission item to current setpoint
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	mission_apply_limitation(_mission_item);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+	_navigator->set_can_loiter_at_sp(true);
+
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+loiter_point_s VtolLand::chooseBestLandingApproach()
+{
+	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_home_land_approaches.num_approaches_max; i++) {
+
+		if (_vtol_home_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_home_land_approaches.approaches[i].lat,
+							 _vtol_home_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_home_land_approaches.approaches[min_index];
+
+	} else {
+
+		return loiter_point_s();
+	}
+}
+
+void VtolLand::readVtolHomeLandApproachesFromStorage()
+{
+
+	// 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
+	_vtol_home_land_approaches.resetAllApproaches();
+
+	if (!(_home_pos_sub.get().valid_alt && _home_pos_sub.get().valid_hpos)) {
+		return;
+	}
+
+	mission_stats_entry_s stats;
+	int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	int num_mission_items = 0;
+	bool foundHomeLandApproaches = false;
+	uint8_t sector_counter = 0;
+
+	if (ret == sizeof(mission_stats_entry_s)) {
+		num_mission_items = stats.num_items;
+	}
+
+	for (int current_seq = 1; current_seq <= num_mission_items; ++current_seq) {
+		mission_item_s mission_item{};
+
+		if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_item, sizeof(mission_item_s)) !=
+		    sizeof(mission_item_s)) {
+			PX4_ERR("dm_read failed");
+			break;
+		}
+
+		if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) {
+
+			if (foundHomeLandApproaches) {
+				break;
+			}
+
+			const float dist_to_home = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, _home_pos_sub.get().lat,
+						   _home_pos_sub.get().lon);
+
+			if (!mission_item.is_mission_rally_point && dist_to_home < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
+				foundHomeLandApproaches = true;
+				_vtol_home_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_home_land_approaches.approaches[sector_counter].lat = mission_item.lat;
+			_vtol_home_land_approaches.approaches[sector_counter].lon = mission_item.lon;
+			_vtol_home_land_approaches.approaches[sector_counter].height_m = mission_item.altitude;
+			_vtol_home_land_approaches.approaches[sector_counter].loiter_radius_m = mission_item.loiter_radius;
+			sector_counter++;
+		}
+	}
+}
+
+rtl_time_estimate_s VtolLand::calc_rtl_time_estimate()
+{
+	rtl_time_estimate_s time_estimate;
+	time_estimate.valid = false;
+	time_estimate.timestamp = hrt_absolute_time();
+
+	return time_estimate;
+}

src/modules/navigator/vtol_land.h

@@ -0,0 +1,106 @@
+/***************************************************************************
+ *
+ *   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 vtol_land.h
+ *
+ * Helper class to do a VTOL landing using a loiter down to altitude landing pattern.
+ *
+ */
+
+#pragma once
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+#include "safe_point_land.hpp"
+
+#include <lib/mathlib/mathlib.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/rtl_time_estimate.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/wind.h>
+
+#include <px4_platform_common/module_params.h>
+
+class VtolLand : public MissionBlock, public ModuleParams
+{
+public:
+	VtolLand(Navigator *navigator);
+	~VtolLand() = default;
+
+	void on_activation() override;
+	void on_active() override;
+	void on_inactive() override;
+
+	bool hasVtolHomeLandApproach();
+
+	rtl_time_estimate_s calc_rtl_time_estimate();
+private:
+
+	enum class vtol_land_state {
+		MOVE_TO_LOITER = 0,
+		LOITER_DOWN,
+		TRANSITION_TO_MC,
+		LAND,
+		IDLE
+	} _land_state;
+
+	matrix::Vector2<double> _land_pos_lat_lon;
+	matrix::Vector2<double> _loiter_pos_lat_lon;
+	loiter_point_s _land_approach;
+
+	static constexpr float _min_loiter_time_before_land = 10.0f;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::VTO_LOITER_ALT>) _param_loiter_alt,
+		(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_return_alt_rel_m,
+		(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad
+	)
+
+	void set_loiter_position();
+
+	loiter_point_s chooseBestLandingApproach();
+
+	land_approaches_s _vtol_home_land_approaches{};
+
+	void readVtolHomeLandApproachesFromStorage();
+
+	static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES =
+		10.0; // [m] We don't consider home land approaches valid if the distance from the current home to the land location is greater than this distance
+
+	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<wind_s>		_wind_sub{ORB_ID(wind)};
+
+};