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

cammander: state machine can now deny current state (e.g. when position lock lost during EASY mode), added FAILSAFE_STATE_LAND

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-26 11:50:34 +01:00
parent 062b64a1e2
commit c7f0553938
5 changed files with 148 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+32 -3
View File
@@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[])
/* fill current_status according to mode switches */
check_mode_switches(&sp_man, &status);
/* evaluate the main state machine */
/* evaluate the main state machine according to mode switches */
res = check_main_state_machine(&status);
if (res == TRANSITION_CHANGED) {
@@ -1160,12 +1160,41 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = true;
status_changed = true;
}
if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
if (status.main_state == MAIN_STATE_AUTO) {
/* check if AUTO mode still allowed */
transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
if (res == TRANSITION_DENIED) {
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
} else if (res == TRANSITION_DENIED) {
/* LAND mode denied, switch to failsafe state TERMINATION */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
}
}
}
} else if (armed.armed) {
/* failsafe for manual modes */
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL");
} else if (res == TRANSITION_DENIED) {
/* RTL not allowed (no global position estimate), try LAND */
res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
} else if (res == TRANSITION_DENIED) {
res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
if (res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
}
}
}
// TODO add other failsafe modes if position estimate not available
}
}
}
src/modules/commander/state_machine_helper.cpp
+54 -45
View File
@@ -223,51 +223,50 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
{
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 (!current_state->is_rotary_wing ||
(current_state->condition_local_altitude_valid ||
current_state->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;
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;
}
break;
case MAIN_STATE_AUTO:
/* need global position estimate */
if (current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
}
if (ret == TRANSITION_CHANGED) {
break;
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;
}
break;
case MAIN_STATE_AUTO:
/* need global position estimate */
if (current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
}
if (ret == TRANSITION_CHANGED) {
if (current_state->main_state != new_main_state) {
current_state->main_state = new_main_state;
main_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
@@ -367,17 +366,22 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
{
transition_result_t ret = TRANSITION_DENIED;
/* 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;
/* 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 if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) {
} else {
switch (new_failsafe_state) {
case FAILSAFE_STATE_NORMAL:
ret = TRANSITION_CHANGED;
break;
case FAILSAFE_STATE_RTL:
ret = TRANSITION_CHANGED;
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case FAILSAFE_STATE_TERMINATION:
ret = TRANSITION_CHANGED;
@@ -388,8 +392,13 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
}
if (ret == TRANSITION_CHANGED) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
if (status->failsafe_state != new_failsafe_state) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
}
src/modules/navigator/navigator_main.cpp
+57 -1
View File
@@ -216,6 +216,7 @@ private:
EVENT_LOITER_REQUESTED,
EVENT_MISSION_REQUESTED,
EVENT_RTL_REQUESTED,
EVENT_LAND_REQUESTED,
EVENT_MISSION_CHANGED,
EVENT_HOME_POSITION_CHANGED,
MAX_EVENT
@@ -292,7 +293,7 @@ private:
void start_loiter();
void start_mission();
void start_rtl();
void finish_rtl();
void start_land();
/**
* Guards offboard mission
@@ -733,6 +734,12 @@ Navigator::task_main()
dispatch(EVENT_RTL_REQUESTED);
}
} else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) {
/* LAND on failsafe */
if (myState != NAV_STATE_READY) {
dispatch(EVENT_LAND_REQUESTED);
}
} else {
/* shouldn't act */
dispatch(EVENT_NONE_REQUESTED);
@@ -892,6 +899,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
},
@@ -902,6 +910,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY},
},
@@ -912,6 +921,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
},
@@ -922,6 +932,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION},
},
@@ -932,9 +943,21 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state
},
{
/* STATE_LAND */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
},
};
void
@@ -1142,6 +1165,27 @@ Navigator::start_rtl()
set_rtl_item();
}
void
Navigator::start_land()
{
_do_takeoff = false;
_reset_loiter_pos = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.current.type = SETPOINT_TYPE_LAND;
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.alt = _global_pos.alt;
_pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.yaw = NAN;
mavlink_log_info(_mavlink_fd, "[navigator] LAND started");
}
void
Navigator::set_rtl_item()
{
@@ -1508,9 +1552,21 @@ Navigator::publish_control_mode()
navigator_enabled = true;
break;
case FAILSAFE_STATE_LAND:
/* land with or without position control */
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
break;
case FAILSAFE_STATE_TERMINATION:
navigator_enabled = true;
/* disable all controllers on termination */
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = false;
_control_mode.flag_control_attitude_enabled = false;
_control_mode.flag_control_position_enabled = false;
src/modules/uORB/topics/vehicle_control_mode.h
+1
View File
@@ -67,6 +67,7 @@ typedef enum {
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_RTL,
NAV_STATE_LAND,
NAV_STATE_MAX
} nav_state_t;
src/modules/uORB/topics/vehicle_status.h
+4 -3
View File
@@ -84,9 +84,10 @@ typedef enum {
} hil_state_t;
typedef enum {
FAILSAFE_STATE_NORMAL = 0,
FAILSAFE_STATE_RTL,
FAILSAFE_STATE_TERMINATION,
FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */
FAILSAFE_STATE_RTL, /**< Return To Launch */
FAILSAFE_STATE_LAND, /**< Land without position control */
FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */
FAILSAFE_STATE_MAX
} failsafe_state_t;