fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-06 18:10:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+19 -20
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;
}
src/modules/commander/state_machine_helper.cpp
+29 -31
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;
}
src/modules/commander/state_machine_helper.h
+2 -2
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();