diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-29 21:21:16 +0100 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-29 21:27:44 +0100 |
commit | cf6d89301b774ae019d1d2c089a30a8ed0d7308f (patch) | |
tree | 7e56739c95dedf53cfb0bae88e6ba65c88b661c8 /src/modules/commander/state_machine_helper.cpp | |
parent | 01c9092213449c761759bcda11ef9613226be713 (diff) | |
parent | 6f559b279e3d03dbf28eff436b41f3b022c5fa82 (diff) | |
download | px4-firmware-cf6d89301b774ae019d1d2c089a30a8ed0d7308f.tar.gz px4-firmware-cf6d89301b774ae019d1d2c089a30a8ed0d7308f.tar.bz2 px4-firmware-cf6d89301b774ae019d1d2c089a30a8ed0d7308f.zip |
Merge branch 'beta' into offboard2
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 140 |
1 files changed, 85 insertions, 55 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4dd63b5e1..23d3be55d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -63,7 +63,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool flighttermination_state_changed = true; +static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -220,61 +220,66 @@ check_arming_state_changed() } transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - } else { + case MAIN_STATE_SEATBELT: - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* need at minimum altitude estimate */ + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } + } - break; + break; - case MAIN_STATE_EASY: + case MAIN_STATE_EASY: - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } + /* need at minimum local position estimate */ + if (status->condition_local_position_valid || + status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } - break; + break; - case MAIN_STATE_AUTO: + case MAIN_STATE_AUTO: - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } + /* need global position estimate */ + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } - break; + break; - case MAIN_STATE_OFFBOARD: + case MAIN_STATE_OFFBOARD: + /* need global position estimate */ + if (!status->offboard_control_signal_lost) { ret = TRANSITION_CHANGED; - - break; } - if (ret == TRANSITION_CHANGED) { - current_state->main_state = new_main_state; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + if (status->main_state != new_main_state) { + status->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -294,10 +299,10 @@ check_main_state_changed() } bool -check_flighttermination_state_changed() +check_failsafe_state_changed() { - if (flighttermination_state_changed) { - flighttermination_state_changed = false; + if (failsafe_state_changed) { + failsafe_state_changed = false; return true; } else { @@ -368,28 +373,49 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** -* Transition from one flightermination state to another +* Transition from one failsafe state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) { 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->flighttermination_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { + /* transitions from TERMINATION to other states not allowed */ + if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { + ret = TRANSITION_NOT_CHANGED; + } } else { - - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* always allowed (except from TERMINATION state) */ ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); break; - case FLIGHTTERMINATION_STATE_OFF: + case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->set_nav_state = NAV_STATE_RTL; + status->set_nav_state_timestamp = hrt_absolute_time(); + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ + if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + status->set_nav_state = NAV_STATE_LAND; + status->set_nav_state_timestamp = hrt_absolute_time(); + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_TERMINATION: + /* always allowed */ ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; break; default: @@ -397,9 +423,13 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * } if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; + } } } |