From a7f2f2908b16edd2c56fae9b93f4d421384dc884 Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Wed, 15 Apr 2020 19:30:33 +0200 Subject: [PATCH] Commander: make optional tilt-check after takeoff (failure detector). - Introduced COM_LKDOWN_TKO parameter - Introduced auto disarm for lockdown state - Do not trigger flight termiantion if system is in lockdown Signed-off-by: Claudio Micheli --- src/modules/commander/Commander.cpp | 15 ++++++++++----- src/modules/commander/Commander.hpp | 1 + src/modules/commander/commander_params.c | 16 ++++++++++++++++ 3 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0bced55a30..9cf9434757 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1627,10 +1627,16 @@ Commander::run() } // Auto disarm after 5 seconds if kill switch is engaged - _auto_disarm_killed.set_state_and_update(armed.manual_lockdown, hrt_absolute_time()); + _auto_disarm_killed.set_state_and_update(armed.manual_lockdown || armed.lockdown, hrt_absolute_time()); if (_auto_disarm_killed.get_state()) { - arm_disarm(false, true, &mavlink_log_pub, "Kill-switch still engaged, disarming"); + if (armed.manual_lockdown) { + arm_disarm(false, true, &mavlink_log_pub, "Kill-switch still engaged, disarming"); + + } else { + arm_disarm(false, true, &mavlink_log_pub, "System in lockdown, disarming"); + } + } } else { @@ -2152,10 +2158,9 @@ Commander::run() if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request"); - } - } else if (hrt_elapsed_time(&_time_at_takeoff) < 3_s) { + } else if (hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get())) { // This handles the case where something fails during the early takeoff phase if (!_lockdown_triggered) { @@ -2167,7 +2172,7 @@ Commander::run() } } else if (!status_flags.circuit_breaker_flight_termination_disabled && - !_flight_termination_triggered) { + !_flight_termination_triggered && !_lockdown_triggered) { armed.force_failsafe = true; _flight_termination_triggered = true; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 008a7d2f2d..1f5aa14841 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -210,6 +210,7 @@ private: (ParamBool) _param_com_mot_test_en, (ParamFloat) _param_com_kill_disarm, + (ParamFloat) _param_com_lkdown_tko, // Engine failure (ParamFloat) _param_ef_throttle_thres, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index fd8b4aff7a..f7b9990fe0 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -924,3 +924,19 @@ PARAM_DEFINE_FLOAT(COM_CPU_MAX, 90.0f); * @max 4 */ PARAM_DEFINE_INT32(COM_POWER_COUNT, 1); + +/** + * Timeout for detecting a failure after takeoff + * + * A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to put the vehicle into + * a lockdown state if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R. + * The check is not executed for flight modes that do support acrobatic maneuvers, e.g: Acro (MC/FW), Rattitude and Manual (FW). + * A zero or negative value means that the check is disabled. + * + * @group Commander + * @unit s + * @min -1.0 + * @max 5.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f);