rtl_direct: Move the time estimation calculation into a separate helper class

2024-01-26 09:46:00 +01:00 · 2024-01-26 09:46:00 +01:00 · 14e4169473
parent 8dcfcf5b9e
commit 14e4169473
7 changed files with 481 additions and 217 deletions
--- a/src/lib/CMakeLists.txt
+++ b/src/lib/CMakeLists.txt
@ -67,6 +67,7 @@ add_subdirectory(pid_design EXCLUDE_FROM_ALL)
 add_subdirectory(rate_control EXCLUDE_FROM_ALL)
 add_subdirectory(rc EXCLUDE_FROM_ALL)
 add_subdirectory(ringbuffer EXCLUDE_FROM_ALL)
+add_subdirectory(rtl EXCLUDE_FROM_ALL)
 add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL)
 add_subdirectory(slew_rate EXCLUDE_FROM_ALL)
 add_subdirectory(systemlib EXCLUDE_FROM_ALL)
--- a/src/lib/rtl/CMakeLists.txt
+++ b/src/lib/rtl/CMakeLists.txt
@ -0,0 +1,34 @@
+############################################################################
+#
+#   Copyright (c) 2024 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(rtl_time_estimator rtl_time_estimator.cpp)
--- a/src/lib/rtl/rtl_time_estimator.cpp
+++ b/src/lib/rtl/rtl_time_estimator.cpp
@ -0,0 +1,267 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 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_time_estimator.cpp
+ *
+ * Helper class to calculate the remaining time estimate to go to RTL landing point.
+ *
+ */
+
+#include "rtl_time_estimator.h"
+
+#include <float.h>
+#include <math.h>
+
+#include <px4_platform_common/defines.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/vehicle_status.h>
+
+RtlTimeEstimator::RtlTimeEstimator() : ModuleParams(nullptr)
+{
+	_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");
+};
+
+rtl_time_estimate_s RtlTimeEstimator::getEstimate() const
+{
+	rtl_time_estimate_s time_estimate{};
+
+	if (_is_valid && PX4_ISFINITE(_time_estimate)) {
+		time_estimate.valid = true;
+		time_estimate.time_estimate = _time_estimate;
+		// Use actual time estimate to compute the safer time estimate with additional scale factor and a margin
+		time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * _time_estimate + _param_rtl_time_margin.get();
+
+	} else {
+		time_estimate.valid = false;
+	}
+
+	time_estimate.timestamp = hrt_absolute_time();
+	return time_estimate;
+}
+
+void RtlTimeEstimator::update()
+{
+	_vehicle_status_sub.update();
+	_wind_sub.update();
+
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		// If any parameter updated, call updateParams() to check if
+		// this class attributes need updating (and do so).
+		updateParams();
+	}
+}
+
+void RtlTimeEstimator::addVertDistance(float alt)
+{
+	if (PX4_ISFINITE(alt)) {
+		_is_valid = true;
+
+		_time_estimate += calcVertTimeEstimate(alt);
+	}
+}
+
+void RtlTimeEstimator::addDistance(float hor_dist, const matrix::Vector2f &direction, float vert_dist)
+{
+	if (PX4_ISFINITE(hor_dist) && PX4_ISFINITE(vert_dist)) {
+		_is_valid = true;
+
+		float hor_time_estimate{0.f};
+
+		if (hor_dist > FLT_EPSILON && PX4_ISFINITE(hor_dist)) {
+			hor_time_estimate = hor_dist / getCruiseGroundSpeed(direction.normalized());
+		}
+
+		float ver_time_estimate{calcVertTimeEstimate(vert_dist)};
+
+		_time_estimate += math::max(hor_time_estimate, ver_time_estimate);
+
+	}
+}
+
+void RtlTimeEstimator::addWait(float time_s)
+{
+	if (PX4_ISFINITE(time_s)) {
+		_is_valid = true;
+
+		if (time_s > FLT_EPSILON) {
+			_time_estimate += time_s;
+		}
+	}
+}
+
+void RtlTimeEstimator::addDescendMCLand(float alt)
+{
+	if (PX4_ISFINITE(alt)) {
+		_is_valid = true;
+
+		if (alt < -FLT_EPSILON && PX4_ISFINITE(alt)) {
+			_time_estimate += -alt / getHoverLandSpeed();
+		}
+	}
+}
+
+float RtlTimeEstimator::getCruiseGroundSpeed(const matrix::Vector2f &direction_norm)
+{
+	float cruise_speed = getCruiseSpeed();
+
+	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		matrix::Vector2f wind = get_wind();
+
+		const float wind_along_dir = wind.dot(direction_norm);
+		const float wind_across_dir = matrix::Vector2f(wind - direction_norm * wind_along_dir).norm();
+
+		// Note: use fminf so that we don't _rely_ on tailwind towards direction to make RTL more efficient
+		const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_dir * wind_across_dir) + fminf(
+						   0.f, wind_along_dir);
+
+		cruise_speed = ground_speed;
+	}
+
+	return cruise_speed;
+}
+
+float RtlTimeEstimator::calcVertTimeEstimate(float alt)
+{
+	float vertical_rate{0.1f};
+	float time_estimate{0.f};
+
+	if (alt > FLT_EPSILON) {
+		vertical_rate = getClimbRate();
+
+	} else {
+		vertical_rate = getDescendRate();
+	}
+
+	float abs_alt = fabsf(alt);
+
+	if (abs_alt > FLT_EPSILON) {
+		time_estimate = abs_alt / vertical_rate;
+	}
+
+	return time_estimate;
+}
+
+float RtlTimeEstimator::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 RtlTimeEstimator::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 RtlTimeEstimator::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 RtlTimeEstimator::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 RtlTimeEstimator::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;
+}
--- a/src/lib/rtl/rtl_time_estimator.h
+++ b/src/lib/rtl/rtl_time_estimator.h
@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 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_time_estimator.h
+ *
+ * Helper class to calculate the remaining time estimate to go to RTL landing point.
+ *
+ */
+
+#ifndef RTL_TIME_ESTIMATOR_H_
+#define RTL_TIME_ESTIMATOR_H_
+
+#include <px4_platform_common/module_params.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/rtl_time_estimate.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/wind.h>
+
+#include <matrix/Vector2.hpp>
+
+using namespace time_literals;
+
+class Navigator;
+
+class RtlTimeEstimator : public ModuleParams
+{
+public:
+	RtlTimeEstimator();
+	~RtlTimeEstimator() = default;
+
+	void update();
+	void reset() { _time_estimate = 0.f; _is_valid = false;};
+	rtl_time_estimate_s getEstimate() const;
+	void addDistance(float hor_dist, const matrix::Vector2f &hor_direction, float vert_dist);
+	void addVertDistance(float alt);
+	void addWait(float time_s);
+	void addDescendMCLand(float alt);
+
+private:
+	/**
+	 * @brief Get the Cruise Ground Speed
+	 *
+	 * @param direction_norm normalized direction in which to fly
+	 * @return Ground speed in cruise mode [m/s].
+	 */
+	float getCruiseGroundSpeed(const matrix::Vector2f &direction_norm);
+
+	/**
+	 * @brief Get time estimate of vertical distance
+	 *
+	 */
+	float calcVertTimeEstimate(float alt);
+
+	/**
+	 * @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();
+
+	/**
+	 * @brief Get the horizontal wind velocity
+	 *
+	 * @return horizontal wind velocity.
+	 */
+	matrix::Vector2f get_wind();
+
+	float _time_estimate; 		/**< Accumulated time estimate [s] */
+	bool _is_valid{false};		/**< Checks if time estimate is valid */
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,  /**< Safety factory for safe time estimate */
+		(ParamInt<px4::params::RTL_TIME_MARGIN>)   _param_rtl_time_margin   /**< Safety margin for safe time estimate */
+	)
+
+	param_t		_param_mpc_z_v_auto_up{PARAM_INVALID}; 	/**< MC climb velocity parameter */
+	param_t		_param_mpc_z_v_auto_dn{PARAM_INVALID};  /**< MC descend velocity parameter */
+	param_t		_param_mpc_land_speed{PARAM_INVALID};   /**< MC land descend speed parameter */
+	param_t		_param_fw_climb_rate{PARAM_INVALID};    /**< FW climb speed parameter */
+	param_t		_param_fw_sink_rate{PARAM_INVALID};     /**< FW descend speed parameter */
+
+	param_t 	_param_fw_airspeed_trim{PARAM_INVALID}; /**< FW cruise airspeed parameter */
+	param_t 	_param_mpc_xy_cruise{PARAM_INVALID};  	/**< MC horizontal cruise speed parameter */
+	param_t 	_param_rover_cruise_speed{PARAM_INVALID}; /**< Rover cruise speed parameter */
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< Parameter update topic */
+	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};	/**< vehicle status subscription */
+	uORB::SubscriptionData<wind_s>		_wind_sub{ORB_ID(wind)};		/**< wind topic */
+};
+
+#endif /* RTL_TIME_ESTIMATOR_H_ */
--- a/src/modules/navigator/CMakeLists.txt
+++ b/src/modules/navigator/CMakeLists.txt
@ -69,4 +69,5 @@ px4_add_module(
 		geofence_breach_avoidance
 		motion_planning
 		mission_feasibility_checker
+		rtl_time_estimator
 	)
--- a/src/modules/navigator/rtl_direct.cpp
+++ b/src/modules/navigator/rtl_direct.cpp
@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2024 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
@ -59,15 +59,6 @@ RtlDirect::RtlDirect(Navigator *navigator) :
 	_land_approach.lat = static_cast<double>(NAN);
 	_land_approach.lon = static_cast<double>(NAN);
 	_land_approach.height_m = NAN;
-
-	_param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP");
-	_param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN");
-	_param_mpc_land_speed = param_find("MPC_LAND_SPEED");
-	_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_inactivation()
@ -379,8 +370,9 @@ RtlDirect::RTLState RtlDirect::getActivationLandState()
 rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 {
 	_global_pos_sub.update();
+	_rtl_time_estimator.update();

-	rtl_time_estimate_s rtl_time_estimate{};
+	_rtl_time_estimator.reset();

 	RTLState start_state_for_estimate;

@ -393,12 +385,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()

 	// 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;
-		rtl_time_estimate.time_estimate = 0.f;
+	if (_navigator->home_global_position_valid()) {

 		const float loiter_altitude = min(_land_approach.height_m, _rtl_alt);

@ -406,19 +393,19 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 		switch (start_state_for_estimate) {
 		case RTLState::CLIMBING: {
 				// Climb segment is only relevant if the drone is below return altitude
-				const float climb_dist = _global_pos_sub.get().alt < _rtl_alt ? (_rtl_alt - _global_pos_sub.get().alt) : 0;
-
-				if (climb_dist > FLT_EPSILON) {
-					rtl_time_estimate.time_estimate += climb_dist / getClimbRate();
+				if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) {
+					_rtl_time_estimator.addVertDistance(_rtl_alt - _global_pos_sub.get().alt);
 				}
 			}

 		// FALLTHROUGH
-		case RTLState::MOVE_TO_LOITER:
-
-			// Add cruise segment to home
-			rtl_time_estimate.time_estimate += get_distance_to_next_waypoint(
-					_land_approach.lat, _land_approach.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed();
+		case RTLState::MOVE_TO_LOITER: {
+				matrix::Vector2f direction{};
+				get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _land_approach.lat,
+							    _land_approach.lon, &direction(0), &direction(1));
+				_rtl_time_estimator.addDistance(get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
+								_land_approach.lat, _land_approach.lon), direction, 0.f);
+			}

 		// FALLTHROUGH
 		case RTLState::LOITER_DOWN: {
@ -434,15 +421,18 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 					initial_altitude = _rtl_alt; // CLIMB and RETURN
 				}

-				// Add descend segment (first landing phase: return alt to loiter alt)
-				rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate();
+				_rtl_time_estimator.addVertDistance(loiter_altitude - initial_altitude);
 			}

 		// FALLTHROUGH
 		case RTLState::LOITER_HOLD:
 			// Add land delay (the short pause for deploying landing gear)
-			// TODO: Check if landing gear is deployed or not
-			rtl_time_estimate.time_estimate += _param_rtl_land_delay.get();
+			_rtl_time_estimator.addWait(_param_rtl_land_delay.get());
+
+			if (_param_rtl_land_delay.get() < -FLT_EPSILON) { // Set to loiter infinitely and not land. Stop calculation here
+				break;
+			}
+

 		// FALLTHROUGH
 		case RTLState::MOVE_TO_LAND:
@ -450,19 +440,22 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 		case RTLState::MOVE_TO_LAND_HOVER: {
 				// Add cruise segment to home
 				float move_to_land_dist{0.f};
+				matrix::Vector2f direction{};

 				if (start_state_for_estimate >= RTLState::MOVE_TO_LAND) {
 					move_to_land_dist = get_distance_to_next_waypoint(
-								    _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon);
+								    _global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, _destination.lon);
+					get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, _destination.lon,
+								    &direction(0), &direction(1));

 				} else {
 					move_to_land_dist = get_distance_to_next_waypoint(
-								    _destination.lat, _destination.lon, _land_approach.lat, _land_approach.lon);
+								    _land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon);
+					get_vector_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon, &direction(0),
+								    &direction(1));
 				}

-				if (move_to_land_dist > FLT_EPSILON) {
-					rtl_time_estimate.time_estimate += move_to_land_dist / getCruiseGroundSpeed();
-				}
+				_rtl_time_estimator.addDistance(move_to_land_dist, direction, 0.f);
 			}

 		// FALLTHROUGH
@ -482,10 +475,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 					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();
-				}
+				_rtl_time_estimator.addDescendMCLand(_destination.alt - initial_altitude);
 			}

 			break;
@ -494,132 +484,9 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate()
 			// 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 (_navigator->get_vstatus()->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 (_navigator->get_vstatus()->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 (_navigator->get_vstatus()->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 (_navigator->get_vstatus()->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 (_navigator->get_vstatus()->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 (_navigator->get_vstatus()->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 (_navigator->get_vstatus()->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 (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-		const vehicle_global_position_s &global_position = *_navigator->get_global_position();
-		matrix::Vector2f wind = get_wind();
-
-		matrix::Vector2f to_destination_vec;
-		get_vector_to_next_waypoint(global_position.lat, global_position.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;
+	return _rtl_time_estimator.getEstimate();
 }

 void RtlDirect::parameters_update()
--- a/src/modules/navigator/rtl_direct.h
+++ b/src/modules/navigator/rtl_direct.h
@ -54,6 +54,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>

+#include <lib/rtl/rtl_time_estimator.h>
 #include "mission_block.h"
 #include "navigation.h"
 #include "safe_point_land.hpp"
@ -119,54 +120,12 @@ private:
 	} _rtl_state{RTLState::IDLE}; /*< Current state in the state machine.*/

 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 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();
-
 	/**
 	 * Check for parameter changes and update them if needed.
 	 */
@ -178,6 +137,7 @@ private:

 	bool _enforce_rtl_alt{false};
 	bool _force_heading{false};
+	RtlTimeEstimator _rtl_time_estimator;

 	PositionYawSetpoint _destination; ///< the RTL position to fly to
 	loiter_point_s _land_approach;
@ -190,23 +150,11 @@ private:
 		(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,
-		(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,
-		(ParamInt<px4::params::RTL_TIME_MARGIN>)   _param_rtl_time_margin,

 		// external params
 		(ParamBool<px4::params::WV_EN>) _param_wv_en
 	)

-	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::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)};	/**< global position subscription */
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */