From f6282f5b3d7305b84d4a3fd6355954b8bb85ff99 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 5 Dec 2016 10:09:50 +0100
Subject: [PATCH] Arm button fix: include the arm switch into the structure of
all the checks for RC arming
---
src/modules/commander/commander.cpp | 75 +++++++++++++----------------
1 file changed, 34 insertions(+), 41 deletions(-)
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);
--
GitLab