[RTL]: Define separate RTL module performing all the different RTL in their respective classes.

- RTL Direct, for directly flying to the destination and landing there
- RTL Mission Fast for Landing at the mission landing spot by going along the mission path.
- RTL Mission Fast Reverse for landing at home but going along the mision path.

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/mission/planned_mission_interface.cpp

@@ -277,7 +277,7 @@ int PlannedMissionInterface::setMissionIndex(int32_t index)
 	}
 }
 
-int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, float alt, float home_alt,
+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);
@@ -311,6 +311,14 @@ int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, flo
 		}
 	}
 
+	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);
 }
 

src/lib/mission/planned_mission_interface.h

@@ -64,6 +64,8 @@ public:
 	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;
 
@@ -101,11 +103,9 @@ protected:
 			       _land_pos{.lat = 0.0,
 					 .lon = 0.0,
 					 .alt = 0.0f};
-
-private:
-	bool _is_land_start_item_searched{false};
-
 	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/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/navigator/CMakeLists.txt

@@ -43,6 +43,9 @@ 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

src/modules/navigator/mission.h

@@ -71,30 +71,25 @@
 
 class Navigator;
 
-class Mission : public MissionBlock, public ModuleParams, protected PlannedMissionInterface
+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);
-
-	uint16_t get_land_start_index() const { return _land_start_index; }
-	bool get_land_start_available() const { return _land_start_index != _invalid_index; }
-
-private:
+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_AFTER_TAKEOFF,
+		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) */
@@ -104,12 +99,10 @@ private:
 		MISSION_TYPE_MISSION
 	} _mission_type{MissionType::MISSION_TYPE_NONE};
 
-	void onMissionUpdate(bool has_mission_items_changed) override;
-
 	/**
 	 * Update mission topic
 	 */
-	void update_mission();
+	virtual void update_mission();
 
 	/**
 	 * Move on to next mission item or switch to loiter
@@ -125,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.
@@ -175,63 +185,59 @@ private:
 	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 mission result
-	 */
-	void set_mission_result();
-
-	/**
-	 * Check whether a mission is ready to go
-	 */
-	void check_mission_valid();
-
-	/**
-	 * Reset mission
-	 */
-	void checkMissionRestart();
-
-	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
-
-	void publish_navigator_mission_item();
-
 	void setCameraTrigger(bool enable);
 
-	void setEndOfMissionSetpoint();
-
-	void setMissionSetpoint(mission_item_s next_mission_items[], size_t &num_found_items);
-
-	WorkItemType handleTakeoff(mission_item_s next_mission_items[], size_t &num_found_items);
-
-	WorkItemType handleLanding(mission_item_s next_mission_items[], size_t &num_found_items);
-
-	WorkItemType handleVtolTransition(mission_item_s next_mission_items[], size_t &num_found_items);
-
-
 	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::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 */
-
-	bool _is_current_planned_mission_item_valid{false};
-
-	bool _initialized_mission_checked{false};
-	bool _is_mission_valid{false};
-
-	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};
-
-	bool _mission_has_been_activated{false};
-	bool _system_disarmed_while_inactive{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

@@ -938,6 +938,37 @@ float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission
 	}
 }
 
+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

@@ -127,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/navigator.h

@@ -277,6 +277,7 @@ public:
 	int  get_mission_landing_index() { return _mission.get_land_start_index(); }
 
 	// RTL
+
 	bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
 
 	bool abort_landing();

src/modules/navigator/navigator_main.cpp

@@ -549,7 +549,8 @@ void Navigator::run()
 				 * use MAV_CMD_MISSION_START to start the mission there
 				 */
 				uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED};
-                                if (_mission.get_land_start_available()) {
+
+				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();

src/modules/navigator/rtl.h

@@ -41,58 +41,32 @@
 
 #pragma once
 
+#include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
 
 #include "navigator_mode.h"
-#include "mission_block.h"
+#include "lib/mission/planned_mission_interface.h"
+#include "rtl_direct.h"
+#include "rtl_mission_fast.h"
+#include "rtl_mission_fast_reverse.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_MISSION_FAST,
+		RTL_MISSION_FAST_REVERSE,
 	};
 
 	void on_inactivation() override;
@@ -100,97 +74,64 @@ 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 onMissionUpdate(bool has_mission_items_changed) override {};
 
-	void set_rtl_item();
+	void setRtlTypeAndDestination();
 
-	void advance_rtl();
+	/**
+	 * @brief Find RTL destination.
+	 *
+	 */
+	void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt);
 
-	float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
-	void calc_and_pub_rtl_time_estimate(const RTLState rtl_state);
+	/**
+	 * @brief Set the position of the land start marker in the planned mission as destination.
+	 *
+	 */
+	void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position);
 
-	float getCruiseGroundSpeed();
+	/**
+	 * @brief Set the safepoint as destination.
+	 *
+	 * @param mission_safe_point is the mission safe point/rally point to set as destination.
+	 */
+	void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_safe_point_s &mission_safe_point);
 
-	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;
-		}
-	};
-
-	RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point)
+	/**
+	 * @brief
+	 *
+	 * @param cone_half_angle_deg
+	 * @return float
+	 */
+	float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &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;
+
+	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,224 @@
+/***************************************************************************
+ *
+ *   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 <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:
+	/**
+	 * @brief Return to launch position.
+	 * Defines the position and landing yaw for the return to launch destination.
+	 *
+	 */
+	struct RtlPosition {
+		double lat;	/**< latitude in WGS84 [rad].*/
+		double lon;	/**< longitude in WGS84 [rad].*/
+		float alt;	/**< altitude in MSL [m].*/
+		float yaw;	/**< final yaw when landed [rad].*/
+	};
+
+	RtlDirect(Navigator *navigator);
+
+	~RtlDirect() = default;
+
+	/**
+	 * @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(RtlPosition 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};
+
+	RtlPosition _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 */
+};