diff options
author | Don Gagne <don@thegagnes.com> | 2014-03-16 21:39:06 -0700 |
---|---|---|
committer | Don Gagne <don@thegagnes.com> | 2014-03-16 21:39:06 -0700 |
commit | 4b980b508c93f8bb840ababf19e61862dcedbda9 (patch) | |
tree | bbdb26e82c61af5e959639a8d68ea5339b00d4ac /src | |
parent | dec13e7f215caa3ee002fbc3b6400ae82e249b2e (diff) | |
download | px4-firmware-4b980b508c93f8bb840ababf19e61862dcedbda9.tar.gz px4-firmware-4b980b508c93f8bb840ababf19e61862dcedbda9.tar.bz2 px4-firmware-4b980b508c93f8bb840ababf19e61862dcedbda9.zip |
arming_state_transition now outputs error messages
If mavlink fd is passed in method will output reason for arming failure
to mavlink.
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 21 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.h | 2 |
2 files changed, 14 insertions, 9 deletions
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; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f04879ff9..0ddd4f05a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, 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 = 0); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); |