fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add COM_DLL_EXCEPT to specifiy exceptions for data link loss failsafe

Browse Source
This commit is contained in:
Roman Bapst 2025-03-28 19:41:24 +03:00 committed by GitHub
parent 72454c4fd2
commit 2c8ef05c2d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 44 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/commander/commander_params.c
View File

@ -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
*

28
src/modules/commander/failsafe/failsafe.cpp
View File

@ -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));
}

7
src/modules/commander/failsafe/failsafe.h
View File

@ -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<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
(ParamInt<px4::params::COM_DLL_EXCEPT>) _param_com_dll_except,
(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl,
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,