diff --git a/docs/en/config/safety.md b/docs/en/config/safety.md index df12c630b1..bef0e0ea18 100644 --- a/docs/en/config/safety.md +++ b/docs/en/config/safety.md @@ -128,14 +128,15 @@ Additional (and underlying) parameter settings are shown below. | Parameter | Setting | Description | | ----------------------------------------------------------------------------------------------------- | --------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| [COM_RC_LOSS_T](../advanced_config/parameter_reference.md#COM_RC_LOSS_T) | Manual Control Loss Timeout | Time after last setpoint received from the selected manual control source after which manual control is considered lost. This must be kept short because the vehicle will continue to fly using the old manual control setpoint until the timeout triggers. | +| [COM_RC_LOSS_T](../advanced_config/parameter_reference.md#COM_RC_LOSS_T) | Manual Control Loss Timeout | Time after last setpoint received from the selected manual control source after which manual control is considered lost. This must be kept short because the vehicle will continue to fly using the last known stick position until the timeout triggers. | | [COM_FAIL_ACT_T](../advanced_config/parameter_reference.md#COM_FAIL_ACT_T) | Failsafe Reaction Delay | Delay in seconds between failsafe condition being triggered (`COM_RC_LOSS_T`) and failsafe action (RTL, Land, Hold). In this state the vehicle waits in hold mode for the manual control source to reconnect. This might be set longer for long-range flights so that intermittent connection loss doesn't immediately invoke the failsafe. It can be to zero so that the failsafe triggers immediately. | | [NAV_RCL_ACT](../advanced_config/parameter_reference.md#NAV_RCL_ACT) | Failsafe Action | Disabled, Loiter, Return, Land, Disarm, Terminate. | -| [COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT) | RC Loss Exceptions | Set the modes in which manual control loss is ignored. | +| [COM_RCL_EXCEPT](../advanced_config/parameter_reference.md#COM_RCL_EXCEPT) | RC Loss Exceptions | Set modes in which manual control loss is ignored. | ## Data Link Loss Failsafe -The Data Link Loss failsafe is triggered if a telemetry link (connection to ground station) is lost. +The Data Link Loss failsafe is triggered if the connection to the last MAVLink ground station like QGroundControl is lost. +Users that want to disable this failsafe in specific modes can do so using the parameter [COM_DLL_EXCEPT](#COM_DLL_EXCEPT). ![Safety - Data Link Loss (QGC)](../../assets/qgc/setup/safety/safety_data_link_loss.png) @@ -145,12 +146,7 @@ The settings and underlying parameters are shown below. | ---------------------- | ------------------------------------------------------------------------ | --------------------------------------------------------------------------------- | | Data Link Loss Timeout | [COM_DL_LOSS_T](../advanced_config/parameter_reference.md#COM_DL_LOSS_T) | Amount of time after losing the data connection before the failsafe will trigger. | | Failsafe Action | [NAV_DLL_ACT](../advanced_config/parameter_reference.md#NAV_DLL_ACT) | Disabled, Hold mode, Return mode, Land mode, Disarm, Terminate. | - -The following settings also apply, but are not displayed in the QGC UI. - -| Setting | Parameter | Description | -| ----------------------------------------------------------- | -------------------------------------------------------------------------- | ---------------------------------------------------- | -| Mode exceptions for DLL failsafe | [COM_DLL_EXCEPT](../advanced_config/parameter_reference.md#COM_DLL_EXCEPT) | Set modes where DL loss will not trigger a failsafe. | +| Mode exceptions for DLL failsafe | [COM_DLL_EXCEPT](../advanced_config/parameter_reference.md#COM_DLL_EXCEPT) | Set modes in which data link loss is ignored. | ## Geofence Failsafe diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index c060884d68..eb3c2109e4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -607,7 +607,7 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2); /** * Manual control loss exceptions * - * Specify modes where manual control loss is ignored and no failsafe is triggered. + * Specify modes in which stick input is ignored and no failsafe action is triggered. * External modes requiring stick input will still failsafe. * Auto modes are: Hold, Takeoff, Land, RTL, Descend, Follow Target, Precland, Orbit. * @@ -625,13 +625,16 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); /** * Datalink loss exceptions * - * Specify modes in which datalink loss is ignored and the failsafe action not triggered. + * Specify modes in which ground control station connection loss is ignored and no failsafe action is triggered. + * See also COM_RCL_EXCEPT. * * @min 0 - * @max 7 + * @max 31 * @bit 0 Mission - * @bit 1 Hold + * @bit 1 Auto modes * @bit 2 Offboard + * @bit 3 External Mode + * @bit 4 Altitude Cruise * @group Commander */ PARAM_DEFINE_INT32(COM_DLL_EXCEPT, 0); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index f145d332b5..5090b22a9c 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -442,18 +442,53 @@ FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int par return options; } +bool Failsafe::isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter) +{ + switch (user_intended_mode) { + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + return exception_mask_parameter & (int)LinkLossExceptionBits::Mission; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + case vehicle_status_s::NAVIGATION_STATE_ORBIT: + return exception_mask_parameter & (int)LinkLossExceptionBits::AutoModes; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + return exception_mask_parameter & (int)LinkLossExceptionBits::Offboard; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: + return exception_mask_parameter & (int)LinkLossExceptionBits::ExternalMode; + + case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE: + return exception_mask_parameter & (int)LinkLossExceptionBits::AltitudeCruise; + + default: + return false; + } +} + void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const failsafe_flags_s &status_flags) { updateArmingState(time_us, state.armed, status_flags); - const bool in_forward_flight = state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - || state.vtol_in_transition_mode; - // 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_any_link_loss_vtol_takeoff_fixedwing = state.user_intended_mode == - vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF + const bool in_forward_flight = (state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || state.vtol_in_transition_mode; + 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 or joystick) loss @@ -462,67 +497,23 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, _manual_control_lost_at_arming = false; } - const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Mission); - - const bool rc_loss_ignored_auto_modes = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_DESCEND || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_ORBIT) - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::AutoModes); - - const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Offboard); - - const bool rc_loss_ignored_altitude_cruise = (state.user_intended_mode == - vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::AltitudeCruise)); - - const bool rc_loss_ignored_external_mode = - (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL2 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL3 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL4 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL5 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL6 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL7 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL8) - && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::ExternalMode); - - const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_auto_modes || rc_loss_ignored_offboard || - rc_loss_ignored_external_mode || ignore_any_link_loss_vtol_takeoff_fixedwing - || _manual_control_lost_at_arming || rc_loss_ignored_altitude_cruise; + const bool rc_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get()) + || ignore_any_link_loss_vtol_takeoff_fixedwing || _manual_control_lost_at_arming; if (_param_com_rc_in_mode.get() != int32_t(RcInMode::DisableManualControl) && !rc_loss_ignored) { CHECK_FAILSAFE(status_flags, manual_control_signal_lost, fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss)); } - // GCS connection loss + // Ground control station connection loss 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; - 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; + const bool dll_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_dll_except.get()) + || 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)); + CHECK_FAILSAFE(status_flags, gcs_connection_lost, fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss)); } // VTOL transition failure (quadchute) @@ -539,7 +530,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, // If manual control loss and GCS connection loss are disabled and we lose both command links and the mission finished, // trigger RTL to avoid losing the vehicle - if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl) || rc_loss_ignored_mission) + if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl) + || isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get())) && _param_nav_dll_act.get() == int32_t(gcs_connection_loss_failsafe_mode::Disabled) && state.mission_finished) { _last_state_mission_control_lost = checkFailsafe(_caller_id_mission_control_lost, _last_state_mission_control_lost, diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 2e8bc171b3..d5b3f0a79e 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -53,7 +53,7 @@ protected: private: void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags); - enum class ManualControlLossExceptionBits : int32_t { + enum class LinkLossExceptionBits : int32_t { Mission = (1 << 0), AutoModes = (1 << 1), Offboard = (1 << 2), @@ -61,12 +61,6 @@ private: AltitudeCruise = (1 << 4) }; - 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 @@ -175,6 +169,8 @@ private: static ActionOptions fromPosLowActParam(int param_value); static ActionOptions fromRemainingFlightTimeLowActParam(int param_value); + static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter); + const int _caller_id_mode_fallback{genCallerId()}; bool _last_state_mode_fallback{false}; const int _caller_id_mission_control_lost{genCallerId()};