zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Arm button fix: include the arm switch into the structure of all the checks for RC arming

sbg
Matthias Grob 8 years ago committed by Lorenz Meier
parent
94c8371ffe
commit
f6282f5b3d
1 changed files with 34 additions and 41 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 59
      src/modules/commander/commander.cpp

59
src/modules/commander/commander.cpp
Unescape View File

@ -2560,8 +2560,13 @@ int commander_thread_main(int argc, char *argv[]) @@ -2560,8 +2560,13 @@ 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 ||
@ -2569,9 +2574,11 @@ int commander_thread_main(int argc, char *argv[]) @@ -2569,9 +2574,11 @@ int commander_thread_main(int argc, char *argv[])
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)) ) {
((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) {
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[]) @@ -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) {
((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[]) @@ -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[]) @@ -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);

Write Preview
Loading…
Cancel
Save