diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-08 16:52:41 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-08 16:52:41 +0100 |
commit | 278e05e425f6aca75e2d6b43a17945b095176997 (patch) | |
tree | a34782d0ca8f528d589ec18b3f037c5a84242354 /src/modules/commander/state_machine_helper.cpp | |
parent | 4d846b480c9118090fe60a887fb1eb0824b38f56 (diff) | |
download | px4-firmware-278e05e425f6aca75e2d6b43a17945b095176997.tar.gz px4-firmware-278e05e425f6aca75e2d6b43a17945b095176997.tar.bz2 px4-firmware-278e05e425f6aca75e2d6b43a17945b095176997.zip |
added simple flight termination state machine which enbales parachute on request
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 51 |
1 files changed, 51 insertions, 0 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a50af7daf..6c21dfab0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -65,6 +65,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; static bool navigation_state_changed = true; +static bool flighttermination_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -451,6 +452,18 @@ check_navigation_state_changed() } } +bool +check_flighttermination_state_changed() +{ + if (flighttermination_state_changed) { + flighttermination_state_changed = false; + return true; + + } else { + return false; + } +} + void set_navigation_state_changed() { @@ -523,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s } +/** +* Transition from one flightermination state to another +*/ +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_flighttermination_state == status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_flighttermination_state) { + case FLIGHTTERMINATION_STATE_ON: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; + warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); + break; + case FLIGHTTERMINATION_STATE_OFF: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + flighttermination_state_changed = true; + control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + } + } + + return ret; +} + + // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status |