From ebc7cb03b726ebfb864e770a82b92bb67b6bfd4c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:24:12 +0100 Subject: «flighttermination state» replaced by more general «failsafe state» MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/commander/commander.cpp | 39 ++-- src/modules/commander/state_machine_helper.cpp | 58 +++-- src/modules/commander/state_machine_helper.h | 4 +- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 238 ++++++++++++--------- src/modules/uORB/topics/vehicle_control_mode.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 11 +- 7 files changed, 198 insertions(+), 156 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d51bb63ff..7715f73e0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -512,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -1112,6 +1112,14 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); + } + } + /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); @@ -1135,32 +1143,23 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); - } + mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); } + // TODO add other failsafe modes if position estimate not available } } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - if (flighttermination_res == TRANSITION_CHANGED) { + // TODO remove this hack + /* flight termination in manual mode if assisted switch is on easy position */ + if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1176,12 +1175,12 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); + if (arming_state_changed || main_state_changed || failsafe_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state); status_changed = true; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ff..90ca2a6d2 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, @@ -287,10 +287,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 { @@ -361,41 +361,39 @@ 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; - - } else { + /* only check transition if the new state is actually different from the current one */ + if (new_failsafe_state == status->failsafe_state) { + ret = TRANSITION_NOT_CHANGED; - 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; + } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_RTL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_TERMINATION: + ret = TRANSITION_CHANGED; + break; - default: - break; - } + default: + break; + } - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } + if (ret == TRANSITION_CHANGED) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; } + } - return ret; + return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f3..f04879ff9 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -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); bool check_navigation_state_changed(); -bool check_flighttermination_state_changed(); +bool check_failsafe_state_changed(); void set_navigation_state_changed(); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index dc2196de6..17b1028f9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() } /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_flighttermination_enabled) { + if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cfcc886b6..89a62e166 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -643,87 +643,101 @@ Navigator::task_main() vehicle_status_update(); pub_control_mode = true; - /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { - bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - stick_mode = true; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + /* evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { + if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (_vstatus.main_state == MAIN_STATE_AUTO) { + bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + stick_mode = true; } else { - dispatch(EVENT_LOITER_REQUESTED); + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + stick_mode = true; + } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } } - stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } - } - } - - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); } else { - dispatch(EVENT_LOITER_REQUESTED); + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - break; - - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; } - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + /* not in AUTO mode */ + dispatch(EVENT_NONE_REQUESTED); } + + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { + /* RTL on failsafe */ + dispatch(EVENT_RTL_REQUESTED); + + } else { + /* shouldn't act */ + dispatch(EVENT_NONE_REQUESTED); } } else { - /* not in AUTO */ + /* not armed */ dispatch(EVENT_NONE_REQUESTED); } } @@ -1442,40 +1456,74 @@ Navigator::publish_control_mode() _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_flighttermination_enabled = false; + _control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator has control */ + bool navigator_enabled = false; + + switch (_vstatus.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } break; - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + navigator_enabled = true; + /* disable all controllers on termination */ + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_termination_enabled = true; break; - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; + default: break; + } - case MAIN_STATE_AUTO: + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ @@ -1493,10 +1541,6 @@ Navigator::publish_control_mode() _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; } - break; - - default: - break; } _control_mode.timestamp = hrt_absolute_time(); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 26dcbd985..f9f8414df 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -92,7 +92,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f5..a3a862d85 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -82,9 +82,11 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, + FAILSAFE_STATE_RTL, + FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -173,6 +175,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ + failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -223,8 +226,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** -- cgit v1.2.3