[RTL] Add VTOL landing approach for safe points the same way as defined for the home position.

Perform this VTOL landing approach if optional safe point loiter position are defined

src/lib/CMakeLists.txt

@@ -53,6 +53,7 @@ add_subdirectory(l1 EXCLUDE_FROM_ALL)
 add_subdirectory(led EXCLUDE_FROM_ALL)
 add_subdirectory(matrix EXCLUDE_FROM_ALL)
 add_subdirectory(mathlib EXCLUDE_FROM_ALL)
+add_subdirectory(mission EXCLUDE_FROM_ALL)
 add_subdirectory(mixer_module EXCLUDE_FROM_ALL)
 add_subdirectory(motion_planning EXCLUDE_FROM_ALL)
 add_subdirectory(npfg EXCLUDE_FROM_ALL)

src/lib/mission/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2022 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.
+#
+############################################################################
+
+px4_add_library(planned_mission_interface
+	planned_mission_interface.cpp
+)

src/lib/mission/planned_mission_interface.cpp

@@ -0,0 +1,603 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 planned_mission_interface.cpp
+ *
+ */
+
+#include "planned_mission_interface.h"
+
+#include <stdlib.h>
+
+#include "dataman/dataman.h"
+#include "lib/geo/geo.h"
+#include "modules/navigator/mission_block.h"
+
+void PlannedMissionInterface::update()
+{
+	_home_pos_sub.update();
+
+	if (!_is_land_start_item_searched && _home_pos_sub.get().valid_alt) {
+		findLandStartItem();
+		_is_land_start_item_searched = true;
+	}
+
+	if (_mission_sub.updated()) {
+		mission_s new_mission;
+		_mission_sub.update(&new_mission);
+
+		if (isMissionValid(new_mission)) {
+			/* Check if it was updated externally*/
+			if (new_mission.timestamp > _mission.timestamp) {
+				bool mission_waypoints_changed{checkMissionWaypointsChanged(_mission, new_mission)};
+				_mission = new_mission;
+
+				if (goToItem(new_mission.current_seq, true) == EXIT_SUCCESS) {
+					if (_home_pos_sub.get().valid_alt) {
+						findLandStartItem();
+
+					} else {
+						_is_land_start_item_searched = false;
+					}
+
+					onMissionUpdate(mission_waypoints_changed);
+				}
+			}
+		}
+	}
+}
+
+void PlannedMissionInterface::getPreviousPositionItems(int32_t start_index, struct mission_item_s items[],
+		size_t &num_found_items, uint8_t max_num_items) const
+{
+	num_found_items = 0u;
+
+	int32_t next_mission_index{start_index};
+
+	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
+		if (next_mission_index < 0) {
+			break;
+		}
+
+		mission_item_s next_mission_item;
+		bool found_next_item{false};
+
+		do {
+			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == EXIT_SUCCESS;
+			next_mission_index--;
+		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
+
+		if (found_next_item) {
+			items[item_idx] = next_mission_item;
+			num_found_items = item_idx + 1;
+
+		} else {
+			break;
+		}
+	}
+}
+
+void PlannedMissionInterface::getNextPositionItems(int32_t start_index, struct mission_item_s items[],
+		size_t &num_found_items, uint8_t max_num_items) const
+{
+	// Make sure vector does not contain any preexisting elements.
+	num_found_items = 0u;
+
+	int32_t next_mission_index{start_index};
+
+	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
+		if (next_mission_index >= _mission.count) {
+			break;
+		}
+
+		mission_item_s next_mission_item;
+		bool found_next_item{false};
+
+		do {
+			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == EXIT_SUCCESS;
+			next_mission_index++;
+		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
+
+		if (found_next_item) {
+			items[item_idx] = next_mission_item;
+			num_found_items = item_idx + 1;
+
+		} else {
+			break;
+		}
+	}
+}
+
+int PlannedMissionInterface::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump,
+		bool write_jumps, bool mission_direction_backward) const
+{
+	if (mission_index >= _mission.count || mission_index < 0) {
+		return EXIT_FAILURE;
+	}
+
+	int32_t new_mission_index{mission_index};
+	mission_item_s new_mission;
+
+	for (uint16_t jump_count = 0u; jump_count < _max_jump_iteraion; jump_count++) {
+		if (readMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) {
+			PX4_ERR("Could not read next item.");
+			return EXIT_FAILURE;
+		}
+
+		if (new_mission.nav_cmd == NAV_CMD_DO_JUMP) {
+			if (new_mission.do_jump_mission_index >= _mission.count || new_mission.do_jump_mission_index < 0) {
+				PX4_ERR("Do Jump mission index is out of bounds.");
+				return EXIT_FAILURE;
+			}
+
+			if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) {
+				if (write_jumps) {
+					new_mission.do_jump_current_count++;
+
+					if (writeMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) {
+						PX4_ERR("Could not update jump count on mission item.");
+						// Still continue searching for next non jump item.
+					}
+				}
+
+				new_mission_index = new_mission.do_jump_mission_index;
+
+			} else {
+				if (mission_direction_backward) {
+					new_mission_index--;
+
+				} else {
+					new_mission_index++;
+				}
+			}
+
+		} else {
+			break;
+		}
+	}
+
+	mission_index = new_mission_index;
+	mission = new_mission;
+
+	return EXIT_SUCCESS;
+}
+
+bool PlannedMissionInterface::hasMissionLandStart() const
+{
+	return (_land_start_index != _invalid_index) && (_land_start_index < _mission.count);
+}
+
+int PlannedMissionInterface::goToNextItem(bool execute_jump)
+{
+	if (_mission.current_seq + 1 >= (_mission.count)) {
+		return EXIT_FAILURE;
+	}
+
+	return goToItem(_mission.current_seq + 1, execute_jump);
+}
+
+int PlannedMissionInterface::goToPreviousItem(bool execute_jump)
+{
+	if (_mission.current_seq <= 0) {
+		return EXIT_FAILURE;
+	}
+
+	return goToItem(_mission.current_seq - 1, execute_jump, true);
+}
+
+int PlannedMissionInterface::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward)
+{
+	mission_item_s mission_item;
+
+	if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == EXIT_SUCCESS) {
+		if (setMissionIndex(index) == EXIT_SUCCESS) {
+			_current_planned_mission_item = mission_item;
+
+		} else {
+			return EXIT_FAILURE;
+		}
+
+	} else {
+		return EXIT_FAILURE;
+	}
+
+	return EXIT_SUCCESS;
+}
+
+int PlannedMissionInterface::goToPreviousPositionItem(bool execute_jump)
+{
+	do {
+		if (goToPreviousItem(execute_jump) != EXIT_SUCCESS) {
+			return EXIT_FAILURE;
+		}
+	} while (!MissionBlock::item_contains_position(_current_planned_mission_item));
+
+	return EXIT_SUCCESS;
+}
+
+int PlannedMissionInterface::goToNextPositionItem(bool execute_jump)
+{
+	do {
+		if (goToNextItem(execute_jump) != EXIT_SUCCESS) {
+			return EXIT_FAILURE;
+		}
+	} while (!MissionBlock::item_contains_position(_current_planned_mission_item));
+
+	return EXIT_SUCCESS;
+}
+
+int PlannedMissionInterface::setMissionIndex(int32_t index)
+{
+	// Nothing to do if it is already at the current index.
+	if (index == _mission.current_seq) {
+		return EXIT_SUCCESS;
+	}
+
+	mission_s mission{_mission};
+	mission.current_seq = index;
+	mission.timestamp = hrt_absolute_time();
+
+	if (writeMission(mission) == EXIT_SUCCESS) {
+		_mission = mission;
+		return EXIT_SUCCESS;
+
+	} else {
+		return EXIT_FAILURE;
+	}
+}
+
+int32_t PlannedMissionInterface::getClosestMissionItemIndex(double lat, double lon, float alt, float home_alt,
+		const vehicle_status_s &vehicle_status)
+{
+	int32_t min_dist_index(-1);
+	float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX);
+
+	for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) {
+		mission_item_s mission;
+
+		if (readMissionItem(mission, mission_item_index) != EXIT_SUCCESS) {
+			PX4_ERR("Could not set mission closest to position.");
+			return EXIT_FAILURE;
+		}
+
+		if (MissionBlock::item_contains_position(mission)) {
+			// do not consider land waypoints for a fw
+			if (!((mission.nav_cmd == NAV_CMD_LAND) &&
+			      (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
+			      (!vehicle_status.is_vtol))) {
+				float dist = get_distance_to_point_global_wgs84(mission.lat, mission.lon,
+						MissionBlock::get_absolute_altitude_for_item(mission, home_alt),
+						lat,
+						lon,
+						alt,
+						&dist_xy, &dist_z);
+
+				if (dist < min_dist) {
+					min_dist = dist;
+					min_dist_index = mission_item_index;
+				}
+			}
+		}
+	}
+
+	return min_dist_index;
+}
+
+int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, float alt, float home_alt,
+		const vehicle_status_s &vehicle_status)
+{
+	int32_t min_dist_index(getClosestMissionItemIndex(lat, lon, alt, home_alt, vehicle_status));
+
+	return goToItem(min_dist_index, false);
+}
+
+int PlannedMissionInterface::goToMissionLandStart()
+{
+	if (!hasMissionLandStart()) {
+		return EXIT_FAILURE;
+	}
+
+	return goToItem(_land_start_index, false);
+}
+
+int PlannedMissionInterface::initMission()
+{
+	mission_s mission;
+	bool ret_val{EXIT_FAILURE};
+
+	if (readMission(mission) == EXIT_SUCCESS) {
+		_mission = mission;
+
+		if (goToItem(mission.current_seq, true) == EXIT_SUCCESS) {
+			findLandStartItem();
+			ret_val = EXIT_SUCCESS;
+		}
+
+	} else {
+		resetMission();
+	}
+
+	_mission_pub.advertise();
+
+	return ret_val;
+}
+
+void PlannedMissionInterface::resetMission()
+{
+	/* we do not need to reset mission if there is none.*/
+	if (_mission.count == 0u) {
+		return;
+	}
+
+	/* Set a new mission*/
+	mission_s new_mission{.timestamp = hrt_absolute_time(),
+			      .current_seq = 0,
+			      .count = 0u,
+			      .dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0};
+
+	if (writeMission(new_mission) == EXIT_SUCCESS) {
+		_mission = new_mission;
+
+	} else {
+		PX4_ERR("Mission Initialization failed.");
+	}
+}
+
+void PlannedMissionInterface::resetMissionJumpCounter()
+{
+	for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) {
+		mission_item_s mission_item;
+
+		if (readMissionItem(mission_item, mission_index) != EXIT_SUCCESS) {
+			PX4_ERR("Could not read mission item for jump count reset.");
+			break;
+		}
+
+		if (mission_item.nav_cmd == NAV_CMD_DO_JUMP) {
+			mission_item.do_jump_current_count = 0u;
+
+			if (writeMissionItem(mission_item, mission_index) != EXIT_SUCCESS) {
+				PX4_ERR("Could not write mission item for jump count reset.");
+				break;
+			}
+		}
+	}
+}
+
+int PlannedMissionInterface::writeMission(mission_s &mission)
+{
+	int ret_val{EXIT_SUCCESS};
+
+	if (!isMissionValid(mission)) {
+		return EXIT_FAILURE;
+	}
+
+	/* lock MISSION_STATE item */
+	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
+		return EXIT_FAILURE;
+	}
+
+	if (dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) {
+		PX4_ERR("Can't save mission state");
+		ret_val = EXIT_FAILURE;
+
+	} else {
+		_mission_pub.publish(mission);
+	}
+
+	dm_unlock(DM_KEY_MISSION_STATE);
+
+	return ret_val;
+}
+
+int PlannedMissionInterface::readMission(mission_s &read_mission) const
+{
+	int ret_val{EXIT_SUCCESS};
+
+	/* lock MISSION_STATE item */
+	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM lock failed.");
+		return EXIT_FAILURE;
+	}
+
+	mission_s mission;
+
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) {
+		PX4_ERR("Can't read mission state.");
+		ret_val = EXIT_FAILURE;
+
+	} else {
+		if (isMissionValid(mission)) {
+			read_mission = mission;
+
+		} else {
+			ret_val = EXIT_FAILURE;
+		}
+	}
+
+	dm_unlock(DM_KEY_MISSION_STATE);
+
+	return ret_val;
+}
+
+int PlannedMissionInterface::readMissionItem(mission_item_s &read_mission_item, size_t index) const
+{
+	int ret_val{EXIT_SUCCESS};
+
+	if (index >= _mission.count) {
+		return EXIT_FAILURE;
+	}
+
+	dm_item_t current_dm_item{static_cast<dm_item_t>(_mission.dataman_id)};
+
+	/* lock current mission item */
+	/*int dm_lock_ret = dm_lock(current_dm_item);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM lock failed.");
+		return EXIT_FAILURE;
+	}*/
+
+	mission_item_s mission_item;
+
+	if (dm_read(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) {
+		PX4_ERR("Can't read mission item from DM.");
+		ret_val = EXIT_FAILURE;
+
+	} else {
+		read_mission_item = mission_item;
+	}
+
+	//dm_unlock(current_dm_item);
+
+	return ret_val;
+}
+
+int PlannedMissionInterface::writeMissionItem(const mission_item_s &mission_item, size_t index) const
+{
+	int ret_val{EXIT_SUCCESS};
+
+	if (index >= _mission.count) {
+		return EXIT_FAILURE;
+	}
+
+	dm_item_t current_dm_item{static_cast<dm_item_t>(_mission.dataman_id)};
+
+	/* lock current mission item */
+	/*int dm_lock_ret = dm_lock(current_dm_item);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM lock failed.");
+		return EXIT_FAILURE;
+	}*/
+
+	if (dm_write(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) {
+		PX4_ERR("Can't write mission item to DM.");
+		ret_val = EXIT_FAILURE;
+	}
+
+	//dm_unlock(current_dm_item);
+
+	return ret_val;
+}
+
+bool PlannedMissionInterface::isMissionValid(mission_s &mission) const
+{
+	bool ret_val{false};
+
+	if ((mission.current_seq < mission.count) &&
+	    (mission.current_seq >= 0) &&
+	    (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) &&
+	    (mission.timestamp != 0u)) {
+		ret_val = true;
+
+	}
+
+	return ret_val;
+}
+
+void PlannedMissionInterface::findLandStartItem()
+{
+	_land_start_index = _invalid_index;
+	_land_index = _invalid_index;
+
+	for (size_t mission_item_index = 1u; mission_item_index < _mission.count; mission_item_index++) {
+		mission_item_s mission;
+
+		if (readMissionItem(mission, mission_item_index) == EXIT_SUCCESS) {
+			if (mission.nav_cmd == NAV_CMD_DO_LAND_START) {
+				_land_start_index = mission_item_index;
+			}
+
+			if ((mission.nav_cmd == NAV_CMD_VTOL_LAND) || (mission.nav_cmd == NAV_CMD_LAND)) {
+				_land_index = mission_item_index;
+				break;
+			}
+
+		} else {
+			break;
+		}
+	}
+
+	if (_land_start_index != _invalid_index) {
+		mission_item_s mission;
+		size_t num_found_item{0u};
+		getNextPositionItems(_land_start_index, &mission, num_found_item, 1u);
+
+		if (num_found_item == 1u) {
+			_land_start_pos.lat = mission.lat;
+			_land_start_pos.lon = mission.lon;
+			_land_start_pos.alt = mission.altitude_is_relative ?	mission.altitude +
+					      _home_pos_sub.get().alt : mission.altitude;
+
+		} else {
+			PX4_ERR("Could not read land start coordinates.");
+			_land_start_pos.lat = 0.0;
+			_land_start_pos.lon = 0.0;
+		}
+
+		_land_pos.lat = _land_start_pos.lat;
+		_land_pos.lon = _land_start_pos.lon;
+		_land_pos.alt = _land_start_pos.alt;
+	}
+
+	if (_land_index != _invalid_index) {
+		mission_item_s mission;
+
+		if (readMissionItem(mission, _land_index) == EXIT_SUCCESS) {
+			_land_pos.lat = mission.lat;
+			_land_pos.lon = mission.lon;
+			_land_pos.alt = mission.altitude_is_relative ?	mission.altitude +
+					_home_pos_sub.get().alt : mission.altitude;
+		}
+
+		if (_land_start_index == _invalid_index) {
+			_land_start_index = _land_index;
+			_land_start_pos.lat = _land_pos.lat;
+			_land_start_pos.lon = _land_pos.lon;
+			_land_start_pos.alt = _land_pos.alt;
+		}
+	}
+}
+
+bool PlannedMissionInterface::checkMissionWaypointsChanged(const mission_s &old_mission,
+		const mission_s &new_mission) const
+{
+	return (new_mission.count != old_mission.count) || (new_mission.dataman_id != old_mission.dataman_id);
+}

src/lib/mission/planned_mission_interface.h

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 planned_mission_interface.h
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/uORB.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include "navigator/navigation.h"
+
+class PlannedMissionInterface
+{
+public:
+	void update();
+	void getPreviousPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items,
+				      uint8_t max_num_items) const;
+	void getNextPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items,
+				  uint8_t max_num_items) const;
+	bool hasMissionLandStart() const;
+	int goToNextItem(bool execute_jump);
+	int goToPreviousItem(bool execute_jump);
+	int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false);
+	int goToPreviousPositionItem(bool execute_jump);
+	int goToNextPositionItem(bool execute_jump);
+	int goToMissionLandStart();
+	int32_t getClosestMissionItemIndex(double lat, double lon, float alt, float home_alt,
+					   const vehicle_status_s &vehicle_status);
+	int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status);
+	virtual void onMissionUpdate(bool has_mission_items_changed) = 0;
+
+	int initMission();
+	void resetMission();
+	void resetMissionJumpCounter();
+
+private:
+	int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps,
+			   bool mission_direction_backward = false) const;
+	int setMissionIndex(int32_t index);
+	int writeMission(mission_s &mission);
+	int readMission(mission_s &read_mission) const;
+	int readMissionItem(mission_item_s &read_mission_item, size_t index) const;
+	int writeMissionItem(const mission_item_s &mission_item, size_t index) const ;
+	bool isMissionValid(mission_s &mission) const;
+	void findLandStartItem();
+	bool checkMissionWaypointsChanged(const mission_s &old_mission, const mission_s &new_mission) const;
+public:
+	static const uint16_t _invalid_index{UINT16_MAX};
+private:
+	static const uint16_t _max_jump_iteraion{10u};
+protected:
+	mission_s _mission;
+	mission_item_s _current_planned_mission_item;
+	uint16_t _land_start_index;
+	uint16_t _land_index;
+	struct {
+		double lat;
+		double lon;
+		float alt;
+	} _land_start_pos{.lat = 0.0,
+				  .lon = 0.0,
+				  .alt = 0.0f},
+			       _land_pos{.lat = 0.0,
+					 .lon = 0.0,
+					 .alt = 0.0f};
+	uORB::Subscription _mission_sub{ORB_ID(mission)};
+	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
+	uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
+private:
+	bool _is_land_start_item_searched{false};
+};

src/modules/dataman/dataman.cpp

@@ -158,7 +158,7 @@ static unsigned g_func_counts[dm_number_of_funcs];
 
 /* table of maximum number of instances for each item type */
 static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
-	DM_KEY_SAFE_POINTS_MAX,
+	DM_KEY_SAFE_POINT_ITEMS_MAX,
 	DM_KEY_FENCE_POINTS_MAX,
 	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
 	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
@@ -170,7 +170,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
 
 /* Table of the len of each item type */
 static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = {
-	sizeof(struct mission_safe_point_s) + DM_SECTOR_HDR_SIZE,
+	sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE,
 	sizeof(struct mission_fence_point_s) + DM_SECTOR_HDR_SIZE,
 	sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE,
 	sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE,

src/modules/dataman/dataman.h

@@ -59,7 +59,7 @@ typedef enum {
 
 #if defined(MEMORY_CONSTRAINED_SYSTEM)
 enum {
-	DM_KEY_SAFE_POINTS_MAX = 8,
+	DM_KEY_SAFE_POINT_ITEMS_MAX = 8,
 	DM_KEY_FENCE_POINTS_MAX = 16,
 	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
 	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
@@ -69,7 +69,7 @@ enum {
 #else
 /** The maximum number of instances for each item type */
 enum {
-	DM_KEY_SAFE_POINTS_MAX = 8,
+	DM_KEY_SAFE_POINT_ITEMS_MAX = 64,
 	DM_KEY_FENCE_POINTS_MAX = 64,
 	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
 	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
@@ -83,11 +83,11 @@ struct dataman_compat_s {
 };
 
 /* increment this define whenever a binary incompatible change is performed */
-#define DM_COMPAT_VERSION	2ULL
+#define DM_COMPAT_VERSION	3ULL
 
 #define DM_COMPAT_KEY ((DM_COMPAT_VERSION << 32) + (sizeof(struct mission_item_s) << 24) + \
 		       (sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \
-		       (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_safe_point_s) << 4) + \
+		       (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_item_s) << 4) + \
 		       sizeof(struct dataman_compat_s))
 
 /** Retrieve from the data manager store */

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -2170,15 +2170,18 @@ FixedwingPositionControl::Run()
 
 				_position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat)
 								    && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
-								    && PX4_ISFINITE(_pos_sp_triplet.previous.alt);
+								    && PX4_ISFINITE(_pos_sp_triplet.previous.alt)
+								    && _pos_sp_triplet.previous.valid;
 
 				_position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat)
 								   && PX4_ISFINITE(_pos_sp_triplet.current.lon)
-								   && PX4_ISFINITE(_pos_sp_triplet.current.alt);
+								   && PX4_ISFINITE(_pos_sp_triplet.current.alt)
+								   && _pos_sp_triplet.current.valid;
 
 				_position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat)
 								&& PX4_ISFINITE(_pos_sp_triplet.next.lon)
-								&& PX4_ISFINITE(_pos_sp_triplet.next.alt);
+								&& PX4_ISFINITE(_pos_sp_triplet.next.alt)
+								&& _pos_sp_triplet.next.valid;
 
 				// reset the altitude foh (first order hold) logic
 				_min_current_sp_distance_xy = FLT_MAX;

src/modules/mavlink/mavlink_mission.cpp

@@ -331,15 +331,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		break;
 
 	case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
-			mission_safe_point_s mission_safe_point;
-			bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s));
-			read_success = (bytes_read == sizeof(mission_safe_point_s));
-
-			mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT;
-			mission_item.frame = mission_safe_point.frame;
-			mission_item.lat = mission_safe_point.lat;
-			mission_item.lon = mission_safe_point.lon;
-			mission_item.altitude = mission_safe_point.alt;
+			bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_item, sizeof(mission_item_s));
+			read_success = (bytes_read == sizeof(mission_item_s));
 		}
 		break;
 
@@ -1175,13 +1168,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 			break;
 
 		case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
-				mission_safe_point_s mission_safe_point;
-				mission_safe_point.lat = mission_item.lat;
-				mission_safe_point.lon = mission_item.lon;
-				mission_safe_point.alt = mission_item.altitude;
-				mission_safe_point.frame = mission_item.frame;
-				write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point,
-							sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s);
+				write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_item,
+							sizeof(mission_item)) != sizeof(mission_item);
 			}
 			break;
 
@@ -1462,6 +1450,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 
 		case MAV_CMD_NAV_RALLY_POINT:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->is_mission_rally_point = (mavlink_mission_item->param1 > 0.0f);
 			break;
 
 		default:
@@ -1746,6 +1735,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 			break;
 
 		case MAV_CMD_NAV_RALLY_POINT:
+			mavlink_mission_item->param1 = mission_item->is_mission_rally_point ? 1 : 0;
 			break;
 
 

src/modules/mavlink/mavlink_mission.h

@@ -143,7 +143,7 @@ private:
 	static constexpr uint16_t	MAX_COUNT[] = {
 		DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
 		DM_KEY_FENCE_POINTS_MAX - 1,
-		DM_KEY_SAFE_POINTS_MAX - 1
+		DM_KEY_SAFE_POINT_ITEMS_MAX - 1
 	};	/**< Maximum number of mission items for each type
 					(fence & safe points use the first item for the stats) */
 

src/modules/navigator/CMakeLists.txt

@@ -43,14 +43,19 @@ px4_add_module(
 		mission.cpp
 		loiter.cpp
 		rtl.cpp
+		rtl_direct.cpp
+		rtl_mission_fast.cpp
+		rtl_mission_fast_reverse.cpp
 		takeoff.cpp
 		land.cpp
 		precland.cpp
 		mission_feasibility_checker.cpp
 		geofence.cpp
+		vtol_land.cpp
 		vtol_takeoff.cpp
 	DEPENDS
 		geo
 		geofence_breach_avoidance
 		motion_planning
+		planned_mission_interface
 	)

src/modules/navigator/mission.h

@@ -52,6 +52,8 @@
 
 #include <float.h>
 
+#include "lib/mission/planned_mission_interface.h"
+
 #include <dataman/dataman.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
@@ -62,58 +64,45 @@
 #include <uORB/topics/navigator_mission_item.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_roi.h>
 #include <uORB/uORB.h>
 
 class Navigator;
 
-class Mission : public MissionBlock, public ModuleParams
+class MissionBase : public MissionBlock, public ModuleParams, protected PlannedMissionInterface
 {
 public:
-	Mission(Navigator *navigator);
-	~Mission() override = default;
+	MissionBase(Navigator *navigator);
+	~MissionBase() override = default;
 
-	void on_inactive() override;
-	void on_inactivation() override;
-	void on_activation() override;
-	void on_active() override;
+	virtual void on_inactive() override;
+	virtual void on_inactivation() override;
+	virtual void on_activation() override;
+	virtual void on_active() override;
 
-	bool set_current_mission_index(uint16_t index);
+protected:
+	// Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message
+	enum class WorkItemType {
+		WORK_ITEM_TYPE_DEFAULT,		/**< default mission item */
+		WORK_ITEM_TYPE_TAKEOFF,		/**< takeoff before moving to waypoint */
+		WORK_ITEM_TYPE_MOVE_TO_LAND,	/**< move to land waypoint before descent */
+		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
+		WORK_ITEM_TYPE_TRANSITON,
+		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
+		WORK_ITEM_TYPE_PRECISION_LAND
+	} _work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};	/**< current type of work to do (sub mission item) */
 
-	bool land_start();
-	bool landing();
-
-	uint16_t get_land_start_index() const { return _land_start_index; }
-	bool get_land_start_available() const { return _land_start_available; }
-	bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; }
-	bool get_mission_changed() const { return _mission_changed ; }
-	bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; }
-	double get_landing_start_lat() { return _landing_start_lat; }
-	double get_landing_start_lon() { return _landing_start_lon; }
-	float get_landing_start_alt() { return _landing_start_alt; }
-
-	double get_landing_lat() { return _landing_lat; }
-	double get_landing_lon() { return _landing_lon; }
-	float get_landing_alt() { return _landing_alt; }
-	float get_landing_loiter_rad() { return _landing_loiter_radius; }
-
-	void set_closest_item_as_current();
-
-	/**
-	 * Set a new mission mode and handle the switching between the different modes
-	 *
-	 * For a list of the different modes refer to mission_result.msg
-	 */
-	void set_execution_mode(const uint8_t mode);
-private:
-
-	void mission_init();
+	enum class MissionType {
+		MISSION_TYPE_NONE,
+		MISSION_TYPE_MISSION
+	} _mission_type{MissionType::MISSION_TYPE_NONE};
 
 	/**
 	 * Update mission topic
 	 */
-	void update_mission();
+	virtual void update_mission();
 
 	/**
 	 * Move on to next mission item or switch to loiter
@@ -129,34 +118,51 @@ private:
 	void set_mission_items();
 
 	/**
-	 * Returns true if we need to do a takeoff at the current state
+	 * Set the mission result
 	 */
-	bool do_need_vertical_takeoff();
+	void set_mission_result();
+
+	virtual void setActiveMissionItems() = 0;
+	virtual bool setNextMissionItem() = 0;
+	void setEndOfMissionItems();
+	void publish_navigator_mission_item();
+	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
+
+	hrt_abstime _time_mission_deactivated{0};
+
+	bool _is_current_planned_mission_item_valid{false};
+	bool _is_mission_valid{false};
+	bool _mission_has_been_activated{false};
+	bool _initialized_mission_checked{false};
+	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
+	bool _system_disarmed_while_inactive{false};
+
+	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_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)};	/**< global position subscription */
+	uORB::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item};
+private:
+	/**
+	 * On mission update
+	 * Change behaviour after external mission update.
+	 * @param[in] has_mission_items_changed flag if the mission items have been changed.
+	 */
+	void onMissionUpdate(bool has_mission_items_changed) override;
 
 	/**
-	 * Returns true if we need to move to waypoint location before starting descent
+	 * Check whether a mission is ready to go
 	 */
-	bool do_need_move_to_land();
+	void check_mission_valid();
 
 	/**
-	 * Returns true if we need to move to waypoint location after vtol takeoff
+	 * Reset mission
 	 */
-	bool do_need_move_to_takeoff();
+	void checkMissionRestart();
 
 	/**
-	 * Copies position from setpoint if valid, otherwise copies current position
+	 * Set a mission item as reached
 	 */
-	void copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint);
-
-	/**
-	 * Create mission item to align towards next waypoint
-	 */
-	void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next);
-
-	/**
-	 * Calculate takeoff height for mission item considering ground clearance
-	 */
-	float calculate_takeoff_altitude(struct mission_item_s *mission_item);
+	void set_mission_item_reached();
 
 	/**
 	 * Updates the heading of the vehicle. Rotary wings only.
@@ -173,125 +179,65 @@ private:
 	 */
 	void do_abort_landing();
 
-	/**
-	 * Read the current and the next mission item. The next mission item read is the
-	 * next mission item that contains a position.
-	 *
-	 * @return true if current mission item available
-	 */
-	bool prepare_mission_items(mission_item_s *mission_item,
-				   mission_item_s *next_position_mission_item, bool *has_next_position_item,
-				   mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr);
-
-	/**
-	 * Read current (offset == 0) or a specific (offset > 0) mission item
-	 * from the dataman and watch out for DO_JUMPS
-	 *
-	 * @return true if successful
-	 */
-	bool read_mission_item(int offset, struct mission_item_s *mission_item);
-
-	/**
-	 * Save current mission state to dataman
-	 */
-	void save_mission_state();
-
 	/**
 	 * Inform about a changed mission item after a DO_JUMP
 	 */
 	void report_do_jump_mission_changed(int index, int do_jumps_remaining);
 
 	/**
-	 * Set a mission item as reached
+	 * Set the Camera Trigger
+	 * Enable or disable the camera trigger for a mission.
+	 * @param enable flag if the camera trigger should be enabled.
 	 */
-	void set_mission_item_reached();
-
-	/**
-	 * Set the current mission item
-	 */
-	void set_current_mission_item();
-
-	/**
-	 * Check whether a mission is ready to go
-	 */
-	void check_mission_valid(bool force);
-
-	/**
-	 * Reset mission
-	 */
-	void reset_mission(struct mission_s &mission);
-
-	/**
-	 * Returns true if we need to reset the mission (call this only when inactive)
-	 */
-	bool need_to_reset_mission();
-
-	/**
-	 * Find and store the index of the landing sequence (DO_LAND_START)
-	 */
-	bool find_mission_land_start();
-
-	/**
-	 * Return the index of the closest mission item to the current global position.
-	 */
-	int32_t index_closest_mission_item() const;
-
-	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
-
-	void publish_navigator_mission_item();
+	void setCameraTrigger(bool enable);
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
 		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
 		(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
 	)
-
-	uORB::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item};
-
-	uORB::Subscription	_mission_sub{ORB_ID(mission)};		/**< mission subscription */
-	mission_s		_mission {};
-
-	int32_t _current_mission_index{-1};
-
-	// track location of planned mission landing
-	bool	_land_start_available{false};
-	uint16_t _land_start_index{UINT16_MAX};		/**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */
-	double _landing_start_lat{0.0};
-	double _landing_start_lon{0.0};
-	float _landing_start_alt{0.0f};
-
-	double _landing_lat{0.0};
-	double _landing_lon{0.0};
-	float _landing_alt{0.0f};
-
-	float _landing_loiter_radius{0.f};
-
-	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
-
-	hrt_abstime _time_mission_deactivated{0};
-
-	enum {
-		MISSION_TYPE_NONE,
-		MISSION_TYPE_MISSION
-	} _mission_type{MISSION_TYPE_NONE};
-
-	bool _inited{false};
-	bool _home_inited{false};
-	bool _need_mission_reset{false};
-	bool _mission_waypoints_changed{false};
-	bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */
-
-	// Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message
-	enum work_item_type {
-		WORK_ITEM_TYPE_DEFAULT,		/**< default mission item */
-		WORK_ITEM_TYPE_TAKEOFF,		/**< takeoff before moving to waypoint */
-		WORK_ITEM_TYPE_MOVE_TO_LAND,	/**< move to land waypoint before descent */
-		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
-		WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
-		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
-		WORK_ITEM_TYPE_PRECISION_LAND
-	} _work_item_type{WORK_ITEM_TYPE_DEFAULT};	/**< current type of work to do (sub mission item) */
-
-	uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL};	/**< the current mode of how the mission is executed,look at mission_result.msg for the definition */
-	bool _execution_mode_changed{false};
+};
+
+class Mission : public MissionBase
+{
+public:
+	Mission(Navigator *navigator);
+	~Mission() = default;
+
+	bool set_current_mission_index(uint16_t index);
+
+	uint16_t get_land_start_index() const { return _land_start_index; }
+	bool get_land_start_available() const { return _land_start_index != _invalid_index; }
+
+private:
+	bool setNextMissionItem() override;
+
+	/**
+	 * Returns true if we need to do a takeoff at the current state
+	 */
+	bool do_need_vertical_takeoff();
+
+	/**
+	 * Returns true if we need to move to waypoint location before starting descent
+	 */
+	bool do_need_move_to_land();
+
+	/**
+	 * Returns true if we need to move to waypoint location after vtol takeoff
+	 */
+	bool do_need_move_to_takeoff();
+
+	/**
+	 * Calculate takeoff height for mission item considering ground clearance
+	 */
+	float calculate_takeoff_altitude(struct mission_item_s *mission_item);
+
+	void setActiveMissionItems() override;
+
+	void handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items);
+
+	void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items);
+
+	void handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
+				  size_t &num_found_items);
 };

src/modules/navigator/mission_block.cpp

@@ -924,15 +924,51 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
 
 float
 MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const
+{
+	return get_absolute_altitude_for_item(mission_item, _navigator->get_home_position()->alt);
+}
+
+float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt)
 {
 	if (mission_item.altitude_is_relative) {
-		return mission_item.altitude + _navigator->get_home_position()->alt;
+		return mission_item.altitude + home_alt;
 
 	} else {
 		return mission_item.altitude;
 	}
 }
 
+void MissionBlock::copy_position_if_valid(struct mission_item_s *const mission_item,
+		const struct position_setpoint_s *const setpoint) const
+{
+	if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+		mission_item->lat = setpoint->lat;
+		mission_item->lon = setpoint->lon;
+		mission_item->altitude = setpoint->alt;
+
+	} else {
+		mission_item->lat = _navigator->get_global_position()->lat;
+		mission_item->lon = _navigator->get_global_position()->lon;
+		mission_item->altitude = _navigator->get_global_position()->alt;
+	}
+
+	mission_item->altitude_is_relative = false;
+}
+
+void MissionBlock::set_align_mission_item(struct mission_item_s *const mission_item,
+		const struct mission_item_s *const mission_item_next) const
+{
+	mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+	copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current));
+	mission_item->altitude_is_relative = false;
+	mission_item->autocontinue = true;
+	mission_item->time_inside = 0.0f;
+	mission_item->yaw = get_bearing_to_next_waypoint(
+				    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				    mission_item_next->lat, mission_item_next->lon);
+	mission_item->force_heading = true;
+}
+
 void
 MissionBlock::initialize()
 {

src/modules/navigator/mission_block.h

@@ -99,6 +99,8 @@ public:
 	 */
 	static bool item_contains_marker(const mission_item_s &item);
 
+	static float get_absolute_altitude_for_item(const  mission_item_s &mission_item, float home_alt);
+
 	/**
 	 * @brief Set the payload deployment successful flag object
 	 *
@@ -125,6 +127,18 @@ public:
 		_payload_deploy_timeout_s = timeout_s;
 	}
 
+	/**
+	 * Copies position from setpoint if valid, otherwise copies current position
+	 */
+	void copy_position_if_valid(struct mission_item_s *const mission_item,
+				    const struct position_setpoint_s *const setpoint) const;
+
+	/**
+	 * Create mission item to align towards next waypoint
+	 */
+	void set_align_mission_item(struct mission_item_s *const mission_item,
+				    const struct mission_item_s *const mission_item_next) const;
+
 protected:
 	/**
 	 * Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items)

src/modules/navigator/navigation.h

@@ -105,6 +105,7 @@ enum NAV_CMD {
 	NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION = 5002,
 	NAV_CMD_FENCE_CIRCLE_INCLUSION = 5003,
 	NAV_CMD_FENCE_CIRCLE_EXCLUSION = 5004,
+	NAV_CMD_RALLY_POINT = 5100,
 	NAV_CMD_CONDITION_GATE = 4501,
 	NAV_CMD_DO_WINCH = 42600,
 	NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
@@ -137,6 +138,19 @@ enum NAV_FRAME {
 #pragma GCC diagnostic error "-Wpadded"
 #endif // GCC >= 5 || Clang
 
+/**
+ * @brief Landing position.
+ * Defines the global 3D position and landing yaw.
+ *
+ */
+struct LandingPosition_s {
+	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].*/
+};
+
+
 /**
  * Mission Item structure
  *
@@ -160,6 +174,7 @@ struct mission_item_s {
 			union {
 				float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
 				float circle_radius;		/**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */
+				bool is_mission_rally_point;	/**< only used for NAV_CMD_RALLY_POINT */
 			};
 			float acceptance_radius;		/**< default radius in which the mission is accepted as reached in meters */
 			float loiter_radius;			/**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */
@@ -227,19 +242,6 @@ struct mission_fence_point_s {
 	uint8_t _padding0[5];				/**< padding struct size to alignment boundary  */
 };
 
-/**
- * Safe Point (Rally Point).
- * Corresponds to the DM_KEY_SAFE_POINTS dataman item
- */
-struct mission_safe_point_s {
-	double lat;
-	double lon;
-	float alt;
-	uint8_t frame;					/**< MAV_FRAME */
-
-	uint8_t _padding0[3];				/**< padding struct size to alignment boundary  */
-};
-
 #if (__GNUC__ >= 5) || __clang__
 #pragma GCC diagnostic pop
 #endif // GCC >= 5 || Clang

src/modules/navigator/navigator.h

@@ -272,31 +272,12 @@ public:
 
 	void set_mission_failure_heading_timeout();
 
-	void setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; }
-
-	bool getMissionLandingInProgress() { return _mission_landing_in_progress; }
-
-	bool is_planned_mission() const { return _navigation_mode == &_mission; }
-
-	bool on_mission_landing() { return _mission.landing(); }
-
-	bool start_mission_landing() { return _mission.land_start(); }
-
 	bool get_mission_start_land_available() { return _mission.get_land_start_available(); }
 
 	int  get_mission_landing_index() { return _mission.get_land_start_index(); }
 
-	double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); }
-	double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); }
-	float  get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); }
-
-	double get_mission_landing_lat() { return _mission.get_landing_lat(); }
-	double get_mission_landing_lon() { return _mission.get_landing_lon(); }
-	float  get_mission_landing_alt() { return _mission.get_landing_alt(); }
-
-	float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); }
-
 	// RTL
+
 	bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
 
 	bool abort_landing();
@@ -410,9 +391,6 @@ private:
 	float _mission_cruising_speed_fw{-1.0f};
 	float _mission_throttle{NAN};
 
-	bool _mission_landing_in_progress{false};	/**< this flag gets set if the mission is currently executing on a landing pattern
-							 * if mission mode is inactive, this flag will be cleared after 2 seconds */
-
 	traffic_buffer_s _traffic_buffer{};
 
 	bool _is_capturing_images{false}; // keep track if we need to stop capturing images

src/modules/navigator/navigator_main.cpp

@@ -548,7 +548,9 @@ void Navigator::run()
 				/* find NAV_CMD_DO_LAND_START in the mission and
 				 * use MAV_CMD_MISSION_START to start the mission there
 				 */
-				if (_mission.land_start()) {
+				uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED};
+
+				if (_mission.get_land_start_available()) {
 					vehicle_command_s vcmd = {};
 					vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
 					vcmd.param1 = _mission.get_land_start_index();
@@ -556,9 +558,10 @@ void Navigator::run()
 
 				} else {
 					PX4_WARN("planned mission landing not available");
+					result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED;
 				}
 
-				publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				publish_vehicle_command_ack(cmd, result);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
 				if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) {
@@ -652,7 +655,6 @@ void Navigator::run()
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 			_pos_sp_triplet_published_invalid_once = false;
 
-			_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL);
 			navigation_mode_new = &_mission;
 
 			break;
@@ -662,109 +664,19 @@ void Navigator::run()
 			navigation_mode_new = &_loiter;
 			break;
 
-		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: {
-				_pos_sp_triplet_published_invalid_once = false;
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+			_pos_sp_triplet_published_invalid_once = false;
 
-				const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_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;
 
-				switch (_rtl.get_rtl_type()) {
-				case RTL::RTL_TYPE_MISSION_LANDING:
-				case RTL::RTL_TYPE_CLOSEST:
-
-					if (!rtl_activated && _rtl.getRTLState() > RTL::RTLState::RTL_STATE_LOITER
-					    && _rtl.getShouldEngageMissionForLanding()) {
-						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
-
-						if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED
-						    && !get_land_detected()->landed) {
-							start_mission_landing();
-						}
-
-						navigation_mode_new = &_mission;
-
-					} else {
-						navigation_mode_new = &_rtl;
-					}
-
-					break;
-
-				case RTL::RTL_TYPE_MISSION_LANDING_REVERSED:
-					if (_mission.get_land_start_available() && !get_land_detected()->landed) {
-						// the mission contains a landing spot
-						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
-
-						if (_navigation_mode != &_mission) {
-							if (_navigation_mode == nullptr) {
-								// switching from an manual mode, go to landing if not already landing
-								if (!on_mission_landing()) {
-									start_mission_landing();
-								}
-
-							} else {
-								// switching from an auto mode, continue the mission from the closest item
-								_mission.set_closest_item_as_current();
-							}
-						}
-
-						if (rtl_activated) {
-							mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t");
-							events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info,
-								     "RTL Mission activated, continue mission");
-						}
-
-						navigation_mode_new = &_mission;
-
-					} else {
-						// fly the mission in reverse if switching from a non-manual mode
-						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE);
-
-						if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) &&
-						    (! _mission.get_mission_finished()) &&
-						    (!get_land_detected()->landed)) {
-							// determine the closest mission item if switching from a non-mission mode, and we are either not already
-							// mission mode or the mission waypoints changed.
-							// The seconds condition is required so that when no mission was uploaded and one is available the closest
-							// mission item is determined and also that if the user changes the active mission index while rtl is active
-							// always that waypoint is tracked first.
-							if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) {
-								_mission.set_closest_item_as_current();
-							}
-
-							if (rtl_activated) {
-								mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t");
-								events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info,
-									     "RTL Mission activated, fly mission in reverse");
-							}
-
-							navigation_mode_new = &_mission;
-
-						} else {
-							if (rtl_activated) {
-								mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t");
-								events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info,
-									     "RTL Mission activated, fly to home");
-							}
-
-							navigation_mode_new = &_rtl;
-						}
-					}
-
-					break;
-
-				default:
-					if (rtl_activated) {
-						mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t");
-						events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated");
-					}
-
-					navigation_mode_new = &_rtl;
-					break;
-
-				}
-
-				break;
+			} else {
+				navigation_mode_new = &_rtl;
 			}
 
+			break;
+
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = &_takeoff;

src/modules/navigator/navigator_mode.cpp

@@ -55,7 +55,6 @@ NavigatorMode::run(bool active)
 {
 	if (active) {
 		if (!_active) {
-			_navigator->set_mission_result_updated();
 			on_activation();
 
 		} else {

src/modules/navigator/navigator_mode.h

@@ -54,6 +54,8 @@ public:
 
 	void run(bool active);
 
+	bool isActive() {return _active;};
+
 	/**
 	 * This function is called while the mode is inactive
 	 */

src/modules/navigator/rtl.h

@@ -41,58 +41,36 @@
 
 #pragma once
 
+#include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
 
 #include "navigator_mode.h"
-#include "mission_block.h"
+#include "navigation.h"
+#include "lib/mission/planned_mission_interface.h"
+#include "rtl_direct.h"
+#include "rtl_mission_fast.h"
+#include "rtl_mission_fast_reverse.h"
+#include "vtol_land.h"
 
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/home_position.h>
+#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/rtl_time_estimate.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/wind.h>
-#include <matrix/math.hpp>
-#include <lib/geo/geo.h>
 
 class Navigator;
 
-class RTL : public MissionBlock, public ModuleParams
+class RTL : public NavigatorMode, protected PlannedMissionInterface, public ModuleParams
 {
 public:
 	RTL(Navigator *navigator);
 
 	~RTL() = default;
 
-	enum RTLType {
-		RTL_TYPE_HOME_OR_RALLY = 0,
-		RTL_TYPE_MISSION_LANDING,
-		RTL_TYPE_MISSION_LANDING_REVERSED,
-		RTL_TYPE_CLOSEST,
-	};
-
-	enum RTLDestinationType {
-		RTL_DESTINATION_HOME = 0,
-		RTL_DESTINATION_MISSION_LANDING,
-		RTL_DESTINATION_SAFE_POINT,
-	};
-
-	enum RTLHeadingMode {
-		RTL_NAVIGATION_HEADING = 0,
-		RTL_DESTINATION_HEADING,
-		RTL_CURRENT_HEADING,
-	};
-
-	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 RtlType {
+		RTL_DIRECT,
+		RTL_DIRECT_SAFE_VTOL,
+		RTL_MISSION_FAST,
+		RTL_MISSION_FAST_REVERSE,
+		VTOL_LAND
 	};
 
 	void on_inactivation() override;
@@ -100,97 +78,73 @@ public:
 	void on_activation() override;
 	void on_active() override;
 
-	void find_RTL_destination();
+	void initialize() override {};
 
-	void set_return_alt_min(bool min) { _rtl_alt_min = min; }
-
-	int get_rtl_type() const { return _param_rtl_type.get(); }
-
-	void get_rtl_xy_z_speed(float &xy, float &z);
-
-	matrix::Vector2f get_wind();
-
-	RTLState getRTLState() { return _rtl_state; }
-
-	bool getShouldEngageMissionForLanding() const { return _should_engange_mission_for_landing; }
+	void set_return_alt_min(bool min) { _enforce_rtl_alt = min; }
 
 private:
-
-	void set_rtl_item();
-
-	void advance_rtl();
-
-	float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
-	void calc_and_pub_rtl_time_estimate(const RTLState rtl_state);
-
-	float getCruiseGroundSpeed();
-
-	float getClimbRate();
-
-	float getDescendRate();
-
-	float getCruiseSpeed();
-
-	float getHoverLandSpeed();
-
-	RTLState _rtl_state{RTL_STATE_NONE};
-
-	struct RTLPosition {
-		double lat;
-		double lon;
-		float alt;
-		float yaw;
-		uint8_t safe_point_index; ///< 0 = home position, 1 = mission landing, >1 = safe landing points (rally points)
-		RTLDestinationType type{RTL_DESTINATION_HOME};
-
-		void set(const home_position_s &home_position)
-		{
-			lat = home_position.lat;
-			lon = home_position.lon;
-			alt = home_position.alt;
-			yaw = home_position.yaw;
-			safe_point_index = 0;
-			type = RTL_DESTINATION_HOME;
-		}
+	enum class DestinationType {
+		DESTINATION_TYPE_HOME,
+		DESTINATION_TYPE_MISSION_LAND,
+		DESTINATION_TYPE_SAFE_POINT,
 	};
 
-	RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point)
+private:
+	void onMissionUpdate(bool has_mission_items_changed) override {};
+
+	void setRtlTypeAndDestination();
+
+	/**
+	 * @brief Find RTL destination.
+	 *
+	 */
+	void findRtlDestination(DestinationType &destination_type, LandingPosition_s &rtl_position, float &rtl_alt);
+
+	/**
+	 * @brief Set the position of the land start marker in the planned mission as destination.
+	 *
+	 */
+	void setLandPosAsDestination(LandingPosition_s &rtl_position);
+
+	/**
+	 * @brief Set the safepoint as destination.
+	 *
+	 * @param mission_safe_point is the mission safe point/rally point to set as destination.
+	 */
+	void setSafepointAsDestination(LandingPosition_s &rtl_position, const mission_item_s &mission_safe_point);
+
+	/**
+	 * @brief
+	 *
+	 * @param cone_half_angle_deg
+	 * @return float
+	 */
+	float calculate_return_alt_from_cone_half_angle(const LandingPosition_s &rtl_position, float cone_half_angle_deg);
 
 	hrt_abstime _destination_check_time{0};
 
-	float _rtl_alt{0.0f};	// AMSL altitude at which the vehicle should return to the home position
+	RtlType _rtl_type{RtlType::RTL_DIRECT};
 
-	bool _rtl_alt_min{false};
-	bool _should_engange_mission_for_landing{false};
+	RtlDirect _rtl_direct;
+
+	RtlMissionFast _rtl_mission;
+
+	RtlMissionFastReverse _rtl_mission_reverse;
+
+	VtolLand _rtl_vtol_land;
+
+	bool _enforce_rtl_alt{false};
 
 	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,
 		(ParamInt<px4::params::RTL_TYPE>)          _param_rtl_type,
 		(ParamInt<px4::params::RTL_CONE_ANG>)      _param_rtl_cone_half_angle_deg,
-		(ParamInt<px4::params::RTL_PLD_MD>)        _param_rtl_pld_md,
-		(ParamFloat<px4::params::RTL_LOITER_RAD>)  _param_rtl_loiter_rad,
-		(ParamInt<px4::params::RTL_HDG_MD>)        _param_rtl_hdg_md,
-		(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,
-		(ParamInt<px4::params::RTL_TIME_MARGIN>)   _param_rtl_time_margin
+		(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
 	)
 
-	param_t		_param_mpc_z_v_auto_up{PARAM_INVALID};
-	param_t		_param_mpc_z_v_auto_dn{PARAM_INVALID};
-	param_t		_param_mpc_land_speed{PARAM_INVALID};
-	param_t		_param_fw_climb_rate{PARAM_INVALID};
-	param_t		_param_fw_sink_rate{PARAM_INVALID};
+	uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)};	/**< global position subscription */
+	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};	/**< vehicle status subscription */
 
-	param_t 	_param_fw_airspeed_trim{PARAM_INVALID};
-	param_t 	_param_mpc_xy_cruise{PARAM_INVALID};
-	param_t 	_param_rover_cruise_speed{PARAM_INVALID};
-
-	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};
 	uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
 };
-
-float time_to_home(const matrix::Vector3f &to_home_vec,
-		   const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s,
-		   float vehicle_descent_speed_m_s);

src/modules/navigator/rtl_direct.cpp

@@ -0,0 +1,714 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2020 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 rtl.cpp
+ *
+ * Helper class to access RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Kent <julian@auterion.com>
+ */
+
+#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)
+{
+	_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");
+	_param_fw_climb_rate = param_find("FW_T_CLMB_R_SP");
+	_param_fw_sink_rate = param_find("FW_T_SINK_R_SP");
+	_param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM");
+	_param_mpc_xy_cruise = param_find("MPC_XY_CRUISE");
+	_param_rover_cruise_speed = param_find("GND_SPEED_THR_SC");
+}
+
+void RtlDirect::on_activation(bool enforce_rtl_alt)
+{
+	_global_pos_sub.update();
+	_local_pos_sub.update();
+	_land_detected_sub.update();
+	_vehicle_status_sub.update();
+	_wind_sub.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;
+	}
+
+	// reset cruising speed and throttle to default for RTL
+	_navigator->set_cruising_speed();
+	_navigator->set_cruising_throttle();
+
+	set_rtl_item();
+
+	MissionBlock::on_active();
+}
+
+void RtlDirect::on_active()
+{
+	_global_pos_sub.update();
+	_local_pos_sub.update();
+	_land_detected_sub.update();
+	_vehicle_status_sub.update();
+	_wind_sub.update();
+
+	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) {
+		advance_rtl();
+		set_rtl_item();
+	}
+
+	if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) {
+		// Need to update the position and type on the current setpoint triplet.
+		_navigator->get_precland()->on_active();
+	}
+}
+
+void RtlDirect::set_rtl_item()
+{
+	_navigator->set_can_loiter_at_sp(false);
+
+	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 RTLHeadingMode rtl_heading_mode = static_cast<RTLHeadingMode>(_param_rtl_hdg_md.get());
+
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB: {
+
+			// do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT,
+			// even if current climb altitude is below (e.g. RTL immediately after take off)
+			if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+			} else {
+				_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
+			}
+
+			_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));
+			break;
+		}
+
+	case RTL_STATE_RETURN: {
+
+			// 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;
+
+
+			} else {
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			}
+
+			_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));
+
+			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;
+
+			// 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);
+
+			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);
+
+			} else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_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 = _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));
+			break;
+		}
+
+	case RTL_STATE_LOITER: {
+			const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON);
+
+			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());
+
+			} else {
+				_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+				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;
+
+			} else {
+				_mission_item.yaw = _destination.yaw;
+			}
+
+			_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();
+
+			_navigator->set_can_loiter_at_sp(true);
+
+			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;
+
+			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);
+
+			} 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.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.time_inside = 0.0f;
+			_mission_item.autocontinue = true;
+			_mission_item.origin = ORIGIN_ONBOARD;
+
+			// Disable previous setpoint to prevent drift.
+			pos_sp_triplet->previous.valid = false;
+			break;
+		}
+
+	case RTL_STATE_TRANSITION_TO_MC: {
+			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
+			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;
+
+			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);
+
+			} 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.acceptance_radius = _navigator->get_acceptance_radius();
+			_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;
+
+			if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_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.land_precision = _param_rtl_pld_md.get();
+
+			if (_mission_item.land_precision > 0u && _mission_item.land_precision <= 2u) {
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+				if (_mission_item.land_precision == 1) {
+					_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
+
+				} else if (_mission_item.land_precision == 2) {
+					_navigator->get_precland()->set_mode(PrecLandMode::Required);
+				}
+
+				_navigator->get_precland()->on_activation();
+			}
+
+			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: {
+			set_idle_item(&_mission_item);
+			break;
+		}
+
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	// 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);
+
+	if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+}
+
+void RtlDirect::advance_rtl()
+{
+	// 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;
+
+	// 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;
+
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB:
+		_rtl_state = RTL_STATE_RETURN;
+		break;
+
+	case RTL_STATE_RETURN:
+		if (vtol_in_fw_mode || descend_and_loiter) {
+			_rtl_state = RTL_STATE_DESCEND;
+
+		} else {
+			_rtl_state = RTL_STATE_LAND;
+		}
+
+		break;
+
+	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_TRANSITION_TO_MC;
+
+		} else {
+			_rtl_state = RTL_STATE_LAND;
+		}
+
+		_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;
+		break;
+
+	default:
+		break;
+	}
+}
+
+rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
+{
+	rtl_time_estimate_s rtl_time_estimate{};
+
+	RTLState start_state_for_estimate{RTL_STATE_NONE};
+
+	if (isActive()) {
+		start_state_for_estimate = _rtl_state;
+	}
+
+	// Calculate RTL time estimate only when there is a valid home position
+	// TODO: Also check if vehicle position is valid
+	if (!_navigator->home_global_position_valid()) {
+		rtl_time_estimate.valid = false;
+
+	} else {
+		rtl_time_estimate.valid = true;
+
+		// Sum up time estimate for various segments of the landing procedure
+		switch (start_state_for_estimate) {
+		case RTL_STATE_NONE:
+		case RTL_STATE_CLIMB: {
+				// 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;
+
+				if (climb_dist > 0) {
+					rtl_time_estimate.time_estimate += climb_dist / getClimbRate();
+				}
+			}
+
+		// FALLTHROUGH
+		case RTL_STATE_RETURN:
+
+			// 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();
+
+		// FALLTHROUGH
+		case RTL_STATE_HEAD_TO_CENTER:
+		case RTL_STATE_TRANSITION_TO_MC:
+		case RTL_STATE_DESCEND: {
+				// when descending, the target altitude is stored in the current mission item
+				float initial_altitude = 0;
+				float loiter_altitude = 0;
+
+				if (start_state_for_estimate == RTL_STATE_DESCEND) {
+					// 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)
+				rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate();
+			}
+
+		// FALLTHROUGH
+		case RTL_STATE_LOITER:
+			// 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: {
+				float initial_altitude;
+
+				// Add land segment (second landing phase) which comes after LOITER
+				if (start_state_for_estimate == RTL_STATE_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;
+
+
+				} 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;
+				}
+
+				// Prevent negative times when close to the ground
+				if (initial_altitude > _destination.alt) {
+					rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed();
+				}
+
+			}
+
+			break;
+
+		case RTL_STATE_LANDED:
+			// Remaining time is 0
+			break;
+		}
+
+		// Prevent negative durations as phyiscally they make no sense. These can
+		// occur during the last phase of landing when close to the ground.
+		rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate);
+
+		// Use actual time estimate to compute the safer time estimate with additional scale factor and a margin
+		rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate
+						       + _param_rtl_time_margin.get();
+	}
+
+	// return message
+	rtl_time_estimate.timestamp = hrt_absolute_time();
+
+	return rtl_time_estimate;
+}
+
+float RtlDirect::getCruiseSpeed()
+{
+	float ret = 1e6f;
+
+	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+
+	} else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+
+	} else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
+		if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+	}
+
+	return ret;
+}
+
+float RtlDirect::getHoverLandSpeed()
+{
+	float ret = 1e6f;
+
+	if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) {
+		ret = 1e6f;
+	}
+
+	return ret;
+}
+
+matrix::Vector2f RtlDirect::get_wind()
+{
+	_wind_sub.update();
+	matrix::Vector2f wind;
+
+	if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) {
+		wind(0) = _wind_sub.get().windspeed_north;
+		wind(1) = _wind_sub.get().windspeed_east;
+	}
+
+	return wind;
+}
+
+float RtlDirect::getClimbRate()
+{
+	float ret = 1e6f;
+
+	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+
+	} else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+
+		if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+	}
+
+	return ret;
+}
+
+float RtlDirect::getDescendRate()
+{
+	float ret = 1e6f;
+
+	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+
+	} else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) {
+			ret = 1e6f;
+		}
+	}
+
+	return ret;
+}
+
+float RtlDirect::getCruiseGroundSpeed()
+{
+	float cruise_speed = getCruiseSpeed();
+
+	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		matrix::Vector2f wind = get_wind();
+
+		matrix::Vector2f to_destination_vec;
+		get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, _destination.lon,
+					    &to_destination_vec(0), &to_destination_vec(1));
+
+		const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero();
+
+		const float wind_towards_home = wind.dot(to_home_dir);
+		const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm();
+
+
+		// Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient
+		const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf(
+						   0.f, wind_towards_home);
+
+		cruise_speed = ground_speed;
+	}
+
+	return cruise_speed;
+}

src/modules/navigator/rtl_direct.h

@@ -0,0 +1,213 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2023 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 rtl_direct.h
+ *
+ * Helper class for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+
+#include "mission_block.h"
+#include "lib/mission/planned_mission_interface.h"
+#include "navigation.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/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>
+
+class Navigator;
+
+class RtlDirect : public MissionBlock, public ModuleParams
+{
+public:
+	RtlDirect(Navigator *navigator);
+
+	~RtlDirect() = default;
+
+	/**
+	 * @brief On activation.
+	 * Initialize the return to launch calculations.
+	 *
+	 * @param[in] enforce_rtl_alt boolean if the minimal return to launch altitude should be enforced at the beginning of the return, even when the current vehicle altitude is above.
+	 */
+	void on_activation(bool enforce_rtl_alt);
+
+	/**
+	 * @brief on active
+	 * Update the return to launch calculation and set new setpoints for controller if necessary.
+	 *
+	 */
+	void on_active() override;
+
+	/**
+	 * @brief Calculate the estimated time needed to return to launch.
+	 *
+	 * @return estimated time to return to launch.
+	 */
+	rtl_time_estimate_s calc_rtl_time_estimate();
+
+	void setRtlAlt(float alt) {_rtl_alt = alt;};
+
+	void setRtlPosition(LandingPosition_s position) {_destination = position;};
+
+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,
+	};
+
+private:
+	/**
+	 * @brief Get the horizontal wind velocity
+	 *
+	 * @return horizontal wind velocity.
+	 */
+	matrix::Vector2f get_wind();
+
+	/**
+	 * @brief Set the return to launch control setpoint.
+	 *
+	 */
+	void set_rtl_item();
+
+	/**
+	 * @brief Advance the return to launch state machine.
+	 *
+	 */
+	void advance_rtl();
+
+	/**
+	 * @brief Get the Cruise Ground Speed
+	 *
+	 * @return Ground speed in cruise mode [m/s].
+	 */
+	float getCruiseGroundSpeed();
+
+	/**
+	 * @brief Get the climb rate
+	 *
+	 * @return Climb rate [m/s]
+	 */
+	float getClimbRate();
+
+	/**
+	 * @brief Get the descend rate
+	 *
+	 * @return descend rate [m/s]
+	 */
+	float getDescendRate();
+
+	/**
+	 * @brief Get the cruise speed
+	 *
+	 * @return cruise speed [m/s]
+	 */
+	float getCruiseSpeed();
+
+	/**
+	 * @brief Get the Hover Land Speed
+	 *
+	 * @return Hover land speed [m/s]
+	 */
+	float getHoverLandSpeed();
+
+	/** Current state in the state machine.*/
+	RTLState _rtl_state{RTL_STATE_NONE};
+
+	LandingPosition_s _destination{}; ///< the RTL position to fly to
+
+	float _rtl_alt{0.0f};	///< AMSL altitude at which the vehicle should return to the home position
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::RTL_TYPE>)          _param_rtl_type,
+		(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,
+		(ParamInt<px4::params::RTL_PLD_MD>)        _param_rtl_pld_md,
+		(ParamFloat<px4::params::RTL_LOITER_RAD>)  _param_rtl_loiter_rad,
+		(ParamInt<px4::params::RTL_HDG_MD>)        _param_rtl_hdg_md,
+		(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,
+		(ParamInt<px4::params::RTL_TIME_MARGIN>)   _param_rtl_time_margin,
+		(ParamFloat<px4::params::NAV_ACC_RAD>)      _param_nav_acc_rad		/**< acceptance for takeoff */
+	)
+
+	param_t		_param_mpc_z_v_auto_up{PARAM_INVALID};
+	param_t		_param_mpc_z_v_auto_dn{PARAM_INVALID};
+	param_t		_param_mpc_land_speed{PARAM_INVALID};
+	param_t		_param_fw_climb_rate{PARAM_INVALID};
+	param_t		_param_fw_sink_rate{PARAM_INVALID};
+	param_t 	_param_fw_airspeed_trim{PARAM_INVALID};
+	param_t 	_param_mpc_xy_cruise{PARAM_INVALID};
+	param_t 	_param_rover_cruise_speed{PARAM_INVALID};
+
+	uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)};	/**< global 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_mission_fast.cpp

@@ -0,0 +1,246 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2023 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 rtl_mission_fast.cpp
+ *
+ * Helper class for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "rtl_mission_fast.h"
+#include "navigator.h"
+
+#include <drivers/drv_hrt.h>
+
+RtlMissionFast::RtlMissionFast(Navigator *navigator) :
+	MissionBase(navigator)
+{
+
+}
+
+void RtlMissionFast::on_activation()
+{
+	goToItem(_init_mission_index, false);
+
+	if (_land_detected_sub.get().landed) {
+		// already landed, no need to do anything, invalidad the position mission item.
+		_is_current_planned_mission_item_valid = false;
+	}
+
+	MissionBase::on_activation();
+}
+
+void RtlMissionFast::on_active()
+{
+	_home_pos_sub.update();
+	MissionBase::on_active();
+}
+
+void RtlMissionFast::on_inactive()
+{
+	_home_pos_sub.update();
+	MissionBase::on_inactive();
+}
+
+void RtlMissionFast::setInitialMissionIndex(int32_t init_mission_index)
+{
+	// Map the input to feasible indexes.
+	if (init_mission_index < 0) {
+		_init_mission_index = 0;
+
+	} else if (init_mission_index >= _mission.count) {
+		_init_mission_index = _mission.count - 1;
+
+	} else {
+		_init_mission_index = init_mission_index;
+	}
+}
+
+bool RtlMissionFast::setNextMissionItem()
+{
+	return (goToNextPositionItem(true) == EXIT_SUCCESS);
+}
+
+void RtlMissionFast::setActiveMissionItems()
+{
+	WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	// Transition to fixed wing if necessary.
+	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
+	    _vehicle_status_sub.get().is_vtol &&
+	    !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
+		set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
+		_mission_item.yaw = _navigator->get_local_position()->heading;
+
+		// keep current setpoints (FW position controller generates wp to track during transition)
+		pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+
+		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITON;
+
+	} else if (item_contains_position(_mission_item)) {
+		if (_mission_item.nav_cmd == NAV_CMD_LAND ||
+		    _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
+			handleLanding(new_work_item_type);
+
+		} else {
+			// convert mission item to a simple waypoint, keep loiter to alt
+			if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) {
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			}
+
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0.0f;
+
+			pos_sp_triplet->previous = pos_sp_triplet->current;
+		}
+
+		mission_item_s next_mission_item;
+		size_t num_found_items = 0;
+		getNextPositionItems(_mission.current_seq + 1, &next_mission_item, num_found_items, 1u);
+
+		if (num_found_items > 0) {
+
+			mission_apply_limitation(next_mission_item);
+			mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next);
+		}
+
+		mission_apply_limitation(_mission_item);
+		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+	}
+
+	issue_command(_mission_item);
+
+	/* set current work item type */
+	_work_item_type = new_work_item_type;
+
+	_navigator->set_can_loiter_at_sp(false);
+	reset_mission_item_reached();
+
+	if (_mission_type == MissionType::MISSION_TYPE_MISSION) {
+		set_mission_result();
+	}
+
+	publish_navigator_mission_item(); // for logging
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void RtlMissionFast::handleLanding(WorkItemType &new_work_item_type)
+{
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	bool needs_to_land = !_land_detected_sub.get().landed &&
+			     ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND)
+			      || (_mission_item.nav_cmd == NAV_CMD_LAND));
+	bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol &&
+				  (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
+				  needs_to_land;
+
+	if (needs_vtol_landing) {
+		if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
+			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
+
+			float altitude = _global_pos_sub.get().alt;
+
+			if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+				altitude = pos_sp_triplet->current.alt;
+			}
+
+			_mission_item.altitude = altitude;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0.0f;
+			_mission_item.vtol_back_transition = true;
+
+			pos_sp_triplet->previous.valid = false;
+
+		}
+
+		/* transition to MC */
+		if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
+
+			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
+			_mission_item.altitude = _global_pos_sub.get().alt;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.yaw = NAN;
+
+			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
+
+			// make previous setpoint invalid, such that there will be no prev-current line following
+			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
+			pos_sp_triplet->previous.valid = false;
+		}
+
+	} else if (needs_to_land) {
+		/* move to landing waypoint before descent if necessary */
+		if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
+		    do_need_move_to_land() &&
+		    (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT ||
+		     _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
+
+			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
+
+			_mission_item.altitude = _global_pos_sub.get().alt;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0.0f;
+
+			// make previous setpoint invalid, such that there will be no prev-current line following.
+			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
+			pos_sp_triplet->previous.valid = false;
+
+		}
+	}
+}
+
+bool RtlMissionFast::do_need_move_to_land()
+{
+	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+			  _global_pos_sub.get().lat, _global_pos_sub.get().lon);
+
+	return d_current > _navigator->get_acceptance_radius();
+
+}
+
+rtl_time_estimate_s RtlMissionFast::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/rtl_mission_fast.h

@@ -0,0 +1,75 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2023 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 rtl_mission_fast.h
+ *
+ * Helper class for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include "mission.h"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/rtl_time_estimate.h>
+
+class Navigator;
+
+class RtlMissionFast : public MissionBase
+{
+public:
+	RtlMissionFast(Navigator *navigator);
+	~RtlMissionFast() = default;
+
+	void on_activation() override;
+	void on_active() override;
+	void on_inactive() override;
+
+	rtl_time_estimate_s calc_rtl_time_estimate();
+
+	void setInitialMissionIndex(int32_t init_mission_index);
+
+private:
+	bool setNextMissionItem() override;
+	void setActiveMissionItems() override;
+	void handleLanding(WorkItemType &new_work_item_type);
+	bool do_need_move_to_land();
+
+	int32_t _init_mission_index{0};
+
+	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
+};

src/modules/navigator/rtl_mission_fast_reverse.cpp

@@ -0,0 +1,263 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2023 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 rtl_mission_fast_reverse.cpp
+ *
+ * Helper class for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "rtl_mission_fast_reverse.h"
+#include "navigator.h"
+
+#include <drivers/drv_hrt.h>
+
+
+RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) :
+	MissionBase(navigator)
+{
+
+}
+
+void RtlMissionFastReverse::on_activation()
+{
+	setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _global_pos_sub.get().alt,
+				_home_pos_sub.get().alt, _vehicle_status_sub.get());
+
+	if (_land_detected_sub.get().landed) {
+		// already landed, no need to do anything, invalidate the position mission item.
+		_is_current_planned_mission_item_valid = false;
+	}
+
+	MissionBase::on_activation();
+}
+
+void RtlMissionFastReverse::on_active()
+{
+	_home_pos_sub.update();
+	MissionBase::on_active();
+}
+
+void RtlMissionFastReverse::on_inactive()
+{
+	_home_pos_sub.update();
+	MissionBase::on_inactive();
+}
+
+bool RtlMissionFastReverse::setNextMissionItem()
+{
+	return (goToPreviousPositionItem(true) == EXIT_SUCCESS);
+}
+
+void RtlMissionFastReverse::setActiveMissionItems()
+{
+	WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	// Transition to fixed wing if necessary.
+	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
+	    _vehicle_status_sub.get().is_vtol &&
+	    !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
+		set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
+		_mission_item.yaw = _navigator->get_local_position()->heading;
+
+		// keep current setpoints (FW position controller generates wp to track during transition)
+		pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+
+		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITON;
+
+	} else if (item_contains_position(_mission_item)) {
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+		    _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
+			handleLanding(new_work_item_type);
+
+		} else {
+			// convert mission item to a simple waypoint, keep loiter to alt
+			if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) {
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			}
+
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0.0f;
+
+			pos_sp_triplet->previous = pos_sp_triplet->current;
+		}
+
+		mission_item_s next_mission_item;
+		size_t num_found_items = 0;
+		getPreviousPositionItems(_mission.current_seq - 1, &next_mission_item, num_found_items, 1u);
+
+		if (num_found_items > 0) {
+
+			mission_apply_limitation(next_mission_item);
+			mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next);
+		}
+
+		mission_apply_limitation(_mission_item);
+		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+	}
+
+	issue_command(_mission_item);
+
+	/* set current work item type */
+	_work_item_type = new_work_item_type;
+
+	_navigator->set_can_loiter_at_sp(false);
+	reset_mission_item_reached();
+
+	if (_mission_type == MissionType::MISSION_TYPE_MISSION) {
+		set_mission_result();
+	}
+
+	publish_navigator_mission_item(); // for logging
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type)
+{
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	bool needs_to_land = !_land_detected_sub.get().landed &&
+			     ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) || (_mission_item.nav_cmd == NAV_CMD_TAKEOFF));
+	bool vtol_in_fw = _vehicle_status_sub.get().is_vtol &&
+			  (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+
+	if (needs_to_land) {
+		if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
+			// Go to Take off location
+			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TAKEOFF;
+
+			float altitude = _global_pos_sub.get().alt;
+
+			if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+				altitude = pos_sp_triplet->current.alt;
+			}
+
+			_mission_item.altitude = altitude;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0.0f;
+			_mission_item.vtol_back_transition = true;
+
+			pos_sp_triplet->previous = pos_sp_triplet->current;
+		}
+
+		if (vtol_in_fw) {
+			if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_TAKEOFF) {
+				// Go to home location
+				new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
+
+				float altitude = _global_pos_sub.get().alt;
+
+				if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+					altitude = pos_sp_triplet->current.alt;
+				}
+
+				_mission_item.lat = _home_pos_sub.get().lat;
+				_mission_item.lon = _home_pos_sub.get().lon;
+				_mission_item.altitude = altitude;
+				_mission_item.altitude_is_relative = false;
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+				_mission_item.autocontinue = true;
+				_mission_item.time_inside = 0.0f;
+				_mission_item.vtol_back_transition = true;
+
+				pos_sp_triplet->previous = pos_sp_triplet->current;
+			}
+
+			/* transition to MC */
+			if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
+
+				set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
+				_mission_item.altitude = _global_pos_sub.get().alt;
+				_mission_item.altitude_is_relative = false;
+				_mission_item.yaw = NAN;
+
+				new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
+
+				// make previous setpoint invalid, such that there will be no prev-current line following
+				// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
+				pos_sp_triplet->previous.valid = false;
+			}
+
+		} else if ((_work_item_type == WorkItemType::WORK_ITEM_TYPE_TAKEOFF ||
+			    _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND ||
+			    _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
+			_mission_item.nav_cmd = NAV_CMD_LAND;
+			_mission_item.lat = _home_pos_sub.get().lat;
+			_mission_item.lon = _home_pos_sub.get().lon;
+			_mission_item.yaw = NAN;
+
+			if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
+			    do_need_move_to_land()) {
+				new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
+
+				_mission_item.altitude = _global_pos_sub.get().alt;
+				_mission_item.altitude_is_relative = false;
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+				_mission_item.autocontinue = true;
+				_mission_item.time_inside = 0.0f;
+
+				// make previous setpoint invalid, such that there will be no prev-current line following.
+				// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
+				pos_sp_triplet->previous.valid = false;
+
+			} else {
+				_mission_item.altitude = _home_pos_sub.get().alt;
+				_mission_item.altitude_is_relative = false;
+				pos_sp_triplet->previous.valid = false;
+			}
+		}
+	}
+}
+
+bool RtlMissionFastReverse::do_need_move_to_land()
+{
+	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+			  _global_pos_sub.get().lat, _global_pos_sub.get().lon);
+
+	return d_current > _navigator->get_acceptance_radius();
+
+}
+
+rtl_time_estimate_s RtlMissionFastReverse::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/rtl_mission_fast_reverse.h

@@ -0,0 +1,71 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2023 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 rtl_mission_fast_reverse.h
+ *
+ * Helper class for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include "mission.h"
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/rtl_time_estimate.h>
+
+class Navigator;
+
+class RtlMissionFastReverse : public MissionBase
+{
+public:
+	RtlMissionFastReverse(Navigator *navigator);
+	~RtlMissionFastReverse() = default;
+
+	void on_activation() override;
+	void on_active() override;
+	void on_inactive() override;
+
+	rtl_time_estimate_s calc_rtl_time_estimate();
+
+private:
+	bool setNextMissionItem() override;
+	void setActiveMissionItems() override;
+	void handleLanding(WorkItemType &new_work_item_type);
+	bool do_need_move_to_land();
+
+	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
+};

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,321 @@
+/****************************************************************************
+ *
+ *   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();
+	_wind_sub.update();
+
+	readVtolLandApproachesFromStorage();
+	set_loiter_position();
+	_land_state = vtol_land_state::MOVE_TO_LOITER;
+
+}
+
+void VtolLand::on_inactive()
+{
+	_global_pos_sub.update();
+	_wind_sub.update();
+}
+
+void
+VtolLand::on_active()
+{
+	_global_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 = _destination.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::hasVtolLandApproach()
+{
+	readVtolLandApproachesFromStorage();
+	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) = _destination.lat;
+		_loiter_pos_lat_lon(1) = _destination.lon;
+	}
+
+
+	_mission_item.lat  = _loiter_pos_lat_lon(0);
+	_mission_item.lon = _loiter_pos_lat_lon(1);
+	_mission_item.altitude = math::max(_destination.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(_destination.lat,
+							 _destination.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::readVtolLandApproachesFromStorage()
+{
+
+	// 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_LAND_FOR_APPROACHES of our current pestination 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 destination
+	_vtol_home_land_approaches.resetAllApproaches();
+
+	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 foundLandApproaches = 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 (foundLandApproaches) {
+				break;
+			}
+
+			const float dist_to_land = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, _destination.lat,
+						   _destination.lon);
+
+			if (!mission_item.is_mission_rally_point && dist_to_land < MAX_DIST_FROM_LAND_FOR_APPROACHES) {
+				foundLandApproaches = true;
+				_vtol_home_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.lat, mission_item.lon);
+			}
+
+			sector_counter = 0;
+		}
+
+		if (foundLandApproaches && 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,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 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 "navigation.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 hasVtolLandApproach();
+
+	void setLandPosition(LandingPosition_s position){_destination = position;};
+
+	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
+	)
+
+	LandingPosition_s _destination;
+
+	void set_loiter_position();
+
+	loiter_point_s chooseBestLandingApproach();
+
+	land_approaches_s _vtol_home_land_approaches{};
+
+	void readVtolLandApproachesFromStorage();
+
+	static constexpr float MAX_DIST_FROM_LAND_FOR_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<wind_s>		_wind_sub{ORB_ID(wind)};
+
+};

src/systemcmds/tests/test_dataman.c

@@ -80,7 +80,7 @@ task_main(int argc, char *argv[])
 	}
 
 	/* try to read an invalid index */
-	if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
+	if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINT_ITEMS_MAX, buffer, sizeof(buffer)) >= 0) {
 		PX4_ERR("%d read an invalid index failed", my_id);
 		goto fail;
 	}

test/mavsdk_tests/CMakeLists.txt

@@ -17,6 +17,7 @@ if(MAVSDK_FOUND)
         test_main.cpp
         autopilot_tester.cpp
         autopilot_tester_failure.cpp
+        autopilot_tester_rtl.cpp
         # follow-me needs a MAVSDK update:
         # https://github.com/mavlink/MAVSDK/pull/1770
         # autopilot_tester_follow_me.cpp

test/mavsdk_tests/autopilot_tester.cpp

@@ -268,7 +268,7 @@ void AutopilotTester::execute_mission_and_lose_gps()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorGps, Failure::FailureType::Off, 0) == Failure::Result::Success);
 
@@ -280,7 +280,7 @@ void AutopilotTester::execute_mission_and_lose_mag()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Off, 0) == Failure::Result::Success);
 
@@ -296,7 +296,7 @@ void AutopilotTester::execute_mission_and_lose_baro()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Off, 0) == Failure::Result::Success);
 
@@ -312,7 +312,7 @@ void AutopilotTester::execute_mission_and_get_baro_stuck()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck, 0) == Failure::Result::Success);
 
@@ -328,7 +328,7 @@ void AutopilotTester::execute_mission_and_get_mag_stuck()
 {
 	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
 
-	start_and_wait_for_first_mission_item();
+	start_and_wait_for_mission_sequence(1);
 
 	CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck, 0) == Failure::Result::Success);
 
@@ -501,6 +501,49 @@ void AutopilotTester::stop_checking_altitude()
 	_telemetry->subscribe_position(nullptr);
 }
 
+void AutopilotTester::check_tracks_mission_raw(float corridor_radius_m, bool reverse)
+{
+	auto mission_raw = _mission_raw->download_mission();
+	CHECK(mission_raw.first == MissionRaw::Result::Success);
+
+	auto mission_items = mission_raw.second;
+	auto ct = get_coordinate_transformation();
+
+	_telemetry->set_rate_position_velocity_ned(5);
+	_telemetry->subscribe_position_velocity_ned([ct, mission_items, corridor_radius_m, reverse,
+	    this](Telemetry::PositionVelocityNed position_velocity_ned) {
+		auto progress = _mission_raw->mission_progress();
+
+
+		std::function<std::array<float, 3>(std::vector<mavsdk::MissionRaw::MissionItem>, unsigned, mavsdk::geometry::CoordinateTransformation)>
+		get_waypoint_for_sequence = [](std::vector<mavsdk::MissionRaw::MissionItem> mission_items, int sequence, auto ct) {
+			for (auto waypoint : mission_items) {
+
+				if (waypoint.seq == (uint32_t)sequence) {
+					return get_local_mission_item_from_raw_item<float>(waypoint, ct);
+				}
+			}
+
+			return  std::array<float, 3>({0.0f, 0.0f, 0.0f});
+		};
+
+		if (progress.current > 0 && progress.current < progress.total) {
+			// Get shortest distance of current position to 3D line between previous and next waypoint
+
+			std::array<float, 3> current { position_velocity_ned.position.north_m,
+						       position_velocity_ned.position.east_m,
+						       position_velocity_ned.position.down_m };
+			std::array<float, 3> wp_prev = get_waypoint_for_sequence(mission_items,
+						       reverse ? progress.current + 1 : progress.current - 1, ct);
+			std::array<float, 3> wp_next = get_waypoint_for_sequence(mission_items, progress.current, ct);
+
+			float distance_to_trajectory = point_to_line_distance(current, wp_prev, wp_next);
+
+			CHECK(distance_to_trajectory < corridor_radius_m);
+		}
+	});
+}
+
 void AutopilotTester::check_tracks_mission(float corridor_radius_m)
 {
 	auto mission = _mission->download_mission();
@@ -535,6 +578,12 @@ void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float
 	CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m);
 }
 
+void AutopilotTester::execute_rtl_when_reaching_mission_sequence(int sequence_number)
+{
+	start_and_wait_for_mission_sequence_raw(sequence_number);
+	execute_rtl();
+}
+
 std::array<float, 3> AutopilotTester::get_current_position_ned()
 {
 	mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
@@ -642,15 +691,15 @@ bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry:
 	return pass;
 }
 
-void AutopilotTester::start_and_wait_for_first_mission_item()
+void AutopilotTester::start_and_wait_for_mission_sequence(int sequence_number)
 {
 	auto prom = std::promise<void> {};
 	auto fut = prom.get_future();
 
-	_mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) {
+	_mission->subscribe_mission_progress([&prom, this, sequence_number](Mission::MissionProgress progress) {
 		std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
 
-		if (progress.current >= 1) {
+		if (progress.current >= sequence_number) {
 			_mission->subscribe_mission_progress(nullptr);
 			prom.set_value();
 		}
@@ -661,6 +710,25 @@ void AutopilotTester::start_and_wait_for_first_mission_item()
 	REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready);
 }
 
+void AutopilotTester::start_and_wait_for_mission_sequence_raw(int sequence_number)
+{
+	auto prom = std::promise<void> {};
+	auto fut = prom.get_future();
+
+	_mission_raw->subscribe_mission_progress([&prom, this, sequence_number](MissionRaw::MissionProgress progress) {
+		std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
+
+		if (progress.current >= sequence_number) {
+			_mission_raw->subscribe_mission_progress(nullptr);
+			prom.set_value();
+		}
+	});
+
+	REQUIRE(_mission_raw->start_mission() == MissionRaw::Result::Success);
+
+	REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready);
+}
+
 void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout)
 {
 	auto prom = std::promise<void> {};

test/mavsdk_tests/autopilot_tester.h

@@ -137,9 +137,11 @@ public:
 	void request_ground_truth();
 	void check_mission_item_speed_above(int item_index, float min_speed_m_s);
 	void check_tracks_mission(float corridor_radius_m = 1.5f);
+	void check_tracks_mission_raw(float corridor_radius_m = 1.f, bool reverse = false);
 	void start_checking_altitude(const float max_deviation_m);
 	void stop_checking_altitude();
 	void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f);
+	void execute_rtl_when_reaching_mission_sequence(int sequence_number);
 
 	// Blocking call to get the drone's current position in NED frame
 	std::array<float, 3> get_current_position_ned();
@@ -199,7 +201,8 @@ private:
 	bool ground_truth_horizontal_position_far_from(const Telemetry::GroundTruth &target_pos, float min_distance_m);
 	bool estimated_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m);
 	bool estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m);
-	void start_and_wait_for_first_mission_item();
+	void start_and_wait_for_mission_sequence(int sequence_number);
+	void start_and_wait_for_mission_sequence_raw(int sequence_number);
 	void wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout);
 	void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout);
 	void wait_for_mission_finished(std::chrono::seconds timeout);

test/mavsdk_tests/autopilot_tester_rtl.cpp

@@ -0,0 +1,57 @@
+
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#include "autopilot_tester_rtl.h"
+
+#include "math_helpers.h"
+#include <iostream>
+#include <future>
+#include <thread>
+#include <unistd.h>
+
+
+void AutopilotTesterRtl::connect(const std::string uri)
+{
+	AutopilotTester::connect(uri);
+}
+
+void AutopilotTesterRtl::set_rtl_type(int rtl_type)
+{
+	CHECK(getParams()->set_param_int("RTL_TYPE", rtl_type) == Param::Result::Success);
+}
+
+void AutopilotTesterRtl::set_takeoff_land_requirements(int req)
+{
+	CHECK(getParams()->set_param_int("MIS_TKO_LAND_REQ", req) == Param::Result::Success);
+}

test/mavsdk_tests/autopilot_tester_rtl.h

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "autopilot_tester.h"
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+
+
+class AutopilotTesterRtl : public AutopilotTester
+{
+public:
+	AutopilotTesterRtl() = default;
+	~AutopilotTesterRtl() = default;
+
+	void set_rtl_type(int rtl_type);
+	void set_takeoff_land_requirements(int req);
+	void connect(const std::string uri);
+
+
+private:
+	std::unique_ptr<mavsdk::Failure> _failure{};
+};

test/mavsdk_tests/math_helpers.h

@@ -44,6 +44,20 @@ std::array<T, 3> get_local_mission_item(const Mission::MissionItem &item, const
 	return {static_cast<T>(local.north_m), static_cast<T>(local.east_m), -item.relative_altitude_m};
 }
 
+template<typename T>
+std::array<T, 3> get_local_mission_item_from_raw_item(const mavsdk::MissionRaw::MissionItem &item,
+		const CoordinateTransformation &ct)
+{
+	using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate;
+	GlobalCoordinate global;
+	global.latitude_deg = item.x / 1e7;
+	global.longitude_deg = item.y / 1e7;
+
+
+	auto local = ct.local_from_global(global);
+	return {static_cast<T>(local.north_m), static_cast<T>(local.east_m), -item.z};
+}
+
 template<typename T>
 T sq(T x)
 {

test/mavsdk_tests/test_vtol_mission.cpp

@@ -31,7 +31,7 @@
  *
  ****************************************************************************/
 
-#include "autopilot_tester.h"
+#include "autopilot_tester_rtl.h"
 
 
 TEST_CASE("Fly VTOL mission", "[vtol]")
@@ -44,3 +44,48 @@ TEST_CASE("Fly VTOL mission", "[vtol]")
 	tester.execute_mission_raw();
 	tester.wait_until_disarmed();
 }
+TEST_CASE("RTL direct Home", "[vtol]")
+{
+	AutopilotTesterRtl tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan");
+	// fly directly to home position
+	tester.set_rtl_type(0);
+	tester.arm();
+	tester.execute_rtl_when_reaching_mission_sequence(2);
+	tester.wait_until_disarmed(std::chrono::seconds(90));
+	tester.check_home_within(5.0f);
+}
+
+TEST_CASE("RTL with Mission Landing", "[vtol]")
+{
+	AutopilotTesterRtl tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan");
+	// Vehicle should follow the mission and use the mission landing
+	tester.set_rtl_type(2);
+	tester.arm();
+	tester.execute_rtl_when_reaching_mission_sequence(2);
+	tester.check_tracks_mission_raw(30.0f);
+	tester.wait_until_disarmed(std::chrono::seconds(90));
+}
+
+TEST_CASE("RTL with Reverse Mission", "[vtol]")
+{
+	AutopilotTesterRtl tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.set_takeoff_land_requirements(0);
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_without_landing.plan");
+	// vehicle should follow the mission in reverse and land at the home position
+	tester.set_rtl_type(2);
+	tester.arm();
+	tester.execute_rtl_when_reaching_mission_sequence(6);
+	/* this check is currently disabled since there seems to be a bug in mavsdk which causes the reported
+	mission sequence to be out of order */
+	tester.check_tracks_mission_raw(300.0f);
+	tester.wait_until_disarmed(std::chrono::seconds(90));
+}

test/mavsdk_tests/vtol_mission_without_landing.plan

@@ -0,0 +1,184 @@
+{
+    "fileType": "Plan",
+    "geoFence": {
+        "circles": [
+        ],
+        "polygons": [
+        ],
+        "version": 2
+    },
+    "groundStation": "QGroundControl",
+    "mission": {
+        "cruiseSpeed": 15,
+        "firmwareType": 12,
+        "globalPlanAltitudeMode": 1,
+        "hoverSpeed": 5,
+        "items": [
+            {
+                "AMSLAltAboveTerrain": 20,
+                "Altitude": 20,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 84,
+                "doJumpId": 1,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.39833113265167,
+                    8.545508725338607,
+                    20
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 20,
+                "Altitude": 20,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 2,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.399332700068925,
+                    8.54481499384454,
+                    20
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 3,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.39908888031702,
+                    8.54344001880591,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 4,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.39760160279815,
+                    8.542394178137585,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 5,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.396941861414504,
+                    8.54282818797708,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 6,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.396686401111786,
+                    8.544419333554089,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 7,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.397202447861446,
+                    8.545440338322464,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 20,
+                "Altitude": 20,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 8,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.39766309343905,
+                    8.545713820298545,
+                    20
+                ],
+                "type": "SimpleItem"
+            }
+        ],
+        "plannedHomePosition": [
+            47.39775218584113,
+            8.545620889782981,
+            489.0021493051957
+        ],
+        "vehicleType": 20,
+        "version": 2
+    },
+    "rallyPoints": {
+        "points": [
+        ],
+        "version": 2
+    },
+    "version": 1
+}