From 4b980b508c93f8bb840ababf19e61862dcedbda9 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 16 Mar 2014 21:39:06 -0700 Subject: arming_state_transition now outputs error messages If mavlink fd is passed in method will output reason for arming failure to mavlink. --- src/modules/commander/state_machine_helper.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) (limited to 'src/modules/commander/state_machine_helper.cpp') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5..94ffc31d6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -70,7 +70,7 @@ static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) { /* * Perform an atomic state update @@ -125,13 +125,18 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * case ARMING_STATE_ARMED: /* allow arming from STANDBY and IN-AIR-RESTORE */ - if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */ - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = true; - } + if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)) { + if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { + // If we need to wait for safety switch then output message, but only if we have fd for mavlink connection + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + } + } else { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = true; + } + } break; -- cgit v1.2.3