diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 377ff285e36..f541a45904c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -74,10 +74,7 @@ #include #include #include -#include -#include #include -#include #include #include @@ -140,8 +137,6 @@ static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; -static systemlib::Hysteresis auto_disarm_hysteresis(false); - static float min_stick_change = 0.25f; static struct vehicle_status_s status = {}; @@ -561,6 +556,8 @@ Commander::Commander() : ModuleParams(nullptr), _failure_detector(this) { + _auto_disarm_landed.set_hysteresis_time_from(false, 10_s); + _auto_disarm_killed.set_hysteresis_time_from(false, 5_s); _battery_sub = orb_subscribe(ORB_ID(battery_status)); } @@ -1742,26 +1739,41 @@ Commander::run() was_falling = land_detector.freefall; } - // Auto disarm when landed - if (!have_taken_off_since_arming) { - // pilot has ten seconds time to take off - auto_disarm_hysteresis.set_hysteresis_time_from(false, 10_s); + + // Auto disarm when landed or kill switch engaged + if (armed.armed) { + + // Check for auto-disarm on landing + if (_disarm_when_landed_timeout.get() > 0) { + + if (!have_taken_off_since_arming) { + // pilot has ten seconds time to take off + _auto_disarm_landed.set_hysteresis_time_from(false, 10_s); + + } else { + _auto_disarm_landed.set_hysteresis_time_from(false, _disarm_when_landed_timeout.get() * 1_s); + } + + _auto_disarm_landed.set_state_and_update(land_detector.landed); + + if (_auto_disarm_landed.get_state()) { + arm_disarm(false, &mavlink_log_pub, "auto disarm on land"); + } + } + + + // Auto disarm after 5 seconds if kill switch is engaged + _auto_disarm_killed.set_state_and_update(armed.manual_lockdown); + + if (_auto_disarm_killed.get_state()) { + arm_disarm(false, &mavlink_log_pub, "kill switch still engaged, disarming"); + } } else { - auto_disarm_hysteresis.set_hysteresis_time_from(false, _disarm_when_landed_timeout.get() * 1_s); + _auto_disarm_landed.set_state_and_update(false); + _auto_disarm_killed.set_state_and_update(false); } - // Check for auto-disarm - if (armed.armed && land_detector.landed && _disarm_when_landed_timeout.get() > FLT_EPSILON) { - auto_disarm_hysteresis.set_state_and_update(true); - - } else { - auto_disarm_hysteresis.set_state_and_update(false); - } - - if (auto_disarm_hysteresis.get_state()) { - arm_disarm(false, &mavlink_log_pub, "auto disarm on land"); - } if (!warning_action_on) { // store the last good main_state when not in an navigation diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 42edbdaafef..f0d1daed82e 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -185,6 +185,9 @@ private: void battery_status_check(); + systemlib::Hysteresis _auto_disarm_landed{false}; + systemlib::Hysteresis _auto_disarm_killed{false}; + // Subscriptions Subscription _estimator_status_sub{ORB_ID(estimator_status)}; Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; diff --git a/src/modules/sensors/rc_params.c b/src/modules/sensors/rc_params.c index d76c9d78cf1..dd6fea51ac9 100644 --- a/src/modules/sensors/rc_params.c +++ b/src/modules/sensors/rc_params.c @@ -1528,7 +1528,7 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); /** - * Kill switch channel + * Emergency Kill switch channel * * @min 0 * @max 18