diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0349882b1a1d..6b007e399b1e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -550,7 +550,7 @@ Commander::Commander() : ModuleParams(nullptr), _failure_detector(this) { - _auto_disarm_landed.set_hysteresis_time_from(false, 10_s); + _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); _auto_disarm_killed.set_hysteresis_time_from(false, 5_s); // We want to accept RC inputs as default @@ -1641,25 +1641,23 @@ Commander::run() // Auto disarm when landed or kill switch engaged if (armed.armed) { - // Check for auto-disarm on landing - if (_param_com_disarm_land.get() > 0) { + // Check for auto-disarm on landing or pre-flight + if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.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 { + if (_param_com_disarm_land.get() > 0 && have_taken_off_since_arming) { _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s); - } + _auto_disarm_landed.set_state_and_update(land_detector.landed, hrt_absolute_time()); - _auto_disarm_landed.set_state_and_update(land_detector.landed, hrt_absolute_time()); + } else if (_param_com_disarm_preflight.get() > 0 && !have_taken_off_since_arming) { + _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); + _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); + } if (_auto_disarm_landed.get_state()) { - arm_disarm(false, &mavlink_log_pub, "Auto disarm on land"); + arm_disarm(false, &mavlink_log_pub, "Auto disarm initiated"); } } - // Auto disarm after 5 seconds if kill switch is engaged _auto_disarm_killed.set_state_and_update(armed.manual_lockdown, hrt_absolute_time()); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e29f54e44ada..07ea6852e171 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -120,6 +120,7 @@ class Commander : public ModuleBase, public ModuleParams (ParamInt) _param_com_low_bat_act, (ParamFloat) _param_com_disarm_land, + (ParamFloat) _param_com_disarm_preflight, (ParamInt) _param_com_obs_avoid, (ParamInt) _param_com_oa_boot_t, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index ebe7c7446663..92228686573f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -262,18 +262,29 @@ PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000); * A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be * automatically disarmed in case a landing situation has been detected during this period. * - * The vehicle will also auto-disarm right after arming if it has not even flown, however the time - * will always be 10 seconds such that the pilot has enough time to take off. - * * A negative value means that automatic disarming triggered by landing detection is disabled. * * @group Commander - * @min -1 - * @max 20 * @unit s * @decimal 2 */ -PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f); + +PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 5.0f); + +/** + * Time-out for auto disarm before the first take off + * + * A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be + * automatically disarmed when the vehicle has never taken off. + * + * A negative value means that automatic disarming triggered by a pre-takeoff timeout is disabled. + * + * @group Commander + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 2.0f); + /** * Allow arming without GPS