diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a4bda51d80..840d7a0ebf 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2560,18 +2560,25 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - /* check if left stick is in lower left position or arm button is pushed and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm + /* DISARM + * check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm + * and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) * do it only for rotary wings in manual mode or fixed wing if landed */ + bool arm_switch_to_disarm_transition = arm_switch_is_button == 0 && + _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON && + sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF; if ((status.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && - (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && - (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || - internal_state.main_state == commander_state_s::MAIN_STATE_ACRO || - internal_state.main_state == commander_state_s::MAIN_STATE_STAB || - internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE || - land_detector.landed) && - ((sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) || (arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) ) { - - if (stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) { + (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && + (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || + internal_state.main_state == commander_state_s::MAIN_STATE_ACRO || + internal_state.main_state == commander_state_s::MAIN_STATE_STAB || + internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE || + land_detector.landed) && + ((sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) || + (arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) || + arm_switch_to_disarm_transition) ) { + + if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR); @@ -2592,16 +2599,23 @@ int commander_thread_main(int argc, char *argv[]) } } stick_off_counter++; - - } else if (arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + /* do not reset the counter when holding the arm button longer than needed */ + } else if (!(arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) { stick_off_counter = 0; } - /* check if left stick is in lower right position or arm button is pushed and we're in MANUAL mode -> arm */ + /* ARM + * check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm + * and we're in MANUAL mode */ + bool arm_switch_to_arm_transition = arm_switch_is_button == 0 && + _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF && + sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON; if (status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && - (status.arming_state != vehicle_status_s::ARMING_STATE_ARMED && status.arming_state != vehicle_status_s::ARMING_STATE_ARMED_ERROR) && - ((sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) || (arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) ) { - if (stick_on_counter == rc_arm_hyst && stick_off_counter < rc_arm_hyst) { + (status.arming_state != vehicle_status_s::ARMING_STATE_ARMED && status.arming_state != vehicle_status_s::ARMING_STATE_ARMED_ERROR) && + ((sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) || + (arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) || + arm_switch_to_arm_transition) ) { + if ((stick_on_counter == rc_arm_hyst && stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) { /* we check outside of the transition function here because the requirement * for being in manual mode only applies to manual arming actions. @@ -2643,11 +2657,13 @@ int commander_thread_main(int argc, char *argv[]) } } stick_on_counter++; - - } else if (arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + /* do not reset the counter when holding the arm button longer than needed */ + } else if (!(arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) { stick_on_counter = 0; } + _last_sp_man_arm_switch = sp_man.arm_switch; + if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { mavlink_log_info(&mavlink_log_pub, "ARMED by RC"); @@ -2696,29 +2712,6 @@ int commander_thread_main(int argc, char *argv[]) armed.lockdown = false; } /* no else case: do not change lockdown flag in unconfigured case */ - - /* check arm switch if it is not used as button */ - if (arm_switch_is_button == 0) { - /* transition of the switch from disarm to arm */ - if (_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF && - sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "arm switch")) { - mavlink_log_info(&mavlink_log_pub, "arming by switch failed"); - } else { - arming_state_changed = true; - } - /* transition of the switch from arm to disarm */ - } else if (_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON && - sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { - if (TRANSITION_CHANGED != arm_disarm(false, &mavlink_log_pub, "arm switch")) { - mavlink_log_info(&mavlink_log_pub, "rejected disarming by switch"); - } else { - arming_state_changed = true; - } - } - /* no else case: do not change arming here if arm switch unconfigured */ - } - _last_sp_man_arm_switch = sp_man.arm_switch; } else { if (!status_flags.rc_input_blocked && !status.rc_signal_lost) { mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);