From 9ca3a03352baea6aaacda808ee296ab6d6bbae3e Mon Sep 17 00:00:00 2001 From: oravla5 Date: Wed, 2 Jul 2025 11:34:13 +0200 Subject: [PATCH] commander: added option to attempt dead-reckon RTL --- src/modules/commander/commander_params.c | 16 ++++++++++++++++ src/modules/commander/failsafe/framework.cpp | 3 ++- src/modules/commander/failsafe/framework.h | 3 ++- .../FlightTaskReturnDeadReckoning.cpp | 14 ++++++++------ 4 files changed, 28 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index e0822148d2..1d70639f68 100644 --- a/src/modules/commander/commander_params.c +++ b/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. * diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 8397a8643f..a57c8f033a 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/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; } diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h index 6ad1f45b76..3277efce47 100644 --- a/src/modules/commander/failsafe/framework.h +++ b/src/modules/commander/failsafe/framework.h @@ -296,7 +296,8 @@ private: void *_on_notify_user_arg{nullptr}; DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, - (ParamFloat) _param_com_fail_act_t + (ParamFloat) _param_com_fail_act_t, + (ParamInt) _param_com_pos_fs_act ); }; diff --git a/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp index 01d70fabd0..231631496e 100644 --- a/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp +++ b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp @@ -97,7 +97,7 @@ void FlightTaskReturnDeadReckoning::_updateState() } else { _state = State::ASCENT; events::send(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), - _home_position(0), _home_position(1)); + _start_vehicle_global_position(0), _start_vehicle_global_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), - _home_position(0), _home_position(1)) < _param_nav_acc_rad.get(); + _start_vehicle_global_position(0), _start_vehicle_global_position(1), + _home_position(0), _home_position(1)) < _param_nav_acc_rad.get(); } if (!ret) {