dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

State machine rewritten, compiles, WIP

This commit is contained in:
Anton Babushkin 2013-07-28 23:12:16 +04:00
parent 7c1693a877
commit 8c1067a017
5 changed files with 944 additions and 1183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

File diff suppressed because it is too large Load Diff

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

@ -62,102 +62,110 @@
#endif
static const int ERROR = -1;
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) {
int ret = ERROR;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_state->arming_state) {
ret = OK;
if (new_arming_state == status->arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_arming_state) {
case ARMING_STATE_INIT:
case ARMING_STATE_INIT:
/* allow going back from INIT for calibration */
if (current_state->arming_state == ARMING_STATE_STANDBY) {
ret = OK;
/* allow going back from INIT for calibration */
if (status->arming_state == ARMING_STATE_STANDBY) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_STANDBY:
/* allow coming from INIT and disarming from ARMED */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED) {
/* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
armed->ready_to_arm = true;
} else {
mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized");
}
break;
case ARMING_STATE_STANDBY:
}
/* allow coming from INIT and disarming from ARMED */
if (current_state->arming_state == ARMING_STATE_INIT
|| current_state->arming_state == ARMING_STATE_ARMED) {
break;
/* sensors need to be initialized for STANDBY state */
if (current_state->condition_system_sensors_initialized) {
ret = OK;
armed->armed = false;
armed->ready_to_arm = true;
} else {
mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized");
}
}
break;
case ARMING_STATE_ARMED:
case ARMING_STATE_ARMED:
/* allow arming from STANDBY and IN-AIR-RESTORE */
if (current_state->arming_state == ARMING_STATE_STANDBY
|| current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
/* allow arming from STANDBY and IN-AIR-RESTORE */
if (status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
}
/* XXX conditions for arming? */
ret = OK;
armed->armed = true;
}
break;
case ARMING_STATE_ARMED_ERROR:
break;
/* an armed error happens when ARMED obviously */
if (current_state->arming_state == ARMING_STATE_ARMED) {
/* XXX conditions for an error state? */
ret = OK;
armed->armed = true;
}
break;
case ARMING_STATE_STANDBY_ERROR:
/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
if (current_state->arming_state == ARMING_STATE_STANDBY
|| current_state->arming_state == ARMING_STATE_INIT
|| current_state->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = OK;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_REBOOT:
case ARMING_STATE_ARMED_ERROR:
/* an armed error happens when ARMED obviously */
if (current_state->arming_state == ARMING_STATE_INIT
|| current_state->arming_state == ARMING_STATE_STANDBY
|| current_state->arming_state == ARMING_STATE_STANDBY_ERROR) {
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_ARMED) {
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = false;
}
ret = OK;
armed->armed = false;
armed->ready_to_arm = false;
break;
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
case ARMING_STATE_STANDBY_ERROR:
/* XXX implement */
break;
default:
break;
/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
if (status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_REBOOT:
/* an armed error happens when ARMED obviously */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {
ret = TRANSITION_CHANGED;
armed->armed = false;
armed->ready_to_arm = false;
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
/* XXX implement */
break;
default:
break;
}
if (ret == OK) {
current_state->arming_state = new_arming_state;
current_state->counter++;
current_state->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_state);
armed->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(actuator_armed), armed_pub, armed);
if (ret == TRANSITION_CHANGED) {
hrt_abstime t = hrt_absolute_time();
status->arming_state = new_arming_state;
armed->timestamp = t;
}
}
@ -165,400 +173,230 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
}
/*
* This functions does not evaluate any input flags but only checks if the transitions
* are valid.
*/
int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) {
int ret = ERROR;
transition_result_t
main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_navigation_state == current_state->navigation_state) {
ret = OK;
if (new_main_state == current_state->main_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_navigation_state) {
case NAVIGATION_STATE_INIT:
switch (new_main_state) {
case MAIN_STATE_MANUAL:
ret = TRANSITION_CHANGED;
break;
/* transitions back to INIT are possible for calibration */
if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) {
case MAIN_STATE_SEATBELT:
ret = OK;
control_mode->flag_control_rates_enabled = false;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = false;
}
break;
/* need position estimate */
// TODO only need altitude estimate really
if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate");
tune_negative();
case NAVIGATION_STATE_MANUAL_STANDBY:
} else {
ret = TRANSITION_CHANGED;
}
/* transitions from INIT and other STANDBY states as well as MANUAL are possible */
if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
break;
/* need to be disarmed first */
if (current_state->arming_state != ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
}
}
break;
case MAIN_STATE_EASY:
case NAVIGATION_STATE_MANUAL:
/* need position estimate */
if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate");
tune_negative();
/* need to be armed first */
if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
}
break;
} else {
ret = TRANSITION_CHANGED;
}
case NAVIGATION_STATE_ASSISTED_STANDBY:
break;
/* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */
if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) {
case MAIN_STATE_AUTO:
/* need to be disarmed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed");
tune_negative();
} else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
/* need position estimate */
if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate");
tune_negative();
case NAVIGATION_STATE_ASSISTED_SEATBELT:
} else {
ret = TRANSITION_CHANGED;
}
/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/
if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_READY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* need to be armed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed");
tune_negative();
} else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
case NAVIGATION_STATE_ASSISTED_SIMPLE:
/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/
if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_READY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* need to be armed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed");
tune_negative();
} else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
case NAVIGATION_STATE_ASSISTED_DESCENT:
/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */
if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_READY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
/* need to be armed and have a position estimate */
if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed");
tune_negative();
} else if (!current_state->condition_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
}
}
break;
case NAVIGATION_STATE_AUTO_STANDBY:
/* transitions from INIT or from other STANDBY modes or from AUTO READY */
if (current_state->navigation_state == NAVIGATION_STATE_INIT
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) {
/* need to be disarmed and have a position and home lock */
if (current_state->arming_state != ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed");
tune_negative();
} else if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_READY:
/* transitions from AUTO_STANDBY or AUTO_LAND */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
// XXX flag test needed?
/* need to be armed and have a position and home lock */
if (current_state->arming_state != ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_TAKEOFF:
/* only transitions from AUTO_READY */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LOITER:
/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
/* need to have a position and home lock */
if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_MISSION:
/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
/* need to have a mission ready */
if (!current_state-> condition_auto_mission_available) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_RTL:
/* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
|| current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
/* need to have a position and home lock */
if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
case NAVIGATION_STATE_AUTO_LAND:
/* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */
if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
/* need to have a position and home lock */
if (!current_state->condition_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock");
tune_negative();
} else if (!current_state->condition_home_position_valid) {
mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos");
tune_negative();
} else {
ret = OK;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
}
break;
default:
break;
break;
}
if (ret == OK) {
current_state->navigation_state = new_navigation_state;
current_state->counter++;
current_state->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_state);
control_mode->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode);
if (ret == TRANSITION_CHANGED) {
current_state->main_state = new_main_state;
}
}
return ret;
}
transition_result_t
navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd)
{
transition_result_t ret = TRANSITION_DENIED;
/* only check transition if the new state is actually different from the current one */
if (new_navigation_state == current_status->navigation_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_navigation_state) {
case NAVIGATION_STATE_STANDBY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = false;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_DIRECT:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = false;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_STABILIZE:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = false;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_ALTHOLD:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = false;
control_mode->flag_control_position_enabled = false;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_VECTOR:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = true;
break;
case NAVIGATION_STATE_AUTO_READY:
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
break;
case NAVIGATION_STATE_AUTO_TAKEOFF:
/* only transitions from AUTO_READY */
if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LOITER:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_MISSION:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_RTL:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
case NAVIGATION_STATE_AUTO_LAND:
/* deny transitions from landed states */
if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
ret = TRANSITION_CHANGED;
control_mode->flag_control_rates_enabled = true;
control_mode->flag_control_attitude_enabled = true;
control_mode->flag_control_velocity_enabled = true;
control_mode->flag_control_position_enabled = true;
control_mode->flag_control_altitude_enabled = true;
control_mode->flag_control_manual_enabled = false;
}
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
current_status->navigation_state = new_navigation_state;
}
}
return ret;
}
@ -582,31 +420,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
switch (new_state) {
case HIL_STATE_OFF:
case HIL_STATE_OFF:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
current_control_mode->flag_system_hil_enabled = false;
mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
valid_transition = true;
}
break;
current_control_mode->flag_system_hil_enabled = false;
mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
valid_transition = true;
}
case HIL_STATE_ON:
break;
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
case HIL_STATE_ON:
current_control_mode->flag_system_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
}
break;
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
default:
warnx("Unknown hil state");
break;
current_control_mode->flag_system_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
}
break;
default:
warnx("Unknown hil state");
break;
}
}
@ -621,6 +461,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
ret = OK;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
}

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

@ -49,10 +49,18 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
typedef enum {
TRANSITION_DENIED = -1,
TRANSITION_NOT_CHANGED = 0,
TRANSITION_CHANGED
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd);
} transition_result_t;
int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd);
transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd);
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);

55
src/modules/mavlink/mavlink.c
View File

@ -196,13 +196,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
}
/* autonomous mode */
if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
}
@ -215,15 +209,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
if (v_status.navigation_state == NAVIGATION_STATE_MANUAL
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
if (v_status.main_state == MAIN_STATE_MANUAL
|| v_status.main_state == MAIN_STATE_SEATBELT
|| v_status.main_state == MAIN_STATE_EASY) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) {
if (v_status.navigation_state == MAIN_STATE_EASY) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
@ -235,41 +228,25 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
/* set calibration state */
if (v_status.preflight_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else if (v_status.system_emergency) {
*mavlink_state = MAV_STATE_EMERGENCY;
// XXX difference between active and armed? is AUTO_READY active?
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct?
|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
} else if (v_status.navigation_state == NAVIGATION_STATE_INIT) {
} else if (v_status.arming_state == ARMING_STATE_INIT
|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
*mavlink_state = MAV_STATE_UNINIT;
} else if (v_status.arming_state == ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_state = MAV_STATE_CRITICAL;
} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF;
} else {
warnx("Unknown mavlink state");
*mavlink_state = MAV_STATE_CRITICAL;
}
}

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

@ -58,29 +58,28 @@
* @addtogroup topics @{
*/
/* State Machine */
/* main state machine */
typedef enum {
NAVIGATION_STATE_INIT = 0,
NAVIGATION_STATE_MANUAL_STANDBY,
NAVIGATION_STATE_MANUAL,
NAVIGATION_STATE_ASSISTED_STANDBY,
NAVIGATION_STATE_ASSISTED_SEATBELT,
NAVIGATION_STATE_ASSISTED_SIMPLE,
NAVIGATION_STATE_ASSISTED_DESCENT,
NAVIGATION_STATE_AUTO_STANDBY,
NAVIGATION_STATE_AUTO_READY,
NAVIGATION_STATE_AUTO_TAKEOFF,
NAVIGATION_STATE_AUTO_LOITER,
NAVIGATION_STATE_AUTO_MISSION,
NAVIGATION_STATE_AUTO_RTL,
NAVIGATION_STATE_AUTO_LAND
} navigation_state_t;
MAIN_STATE_MANUAL = 0,
MAIN_STATE_SEATBELT,
MAIN_STATE_EASY,
MAIN_STATE_AUTO,
} main_state_t;
/* navigation state machine */
typedef enum {
MANUAL_STANDBY = 0,
MANUAL_READY,
MANUAL_IN_AIR
} manual_state_t;
NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed
NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization
NAVIGATION_STATE_STABILIZE, // attitude stabilization
NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization
NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization
NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff
NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode
NAVIGATION_STATE_AUTO_LOITER, // pause mission
NAVIGATION_STATE_AUTO_MISSION, // fly mission
NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND
NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector)
} navigation_state_t;
typedef enum {
ARMING_STATE_INIT = 0,
@ -103,16 +102,16 @@ typedef enum {
MODE_SWITCH_AUTO
} mode_switch_pos_t;
typedef enum {
ASSISTED_SWITCH_SEATBELT = 0,
ASSISTED_SWITCH_EASY
} assisted_switch_pos_t;
typedef enum {
RETURN_SWITCH_NONE = 0,
RETURN_SWITCH_RETURN
} return_switch_pos_t;
typedef enum {
ASSISTED_SWITCH_SEATBELT = 0,
ASSISTED_SWITCH_SIMPLE
} assisted_switch_pos_t;
typedef enum {
MISSION_SWITCH_NONE = 0,
MISSION_SWITCH_MISSION
@ -175,7 +174,8 @@ struct vehicle_status_s
uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
// uint64_t failsave_highlevel_begin; TO BE COMPLETED
navigation_state_t navigation_state; /**< current system state */
main_state_t main_state; /**< main state machine */
navigation_state_t navigation_state; /**< navigation state machine */
arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */
@ -183,6 +183,8 @@ struct vehicle_status_s
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
bool is_rotary_wing;
mode_switch_pos_t mode_switch;
return_switch_pos_t return_switch;
assisted_switch_pos_t assisted_switch;
@ -198,6 +200,7 @@ struct vehicle_status_s
bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool condition_local_position_valid;
bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception is terminally lost */