diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f6b083d30c..5611c0a94d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1612,6 +1612,8 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = false; bool failsafe_old = false; + bool have_taken_off_since_arming = false; + /* initialize low priority thread */ pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); @@ -1686,8 +1688,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_time_thres, &ef_time_thres); param_get(_param_geofence_action, &geofence_action); param_get(_param_disarm_land, &disarm_when_landed); - auto_disarm_hysteresis.set_hysteresis_time_from(false, - (hrt_abstime)disarm_when_landed * 1000000); param_get(_param_low_bat_act, &low_bat_action); param_get(_param_offboard_loss_timeout, &offboard_loss_timeout); @@ -2049,6 +2049,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_and_console_log_info(&mavlink_log_pub, "Landing detected"); } else { mavlink_and_console_log_info(&mavlink_log_pub, "Takeoff detected"); + have_taken_off_since_arming = true; } } @@ -2063,6 +2064,17 @@ int commander_thread_main(int argc, char *argv[]) was_falling = land_detector.freefall; } + + + /* Update hysteresis time. Use a time of factor 5 longer if we have not taken off yet. */ + hrt_abstime timeout_time = disarm_when_landed * 1000000; + + if (!have_taken_off_since_arming) { + timeout_time *= 5; + } + + auto_disarm_hysteresis.set_hysteresis_time_from(false, timeout_time); + // Check for auto-disarm if (armed.armed && land_detector.landed && disarm_when_landed > 0) { auto_disarm_hysteresis.set_state_and_update(true); @@ -2914,6 +2926,11 @@ int commander_thread_main(int argc, char *argv[]) status_changed = false; + if (!armed.armed) { + /* Reset the flag if disarmed. */ + have_taken_off_since_arming = false; + } + /* publish internal state for logging purposes */ if (commander_state_pub != nullptr) { diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 83d67a1264..a8f91cb91a 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -251,6 +251,10 @@ 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 be longer by a factor of 5. + * * A value of zero means that automatic disarming is disabled. * * @group Commander