fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

«flighttermination state» replaced by more general «failsafe state»

Browse Source
This commit is contained in:
Anton Babushkin 2014-01-25 23:24:12 +01:00
parent e8a1b620e9
commit ebc7cb03b7
7 changed files with 206 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/modules/commander/commander.cpp
View File

@ -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;
}

60
src/modules/commander/state_machine_helper.cpp
View File

@ -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;
/* 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;
} else {
} 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;
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;
// TODO
//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
}
default:
break;
}
return ret;
if (ret == TRANSITION_CHANGED) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
}
}
return ret;
}

4
src/modules/commander/state_machine_helper.h
View File

@ -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();

2
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -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 {

252
src/modules/navigator/navigator_main.cpp
View File

@ -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);
/* 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 {
/* 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;
}
}
}
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;
}
} 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);
}
}
}
}
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);
} 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;
}
/* not in AUTO mode */
dispatch(EVENT_NONE_REQUESTED);
}
}
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;
} else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) {
/* RTL on failsafe */
dispatch(EVENT_RTL_REQUESTED);
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;
}
} 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);
}
}
}
} 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;
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;
/* 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;
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 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_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
_control_mode.flag_control_termination_enabled = true;
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;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
default:
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 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();

2
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -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 */
};
/**

11
src/modules/uORB/topics/vehicle_status.h
View File

@ -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;
};
/**