From 2c8ef05c2d9eee2812d6986153a9f338f2fbd483 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 28 Mar 2025 19:41:24 +0300 Subject: [PATCH] Add COM_DLL_EXCEPT to specifiy exceptions for data link loss failsafe --- src/modules/commander/commander_params.c | 16 +++++++++++- src/modules/commander/failsafe/failsafe.cpp | 28 ++++++++++++++++----- src/modules/commander/failsafe/failsafe.h | 7 ++++++ 3 files changed, 44 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 593923ae18..3f350618b6 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -615,7 +615,7 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2); * Specify modes in which RC loss is ignored and the failsafe action not triggered. * * @min 0 - * @max 31 + * @max 7 * @bit 0 Mission * @bit 1 Hold * @bit 2 Offboard @@ -623,6 +623,20 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2); */ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); +/** + * Datalink loss exceptions + * + * Specify modes in which datalink loss is ignored and the failsafe action not triggered. + * + * @min 0 + * @max 7 + * @bit 0 Mission + * @bit 1 Hold + * @bit 2 Offboard + * @group Commander + */ +PARAM_DEFINE_INT32(COM_DLL_EXCEPT, 0); + /** * Set the actuator failure failsafe mode * diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 329a601b6d..07fb568812 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -452,8 +452,9 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, // Do not enter failsafe while doing a vtol takeoff after the vehicle has started a transition and before it reaches the loiter // altitude. The vtol takeoff navigaton mode will set mission_finished to true as soon as the loiter is established - const bool ignore_link_failsafe = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF - && in_forward_flight && !state.mission_finished; + const bool ignore_any_link_loss_vtol_takeoff_fixedwing = state.user_intended_mode == + vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF + && in_forward_flight && !state.mission_finished; // Manual control (RC) loss if (!status_flags.manual_control_signal_lost) { @@ -470,8 +471,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold); + const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || - rc_loss_ignored_takeoff || ignore_link_failsafe || _manual_control_lost_at_arming; + rc_loss_ignored_takeoff || ignore_any_link_loss_vtol_takeoff_fixedwing + || _manual_control_lost_at_arming; if (_param_com_rc_in_mode.get() != int32_t(RcInMode::StickInputDisabled) && !rc_loss_ignored) { CHECK_FAILSAFE(status_flags, manual_control_signal_lost, @@ -479,10 +482,23 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, } // GCS connection loss - const bool gcs_connection_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || ignore_link_failsafe; + const bool dll_loss_ignored_land = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND + || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; - if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !gcs_connection_loss_ignored) { + const bool dll_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION + && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Mission); + const bool dll_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER + && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold); + const bool dll_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD + && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Offboard); + const bool dll_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) + && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold); + + const bool dll_loss_ignored = dll_loss_ignored_mission || dll_loss_ignored_loiter || dll_loss_ignored_offboard || + dll_loss_ignored_takeoff || ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land; + + if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !dll_loss_ignored) { CHECK_FAILSAFE(status_flags, gcs_connection_lost, fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss)); } diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 71308c9d6f..0db6748f4b 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -59,6 +59,12 @@ private: Offboard = (1 << 2) }; + enum class DatalinkLossExceptionBits : int32_t { + Mission = (1 << 0), + Hold = (1 << 1), + Offboard = (1 << 2) + }; + // COM_LOW_BAT_ACT parameter values enum class LowBatteryAction : int32_t { Warning = 0, // Warning @@ -193,6 +199,7 @@ private: (ParamInt) _param_nav_dll_act, (ParamInt) _param_nav_rcl_act, (ParamInt) _param_com_rcl_except, + (ParamInt) _param_com_dll_except, (ParamInt) _param_com_rc_in_mode, (ParamInt) _param_com_posctl_navl, (ParamInt) _param_gf_action,