diff options
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r-- | src/modules/commander/commander.cpp | 39 |
1 files changed, 13 insertions, 26 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 25c5a84ea..1978d8782 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1398,30 +1398,18 @@ int commander_thread_main(int argc, char *argv[]) } } - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { - + /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + * TODO allow disarm when landed + */ + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && + control_mode.flag_control_manual_enabled && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) { - if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { - mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); - tune_negative(); - } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - } - - } else { - mavlink_log_critical(mavlink_fd, "DISARM not allowed"); - tune_negative(); - } + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + tune_positive(); stick_off_counter = 0; } else { @@ -1430,9 +1418,8 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position and we're in manual --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower right position and we're in manual mode --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; |