From 826d5687be209bc5e42ed97b8a84493913123c2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 14:43:12 +0200 Subject: Fixed in-air restart --- src/modules/commander/commander.cpp | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1e86e8e24..fd9067e90 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -470,8 +470,26 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: - // XXX implement later - result = VEHICLE_CMD_RESULT_DENIED; + { + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + } break; default: -- cgit v1.2.3