diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-15 09:52:22 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-15 09:52:22 +0200 |
commit | cc9f1e81adaa71c5f86f56df45cf14f8bc8e7abc (patch) | |
tree | 3346625caf7b752f4b13bcbffd5427f455ada9e5 /src/modules/commander/state_machine_helper.cpp | |
parent | 0bbc4b7012c72fda61dca01a897552e9483a4f5f (diff) | |
download | px4-firmware-cc9f1e81adaa71c5f86f56df45cf14f8bc8e7abc.tar.gz px4-firmware-cc9f1e81adaa71c5f86f56df45cf14f8bc8e7abc.tar.bz2 px4-firmware-cc9f1e81adaa71c5f86f56df45cf14f8bc8e7abc.zip |
Rejecting arming if safety switch is not in safe position, added reboot command
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 21 |
1 files changed, 18 insertions, 3 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 06cd060c5..163aceed2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) { transition_result_t ret = TRANSITION_DENIED; @@ -108,8 +108,10 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi 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) { + if ((status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ + { ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -172,6 +174,19 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi return ret; } +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed) +{ + // System is safe if: + // 1) Not armed + // 2) Armed, but in software lockdown (HIL) + // 3) Safety switch is present AND engaged -> actuators locked + if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + return true; + } else { + return false; + } +} + bool check_arming_state_changed() { |