flight_task DR RTL: added scaling factors for heading and horizontal motion

msg/versioned/VehicleStatus.msg

@@ -40,7 +40,7 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3         # Auto mission mode
 uint8 NAVIGATION_STATE_AUTO_LOITER = 4          # Auto loiter mode
 uint8 NAVIGATION_STATE_AUTO_RTL = 5             # Auto return to launch mode
 uint8 NAVIGATION_STATE_POSITION_SLOW = 6
-uint8 NAVIGATION_STATE_FREE5 = 7
+uint8 NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING = 7		# Auto dead-reckoning return to launch mode
 uint8 NAVIGATION_STATE_FREE4 = 8
 uint8 NAVIGATION_STATE_FREE3 = 9
 uint8 NAVIGATION_STATE_ACRO = 10                # Acro mode

src/lib/events/enums.json

@@ -421,18 +421,22 @@
                             "description": "RTL"
                         },
                         "7": {
+                            "name": "dead_reckon_rtl",
+                            "description": "Dead Reckoning RTL"
+                        },
+                        "8": {
                             "name": "land",
                             "description": "Land"
                         },
-                        "8": {
+                        "9": {
                             "name": "descend",
                             "description": "Descend"
                         },
-                        "9": {
+                        "10": {
                             "name": "disarm",
                             "description": "disarm"
                         },
-                        "10": {
+                        "11": {
                             "name": "terminate",
                             "description": "terminate"
                         }
@@ -604,6 +608,10 @@
                             "name": "external8",
                             "description": "External 8"
                         },
+                        "24": {
+                            "name": "auto_rtl_dr",
+                            "description": "Dead-Reckoning RTL"
+                        },
                         "255": {
                             "name": "unknown",
                             "description": "[Unknown]"

src/lib/modes/ui.hpp

@@ -52,6 +52,7 @@ static inline uint32_t getValidNavStates()
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_ACRO) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) |
 	       (1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) |
@@ -74,7 +75,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
 	"Hold",
 	"Return",
 	"Position Slow",
-	"7: unallocated",
+	"Dead-Reckoning Return",
 	"8: unallocated",
 	"9: unallocated",
 	"Acro",

src/modules/commander/ModeUtil/control_mode.cpp

@@ -106,6 +106,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 		vehicle_control_mode.flag_control_allocation_enabled = true;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING:
 	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
 		vehicle_control_mode.flag_control_auto_enabled = true;
 		vehicle_control_mode.flag_control_climb_rate_enabled = true;

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -133,6 +133,12 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_home_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_prevent_arming);
 
+	// NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING, flags.mode_req_angular_velocity);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING, flags.mode_req_attitude);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING, flags.mode_req_prevent_arming);
+
 	// NAVIGATION_STATE_ACRO
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_manual_control);

src/modules/commander/commander_params.c

@@ -540,6 +540,22 @@ PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
  */
 PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5.f);
 
+/**
+ * Loss of position autonomous failsafe action
+ *
+ * If no autonomous horizontal navigation is possible anymore should the vehicle attempt a dead-reckon
+ * return to home or shall it attempt to descend blindly and land.
+ *
+ * Action to take when autonomous horizontal navigation is lost:
+ * - "Dead-Reckon RTH" can be preferred to bring the vehicle back to Line of Sight (LOS) range, at which point the pilot can take over control.
+ * - "Land if possible" blind with potential drift and uncontrolled landing (risk of hitting obstacles)
+ *
+ * @group Commander
+ * @value 0 Land if possible
+ * @value 1 Dead-Reckoning RTL
+ */
+PARAM_DEFINE_INT32(COM_POS_FS_ACT, 0);
+
 /**
  * Horizontal velocity error threshold.
  *

src/modules/commander/failsafe/framework.cpp

@@ -581,6 +581,16 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 
 		returned_state.cause = Cause::Generic;
 
+	// fallthrough
+	case Action::DeadReckoningRTL:
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING)
+		    && _param_com_pos_fs_act.get() == 1) {
+			selected_action = Action::DeadReckoningRTL;
+			break;
+		}
+
+		returned_state.cause = Cause::Generic;
+
 	// fallthrough
 	case Action::Land:
 		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
@@ -681,6 +691,8 @@ uint8_t FailsafeBase::modeFromAction(const Action &action, uint8_t user_intended
 
 	case Action::RTL: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
+	case Action::DeadReckoningRTL: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING;
+
 	case Action::Land: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
 
 	case Action::Descend: return vehicle_status_s::NAVIGATION_STATE_DESCEND;

src/modules/commander/failsafe/framework.h

@@ -61,6 +61,7 @@ public:
 
 		Hold,
 		RTL,
+		DeadReckoningRTL,
 		Land,
 		Descend,
 		Disarm,
@@ -105,6 +106,8 @@ public:
 
 		case Action::RTL: return "RTL";
 
+		case Action::DeadReckoningRTL: return "Dead Reckoning RTL";
+
 		case Action::Land: return "Land";
 
 		case Action::Descend: return "Descend";
@@ -293,7 +296,8 @@ private:
 	void *_on_notify_user_arg{nullptr};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
-					(ParamFloat<px4::params::COM_FAIL_ACT_T>) 	_param_com_fail_act_t
+					(ParamFloat<px4::params::COM_FAIL_ACT_T>) 	_param_com_fail_act_t,
+					(ParamInt<px4::params::COM_POS_FS_ACT>)		_param_com_pos_fs_act
 				       );
 
 };

src/modules/commander/px4_custom_mode.h

@@ -73,6 +73,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_EXTERNAL6,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL7,
 	PX4_CUSTOM_SUB_MODE_EXTERNAL8,
+	PX4_CUSTOM_SUB_MODE_AUTO_RTL_DR
 };
 
 enum PX4_CUSTOM_SUB_MODE_POSCTL {
@@ -226,6 +227,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL8;
 		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL_DR;
+		break;
 	}
 
 	return custom_mode;

src/modules/flight_mode_manager/CMakeLists.txt

@@ -48,6 +48,7 @@ list(APPEND flight_tasks_all
 	ManualAltitudeSmoothVel
 	ManualPosition
 	Transition
+	ReturnDeadReckoning
 )
 
 if(NOT px4_constrained_flash_build)

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -152,7 +152,10 @@ void FlightModeManager::start_flight_task()
 	bool found_some_task = false;
 	bool matching_task_running = true;
 	bool task_failure = false;
+
 	const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
+	const bool nav_state_rtl_dr = (_vehicle_status_sub.get().nav_state ==
+				       vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DEAD_RECKONING);
 
 	// Follow me
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
@@ -187,7 +190,8 @@ void FlightModeManager::start_flight_task()
 
 	// Navigator interface for autonomous modes
 	if (_vehicle_control_mode_sub.get().flag_control_auto_enabled
-	    && !nav_state_descend) {
+	    && !nav_state_descend
+	    && !nav_state_rtl_dr) {
 		found_some_task = true;
 
 		if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) {
@@ -249,6 +253,17 @@ void FlightModeManager::start_flight_task()
 		matching_task_running = matching_task_running && !task_failure;
 	}
 
+	// Dead-Reckoning Return
+	if (nav_state_rtl_dr) {
+		found_some_task = true;
+
+		FlightTaskError error = switchTask(FlightTaskIndex::ReturnDeadReckoning);
+
+		task_failure = (error != FlightTaskError::NoError);
+
+		matching_task_running = matching_task_running && !task_failure;
+	}
+
 	// Emergency descend
 	if (nav_state_descend || task_failure) {
 		found_some_task = true;

src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   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(FlightTaskReturnDeadReckoning
+	FlightTaskReturnDeadReckoning.cpp
+)
+
+target_link_libraries(FlightTaskReturnDeadReckoning PUBLIC FlightTask)
+target_include_directories(FlightTaskReturnDeadReckoning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp

@@ -0,0 +1,314 @@
+
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2025 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 FlightTaskReturnDeadReckoning.cpp
+ */
+
+#include "FlightTaskReturnDeadReckoning.hpp"
+
+#include <px4_platform_common/events.h>
+
+bool FlightTaskReturnDeadReckoning::activate(const trajectory_setpoint_s &last_setpoint)
+{
+	PX4_INFO("FlightTaskReturnDeadReckoning::activate");
+
+	if (!FlightTask::activate(last_setpoint)) {
+		PX4_ERR("Failed to activate task");
+		return false;
+	}
+
+	_updateSubscriptions();
+	_state = State::INIT;
+
+	if (!(_updateBearingToHome() && _initializeSmoothers())) {
+		PX4_ERR("Failed to initialize task");
+		return false;
+	}
+
+	_computeReturnParameters();
+
+	return true;
+}
+
+bool FlightTaskReturnDeadReckoning::update()
+{
+	if (!FlightTask::update()) {
+		return false;
+	}
+
+	_updateSubscriptions();
+
+	if (_isGlobalPositionValid()) {
+		// Update the bearing to home if global position is valid
+		if (!_updateBearingToHome()) {
+			PX4_ERR("Failed to compute bearing to home");
+			return false;
+		}
+	}
+
+	_updateState();
+	_updateSetpoints();
+	_updateDistanceFlownEstimate();
+
+	return true;
+}
+
+void FlightTaskReturnDeadReckoning::_updateState()
+{
+	switch (_state) {
+	case State::INIT:
+		if (_isAboveReturnAltitude()) {
+			_state = State::RETURN;
+			events::send<float, float>(events::ID("dead_reckon_rtl_return_direct"), events::Log::Info,
+						   "Returning to home position to home position at {1:.2m_v} with bearing {2:.2} deg", _rtl_alt,
+						   math::degrees(_bearing_to_home));
+
+		} else {
+			_state = State::ASCENT;
+			events::send<float, float>(events::ID("dead_reckon_rtl_ascent"), events::Log::Info,
+						   "Ascending to return altitude {1:.2m_v} with bearing {2:.2} deg", _rtl_alt, math::degrees(_bearing_to_home));
+		}
+
+		break;
+
+	case State::ASCENT:
+		if (_isAboveReturnAltitude()) {
+			_state = State::RETURN;
+			events::send<float, float>(events::ID("dead_reckon_rtl_return"), events::Log::Info,
+						   "Returning to home position to home position at {1:.2m_v} with bearing {2:.2} deg", _rtl_alt,
+						   math::degrees(_bearing_to_home));
+		}
+
+		break;
+
+	case State::RETURN:
+		if (_isReturnComplete()) {
+			_state = State::HOLD;
+			events::send<float>(events::ID("dead_reckon_rtl_hold"), events::Log::Info,
+					    "Holding altitude at {1:.2m_v} over home position", _rtl_alt);
+		}
+
+		break;
+
+	case State::HOLD:
+		break;
+
+	default:
+		PX4_ERR("Unknown state");
+		return;
+	}
+}
+
+void FlightTaskReturnDeadReckoning::_updateSetpoints()
+{
+	switch (_state) {
+	case State::INIT:
+		_slew_rate_acceleration_x.update(0.0f, _deltatime);
+		_slew_rate_acceleration_y.update(0.0f, _deltatime);
+		_slew_rate_velocity_z.update(0.0f, _deltatime);
+
+		// Hold current altitude
+		_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
+		break;
+
+	case State::ASCENT:
+		_slew_rate_acceleration_x.update(0.0f, _deltatime);
+		_slew_rate_acceleration_y.update(0.0f, _deltatime);
+		_slew_rate_velocity_z.update(-_param_mpc_z_v_auto_up.get(), _deltatime);
+
+		// Ascent until reaching the return altitude
+		_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
+		break;
+
+	case State::RETURN:
+		_slew_rate_acceleration_x.update(_rtl_acc * cosf(_bearing_to_home), _deltatime);
+		_slew_rate_acceleration_y.update(_rtl_acc * sinf(_bearing_to_home), _deltatime);
+		_slew_rate_velocity_z.update(0.0f, _deltatime);
+
+		// Stay at the return altitude
+		_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
+		break;
+
+	case State::HOLD:
+		_slew_rate_acceleration_x.update(0.0f, _deltatime);
+		_slew_rate_acceleration_y.update(0.0f, _deltatime);
+		_slew_rate_velocity_z.update(0.0f, _deltatime);
+
+		// Stay at the return altitude
+		_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
+		break;
+
+	default:
+		PX4_ERR("Unknown state");
+		return;
+
+	};
+
+	// Heading setpoint
+	_heading_smoothing.update(_bearing_to_home, _deltatime);
+	_yaw_setpoint = _heading_smoothing.getSmoothedHeading();
+	_yawspeed_setpoint = _heading_smoothing.getSmoothedHeadingRate();
+
+	// Acceleration setpoint
+	_acceleration_setpoint.xy() = matrix::Vector2f(
+					      _slew_rate_acceleration_x.getState(),
+					      _slew_rate_acceleration_y.getState()
+				      );
+
+	return;
+}
+
+void FlightTaskReturnDeadReckoning::_updateDistanceFlownEstimate()
+{
+	if (_state == State::RETURN) {
+		_distance_flown_estimate += _param_mpc_xy_vel_max.get() * _deltatime;
+	}
+}
+
+bool FlightTaskReturnDeadReckoning::_updateBearingToHome()
+{
+	if (_readHomePosition(_home_position)) {
+		_readGlobalPosition(_start_vehicle_global_position);
+		_bearing_to_home = _computeBearing(_start_vehicle_global_position, _home_position);
+
+		_distance_flown_estimate = .0f;
+		_initial_distance_to_home = get_distance_to_next_waypoint(
+						    _start_vehicle_global_position(0), _start_vehicle_global_position(1),
+						    _home_position(0), _home_position(1));
+	}
+
+	return !isnanf(_bearing_to_home);
+}
+
+bool FlightTaskReturnDeadReckoning::_readHomePosition(matrix::Vector3d &home_position)
+{
+	home_position.setNaN();
+
+	if (_sub_home_position.get().valid_hpos && _sub_home_position.get().valid_alt) {
+		home_position(0) = _sub_home_position.get().lat;
+		home_position(1) = _sub_home_position.get().lon;
+		home_position(2) = (double) _sub_home_position.get().alt;
+
+		return true;
+	}
+
+	return false;
+}
+
+void FlightTaskReturnDeadReckoning::_readGlobalPosition(matrix::Vector3d &global_position)
+{
+
+	global_position.setNaN();
+
+	global_position(0) = _sub_vehicle_global_position.get().lat;
+	global_position(1) = _sub_vehicle_global_position.get().lon;
+	global_position(2) = (double) _sub_vehicle_global_position.get().alt;
+}
+
+float FlightTaskReturnDeadReckoning::_computeBearing(const matrix::Vector3d &_global_position_start,
+		const matrix::Vector3d &_global_position_end)
+{
+	float bearing = NAN;
+
+	if (_global_position_start.isAllFinite() && _global_position_end.isAllFinite()) {
+		bearing = get_bearing_to_next_waypoint(_global_position_start(0), _global_position_start(1),
+						       _global_position_end(0), _global_position_end(1));
+	}
+
+	return bearing;
+}
+
+void FlightTaskReturnDeadReckoning::_computeReturnParameters()
+{
+	_rtl_alt = math::max((float) _start_vehicle_global_position(2),
+			     (float) _home_position(2) + _param_rtl_return_alt.get());
+
+	_rtl_acc = _param_mpc_acc_hor_max.get();
+}
+
+void FlightTaskReturnDeadReckoning::_updateSubscriptions()
+{
+	_sub_vehicle_global_position.update();
+}
+
+bool FlightTaskReturnDeadReckoning::_initializeSmoothers()
+{
+	// Initialize the heading smoother
+	_heading_smoothing.setMaxHeadingRate(_heading_smoothing_scaling_factor * _param_mpc_yawrauto_max.get());
+	_heading_smoothing.setMaxHeadingAccel(_heading_smoothing_scaling_factor * _param_mpc_yawrauto_acc.get());
+	_heading_smoothing.reset(_sub_vehicle_local_position.get().heading, 0.0f);
+
+	// Initialize the position smoother
+	_slew_rate_velocity_z.setSlewRate(_velocity_z_rate_scaling_factor * _param_mpc_acc_up_max.get());
+	_slew_rate_velocity_z.setForcedValue((float) _sub_vehicle_local_position.get().vz);
+
+	// Initialize the acceleration smoothers
+	_slew_rate_acceleration_x.setSlewRate(_jerk_scaling_factor * _param_mpc_jerk_auto.get());
+	_slew_rate_acceleration_x.setForcedValue(_sub_vehicle_local_position.get().ax);
+	_slew_rate_acceleration_y.setSlewRate(_jerk_scaling_factor * _param_mpc_jerk_auto.get());
+	_slew_rate_acceleration_y.setForcedValue(_sub_vehicle_local_position.get().ay);
+
+	return true;
+}
+
+bool FlightTaskReturnDeadReckoning::_isGlobalPositionValid() const
+{
+	return _sub_vehicle_global_position.get().lat_lon_valid && _sub_vehicle_global_position.get().alt_valid;
+}
+
+bool FlightTaskReturnDeadReckoning::_isAboveReturnAltitude() const
+{
+	float current_alt = _sub_vehicle_global_position.get().alt;
+	float target_alt = _rtl_alt - _param_nav_mc_alt_rad.get();
+	return current_alt > target_alt;
+}
+
+bool FlightTaskReturnDeadReckoning::_isReturnComplete()
+{
+	bool ret = false;
+
+	if (_isGlobalPositionValid()) {
+		// Close enough to home posititon
+		_readGlobalPosition(_start_vehicle_global_position);
+		ret = get_distance_to_next_waypoint(
+			      _start_vehicle_global_position(0), _start_vehicle_global_position(1),
+			      _home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
+	}
+
+	if (!ret) {
+		ret = _distance_flown_estimate > 2.5f * _initial_distance_to_home; // 2.5x the initial distance to home
+	}
+
+	return ret;
+}

src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp

@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2025 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 FlightTaskReturnDeadReckoning.hpp
+ */
+
+#pragma once
+
+#include "FlightTask.hpp"
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_global_position.h>
+#include <lib/motion_planning/HeadingSmoothing.hpp>
+#include <lib/motion_planning/PositionSmoothing.hpp>
+#include <lib/slew_rate/SlewRate.hpp>
+
+class FlightTaskReturnDeadReckoning : public FlightTask
+{
+public:
+	FlightTaskReturnDeadReckoning() = default;
+	virtual ~FlightTaskReturnDeadReckoning() = default;
+
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool update() override;
+
+private:
+	uORB::SubscriptionData<vehicle_global_position_s> _sub_vehicle_global_position{ORB_ID(vehicle_global_position)};
+
+	/**
+	 * Update the flight task state machine
+	 */
+	void _updateState();
+
+	/**
+	 * Update the trajectory setpoints according to the current state
+	 */
+	void _updateSetpoints();
+
+	/**
+	 * Update estimated flown distance since the last time home bearing was updated
+	 */
+	void _updateDistanceFlownEstimate();
+
+	/**
+	 * Update bearing to home position with the last available GNSS position
+	 * @return true on success, false on error
+	 */
+	bool _updateBearingToHome();
+
+	/**
+	 * Initialize home position
+	 * @return true on success, false on error
+	 */
+	bool _readHomePosition(matrix::Vector3d &home_position);
+
+	/**
+	 * Reads current GNSS position
+	 */
+	void _readGlobalPosition(matrix::Vector3d &global_position);
+
+	/**
+	 * Compute bearing to home position
+	 * @return true on success, false on error
+	 */
+	float _computeBearing(const matrix::Vector3d &_global_position_start,
+			      const matrix::Vector3d &_global_position_end);
+
+	/**
+	 * Computes return altitude and acceleration
+	 */
+	void _computeReturnParameters();
+
+	/**
+	 * Update uORB subscriptions
+	 */
+	void _updateSubscriptions();
+
+	/**
+	 * Initialize the trajectory setpoint smoothers
+	 * @return true on success, false on error
+	 */
+	bool _initializeSmoothers();
+
+	/**
+	 * Check if the global position is valid
+	 * @return true if valid, false otherwise
+	 */
+	bool _isGlobalPositionValid() const;
+
+	/**
+	 * Check if the vehicle is above the return altitude
+	 * @return true if above, false otherwise
+	 */
+	bool _isAboveReturnAltitude() const;
+
+	/**
+	 * Check if the vehicle is within the home position waypoint acceptance radius, or if the distance traveled is greater than the initial distance to home (times a factor)
+	 * @return true if completed, false otherwise
+	 */
+	bool _isReturnComplete();
+
+
+	matrix::Vector3d _home_position;			/**< Stores home position */
+	matrix::Vector3d _start_vehicle_global_position;	/**< Stores vehicle last known GNSS position */
+	float _bearing_to_home{0.0f};				/**< Stores bearing between home and last GNSS position */
+	float _initial_distance_to_home{0.0f};			/**< Initial distance to home position */
+	float _distance_flown_estimate{0.0f};
+
+	float _heading_smoothing_scaling_factor{0.5f};		/**< Scaling factor applied to heading smoothing limits */
+	HeadingSmoothing _heading_smoothing;			/**< Smoother for heading */
+	float _jerk_scaling_factor{0.2f};			/**< Scaling factor applied to acceleration slew rate (jerk) */
+	SlewRate<float> _slew_rate_acceleration_x{0.0f};	/**< Slew rate for x-acceleration setpoint */
+	SlewRate<float> _slew_rate_acceleration_y{0.0f};	/**< Slew rate for y-acceleration setpoint */
+	float _velocity_z_rate_scaling_factor{0.4f};		/**< Scaling factor applied to the vertical velocity slew rate */
+	SlewRate<float> _slew_rate_velocity_z{0.0f};		/**< Slew rate for vertical velocity */
+
+	float _rtl_alt{0.0f};					/**< Return altitude */
+	float _rtl_acc{0.0f};					/**< Return acceleration */
+
+	/**< State machine for flight task */
+	enum class State {
+		UNKNOWN = 0,
+		INIT,
+		ASCENT,
+		RETURN,
+		HOLD
+	};
+	State _state{State::UNKNOWN};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
+					(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,	//< RTL altitude
+					(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,	//< max yaw rate in auto modes
+					(ParamFloat<px4::params::MPC_YAWRAUTO_ACC>) _param_mpc_yawrauto_acc,	//< max yaw acceleration in auto modes
+					(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,		//< maximum jerk in auto modes
+					(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,	//< max vertical velocity up
+					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,	//< max vertical acceleration up
+					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,	//< max horizontal velocity
+					(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad,	//< max vertical error
+					(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad		//< max horizontal error
+				       );
+};