commander: added option to attempt dead-reckon RTL

src/modules/commander/commander_params.c

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

@@ -573,7 +573,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 
 	// fallthrough
 	case Action::DeadReckonRTL:
-		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR)) {
+		if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL_DR)
+		    && _param_com_pos_fs_act.get() == 1) {
 			selected_action = Action::DeadReckonRTL;
 			break;
 		}

src/modules/commander/failsafe/framework.h

@@ -296,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/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp

@@ -97,7 +97,7 @@ void FlightTaskReturnDeadReckoning::_updateState()
 		} 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));
+						   "Ascending to return altitude {1:.2m_v} with bearing {2:.2} deg", _rtl_alt, math::degrees(_bearing_to_home));
 		}
 
 		break;
@@ -204,8 +204,8 @@ bool FlightTaskReturnDeadReckoning::_updateBearingToHome()
 
 		_distance_flown_estimate = .0f;
 		_initial_distance_to_home = get_distance_to_next_waypoint(
-			_start_vehicle_global_position(0), _start_vehicle_global_position(1),
+						    _start_vehicle_global_position(0), _start_vehicle_global_position(1),
-			_home_position(0), _home_position(1));
+						    _home_position(0), _home_position(1));
 	}
 
 	return !isnanf(_bearing_to_home);
@@ -251,7 +251,8 @@ float FlightTaskReturnDeadReckoning::_computeBearing(const matrix::Vector3d &_gl
 
 void FlightTaskReturnDeadReckoning::_computeReturnParameters()
 {
-	_rtl_alt = math::max((float) _start_vehicle_global_position(2), (float) _home_position(2) + _param_rtl_return_alt.get());
+	_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();
 }
@@ -296,12 +297,13 @@ bool FlightTaskReturnDeadReckoning::_isAboveReturnAltitude() const
 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),
+			      _start_vehicle_global_position(0), _start_vehicle_global_position(1),
-			       _home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
+			      _home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
 	}
 
 	if (!ret) {