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

Checkpoint: New hierarchical states

This commit is contained in:
Julian Oes 2013-02-20 10:38:06 -08:00
parent aab6214cdc
commit 0e29f2505a
5 changed files with 392 additions and 496 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Documentation/flight_mode_state_machine.odg
View File

Binary file not shown.

117
apps/commander/commander.c
View File

@ -1785,103 +1785,90 @@ int commander_thread_main(int argc, char *argv[])
if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
/*
* Check if manual control modes have to be switched
*/
if (!isfinite(sp_man.mode_switch)) {
warnx("mode sw not finite");
/*
* Check if manual control modes have to be switched
*/
if (!isfinite(sp_man.mode_switch)) {
warnx("mode sw not finite");
/* this switch is not properly mapped, set attitude stabilized */
if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
/* no valid stick position, go to default */
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else {
/* assuming a fixed wing, fall back to direct pass-through */
current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false;
}
} else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
} else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, go to manual mode */
current_status.mode_switch = MODE_SWITCH_MANUAL;
/* this switch is not properly mapped, set attitude stabilized */
if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
} else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
/* assuming a rotary wing, fall back to m */
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else {
/* top stick position, set auto/mission for all vehicle types */
current_status.mode_switch = MODE_SWITCH_AUTO;
/* assuming a fixed wing, set to direct pass-through as requested */
current_status.flag_control_attitude_enabled = false;
current_status.flag_control_rates_enabled = false;
}
} else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
/* top stick position, set auto/mission for all vehicle types */
current_status.flag_control_position_enabled = true;
current_status.flag_control_velocity_enabled = true;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else {
/* center stick position, set seatbelt/simple control */
current_status.flag_control_velocity_enabled = true;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
}
} else {
/* center stick position, set seatbelt/simple control */
current_status.mode_switch = MODE_SWITCH_SEATBELT;
}
// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
/*
* Check if land/RTL is requested
*/
* Check if land/RTL is requested
*/
if (!isfinite(sp_man.return_switch)) {
/* this switch is not properly mapped, set default */
current_status.flag_land_requested = false; // XXX default?
current_status.return_switch = RETURN_SWITCH_NONE;
} else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set altitude hold */
current_status.flag_land_requested = false;
current_status.return_switch = RETURN_SWITCH_NONE;
} else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) {
/* top stick position */
current_status.flag_land_requested = true;
current_status.return_switch = RETURN_SWITCH_RETURN;
} else {
/* center stick position, set default */
current_status.flag_land_requested = false; // XXX default?
current_status.return_switch = RETURN_SWITCH_NONE;
}
/* check mission switch */
if (!isfinite(sp_man.mission_switch)) {
/* this switch is not properly mapped, set default */
current_status.flag_mission_activated = false;
current_status.mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) {
/* top switch position */
current_status.flag_mission_activated = true;
current_status.mission_switch = MISSION_SWITCH_MISSION;
} else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) {
/* bottom switch position */
current_status.flag_mission_activated = false;
current_status.mission_switch = MISSION_SWITCH_NONE;
} else {
/* center switch position, set default */
current_status.flag_mission_activated = false; // XXX default?
current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default?
}
/* Now it's time to handle the stick inputs */
if (current_status.arming_state == ARMING_STATE_ARMED) {
if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_MANUAL );
} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_SEATBELT );
} else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) {
do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_MISSION );
}
}
}
/* handle the case where RC signal was regained */
@ -1890,17 +1877,19 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
} else {
if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
if (current_status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
}
}
/*
* Check if left stick is in lower left position --> switch to standby state.
* Do this only for multirotors, not for fixed wing aircraft.
*/
// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
// ) &&
* Check if left stick is in lower left position --> switch to standby state.
* Do this only for multirotors, not for fixed wing aircraft.
*/
// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
// ) &&
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
@ -1969,8 +1958,8 @@ int commander_thread_main(int argc, char *argv[])
/* decide about attitude control flag, enable in att/pos/vel */
bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
/* decide about rate control flag, enable it always XXX (for now) */
bool rates_ctrl_enabled = true;

678
apps/commander/state_machine_helper.c
View File

@ -54,421 +54,329 @@
/**
* Transition from one navigation state to another
* Transition from one sytem state to another
*/
int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state)
void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
bool valid_path = false;
bool valid_transition = false;
int ret = ERROR;
system_state_t new_system_state;
/*
* Firstly evaluate mode switch position
* */
/* Always accept manual mode */
if (current_status->mode_switch == MODE_SWITCH_MANUAL) {
new_system_state = SYSTEM_STATE_MANUAL;
/* Accept SEATBELT if there is a local position estimate */
} else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) {
if (current_status->flag_local_position_valid) {
new_system_state = SYSTEM_STATE_SEATBELT;
} else {
/* or just fall back to manual */
mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
new_system_state = SYSTEM_STATE_MANUAL);
}
/* Accept AUTO if there is a global position estimate */
} else if (current_status->mode_switch == MODE_SWITCH_AUTO) {
if (current_status->flag_local_position_valid) {
new_system_state = SYSTEM_STATE_SEATBELT);
} else {
mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position");
/* otherwise fallback to seatbelt or even manual */
if (current_status->flag_local_position_valid) {
new_system_state = SYSTEM_STATE_SEATBELT;
} else {
mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
new_system_state = SYSTEM_STATE_MANUAL;
}
}
}
/*
* Next up, check for
*/
/* Update the system state in case it has changed */
if (current_status->system_state != new_system_state) {
/* Check if the transition is valid */
if (system_state_update(current_status->system_state, new_system_state) == OK) {
current_status->system_state = new_system_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
} else {
mavlink_log_critical(mavlink_fd, "System state transition not valid");
}
}
return;
}
/*
* This functions does not evaluate any input flags but only checks if the transitions
* are valid.
*/
int system_state_update(system_state_t current_system_state, system_state_t new_system_state) {
int ret = ERROR;
if (current_status->navigation_state == new_state) {
warnx("Navigation state not changed");
/* only check transition if the new state is actually different from the current one */
if (new_system_state != current_system_state) {
} else {
switch (new_system_state) {
case SYSTEM_STATE_INIT:
switch (new_state) {
case NAVIGATION_STATE_STANDBY:
/* transitions back to INIT are possible for calibration */
if (current_system_state == SYSTEM_STATE_MANUAL
|| current_system_state == SYSTEM_STATE_SEATBELT
|| current_system_state == SYSTEM_STATE_AUTO) {
if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
ret = OK;
}
if (!current_status->flag_system_armed) {
mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed");
}
valid_path = true;
break;
case SYSTEM_STATE_MANUAL:
/* transitions from INIT or from other modes */
if (current_system_state == SYSTEM_STATE_INIT
|| current_system_state == SYSTEM_STATE_SEATBELT
|| current_system_state == SYSTEM_STATE_AUTO) {
ret = OK;
}
break;
case SYSTEM_STATE_SEATBELT:
/* transitions from INIT or from other modes */
if (current_system_state == SYSTEM_STATE_INIT
|| current_system_state == SYSTEM_STATE_MANUAL
|| current_system_state == SYSTEM_STATE_AUTO) {
ret = OK;
}
break;
case SYSTEM_STATE_AUTO:
/* transitions from INIT or from other modes */
if (current_system_state == SYSTEM_STATE_INIT
|| current_system_state == SYSTEM_STATE_MANUAL
|| current_system_state == SYSTEM_STATE_SEATBELT) {
ret = OK;
}
break;
case NAVIGATION_STATE_MANUAL:
case SYSTEM_STATE_REBOOT:
if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
/* from INIT you can go straight to REBOOT */
if (current_system_state == SYSTEM_STATE_INIT) {
/* only check for armed flag when coming from STANDBY XXX does that make sense? */
if (current_status->flag_system_armed) {
mavlink_log_critical(mavlink_fd, "Switched to MANUAL state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed");
}
valid_path = true;
} else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT
|| current_status->navigation_state == NAVIGATION_STATE_LOITER
|| current_status->navigation_state == NAVIGATION_STATE_MISSION
|| current_status->navigation_state == NAVIGATION_STATE_RTL
|| current_status->navigation_state == NAVIGATION_STATE_LAND
|| current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
|| current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
mavlink_log_critical(mavlink_fd, "Switched to MANUAL state");
valid_transition = true;
valid_path = true;
ret = OK;
}
break;
case NAVIGATION_STATE_SEATBELT:
case SYSTEM_STATE_IN_AIR_RESTORE:
if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_LOITER
|| current_status->navigation_state == NAVIGATION_STATE_MISSION) {
/* from INIT you can go straight to an IN AIR RESTORE */
if (current_system_state == SYSTEM_STATE_INIT) {
mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state");
valid_transition = true;
valid_path = true;
}
break;
case NAVIGATION_STATE_DESCENT:
if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
mavlink_log_critical(mavlink_fd, "Switched to DESCENT state");
valid_transition = true;
valid_path = true;
}
break;
case NAVIGATION_STATE_LOITER:
/* Check for position lock when coming from MANUAL or SEATBELT */
if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
valid_transition = true;
if (current_status->flag_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock");
}
valid_path = true;
} else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) {
mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
valid_transition = true;
valid_path = true;
}
break;
case NAVIGATION_STATE_AUTO_READY:
/* coming from STANDBY pos and home lock are needed */
if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
if (!current_status->flag_system_armed) {
mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed");
} else if (!current_status->flag_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock");
} else if (!current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock");
} else {
mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state");
valid_transition = true;
}
valid_path = true;
/* coming from LAND home lock is needed */
} else if (current_status->navigation_state == NAVIGATION_STATE_LAND) {
if (!current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock");
} else {
mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state");
valid_transition = true;
}
valid_path = true;
}
break;
case NAVIGATION_STATE_MISSION:
/* coming from TAKEOFF or RTL is always possible */
if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
|| current_status->navigation_state == NAVIGATION_STATE_RTL) {
mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
valid_transition = true;
valid_path = true;
/* coming from MANUAL or SEATBELT requires home and pos lock */
} else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
if (!current_status->flag_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock");
} else if (!current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock");
mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
valid_transition = true;
} else {
}
valid_path = true;
/* coming from loiter a home lock is needed */
} else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) {
if (current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock");
}
valid_path = true;
}
break;
case NAVIGATION_STATE_RTL:
/* coming from MISSION is always possible */
if (current_status->navigation_state == NAVIGATION_STATE_MISSION) {
mavlink_log_critical(mavlink_fd, "Switched to RTL state");
valid_transition = true;
valid_path = true;
/* coming from MANUAL or SEATBELT requires home and pos lock */
} else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
if (!current_status->flag_global_position_valid) {
mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock");
} else if (!current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock");
} else {
mavlink_log_critical(mavlink_fd, "Switched to RTL state");
valid_transition = true;
}
valid_path = true;
/* coming from loiter a home lock is needed */
} else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) {
if (current_status->flag_valid_launch_position) {
mavlink_log_critical(mavlink_fd, "Switched to RTL state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock");
}
valid_path = true;
}
break;
case NAVIGATION_STATE_TAKEOFF:
if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
/* TAKEOFF is straight forward from AUTO READY */
mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state");
valid_transition = true;
valid_path = true;
}
break;
case NAVIGATION_STATE_LAND:
if (current_status->navigation_state == NAVIGATION_STATE_RTL
|| current_status->navigation_state == NAVIGATION_STATE_LOITER
|| current_status->navigation_state == NAVIGATION_STATE_MISSION) {
mavlink_log_critical(mavlink_fd, "Switched to LAND state");
valid_transition = true;
valid_path = true;
ret = OK;
}
break;
default:
warnx("Unknown navigation state");
break;
}
}
if (valid_transition) {
current_status->navigation_state = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
// publish_armed_status(current_status);
ret = OK;
}
if (!valid_path){
mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition");
}
return ret;
}
/**
* Transition from one arming state to another
*/
int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state)
{
bool valid_transition = false;
int ret = ERROR;
///**
// * Transition from one arming state to another
// */
//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state)
//{
// bool valid_transition = false;
// int ret = ERROR;
//
// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state);
//
// if (current_status->arming_state == new_state) {
// warnx("Arming state not changed");
// valid_transition = true;
//
// } else {
//
// switch (new_state) {
//
// case ARMING_STATE_INIT:
//
// if (current_status->arming_state == ARMING_STATE_STANDBY) {
//
// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE");
// valid_transition = true;
// }
// break;
//
// case ARMING_STATE_STANDBY:
//
// // TODO check for sensors
// // XXX check if coming from reboot?
// if (current_status->arming_state == ARMING_STATE_INIT) {
//
// if (current_status->flag_system_sensors_initialized) {
// current_status->flag_system_armed = false;
// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
// valid_transition = true;
// } else {
// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init.");
// }
//
// } else if (current_status->arming_state == ARMING_STATE_ARMED) {
//
// current_status->flag_system_armed = false;
// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
// valid_transition = true;
// }
// break;
//
// case ARMING_STATE_ARMED:
//
// if (current_status->arming_state == ARMING_STATE_STANDBY
// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
// current_status->flag_system_armed = true;
// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state");
// valid_transition = true;
// }
// break;
//
// case ARMING_STATE_ABORT:
//
// if (current_status->arming_state == ARMING_STATE_ARMED) {
//
// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state");
// valid_transition = true;
// }
// break;
//
// case ARMING_STATE_ERROR:
//
// if (current_status->arming_state == ARMING_STATE_ARMED
// || current_status->arming_state == ARMING_STATE_ABORT
// || current_status->arming_state == ARMING_STATE_INIT) {
//
// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state");
// valid_transition = true;
// }
// break;
//
// case ARMING_STATE_REBOOT:
//
// if (current_status->arming_state == ARMING_STATE_STANDBY
// || current_status->arming_state == ARMING_STATE_ERROR) {
//
// valid_transition = true;
// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
// usleep(500000);
// up_systemreset();
// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
// }
// break;
//
// case ARMING_STATE_IN_AIR_RESTORE:
//
// if (current_status->arming_state == ARMING_STATE_INIT) {
// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state");
// valid_transition = true;
// }
// break;
// default:
// warnx("Unknown arming state");
// break;
// }
// }
//
// if (valid_transition) {
// current_status->arming_state = new_state;
// state_machine_publish(status_pub, current_status, mavlink_fd);
//// publish_armed_status(current_status);
// ret = OK;
// } else {
// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition");
// }
//
// return ret;
//}
warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state);
if (current_status->arming_state == new_state) {
warnx("Arming state not changed");
valid_transition = true;
} else {
switch (new_state) {
case ARMING_STATE_INIT:
if (current_status->arming_state == ARMING_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE");
valid_transition = true;
}
break;
case ARMING_STATE_STANDBY:
// TODO check for sensors
// XXX check if coming from reboot?
if (current_status->arming_state == ARMING_STATE_INIT) {
if (current_status->flag_system_sensors_initialized) {
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
valid_transition = true;
} else {
mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init.");
}
} else if (current_status->arming_state == ARMING_STATE_ARMED) {
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
valid_transition = true;
}
break;
case ARMING_STATE_ARMED:
if (current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state");
valid_transition = true;
}
break;
case ARMING_STATE_ABORT:
if (current_status->arming_state == ARMING_STATE_ARMED) {
mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state");
valid_transition = true;
}
break;
case ARMING_STATE_ERROR:
if (current_status->arming_state == ARMING_STATE_ARMED
|| current_status->arming_state == ARMING_STATE_ABORT
|| current_status->arming_state == ARMING_STATE_INIT) {
mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state");
valid_transition = true;
}
break;
case ARMING_STATE_REBOOT:
if (current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_ERROR) {
valid_transition = true;
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
usleep(500000);
up_systemreset();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
if (current_status->arming_state == ARMING_STATE_INIT) {
mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state");
valid_transition = true;
}
break;
default:
warnx("Unknown arming state");
break;
}
}
if (valid_transition) {
current_status->arming_state = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
// publish_armed_status(current_status);
ret = OK;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition");
}
return ret;
}
/**
* Transition from one hil state to another
*/
int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state)
{
bool valid_transition = false;
int ret = ERROR;
warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
if (current_status->hil_state == new_state) {
warnx("Hil state not changed");
valid_transition = true;
} else {
switch (new_state) {
case HIL_STATE_OFF:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
current_status->flag_hil_enabled = false;
mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
valid_transition = true;
}
break;
case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
current_status->flag_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
}
break;
default:
warnx("Unknown hil state");
break;
}
}
if (valid_transition) {
current_status->hil_state = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
ret = OK;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
}
return ret;
}
///**
// * Transition from one hil state to another
// */
//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state)
//{
// bool valid_transition = false;
// int ret = ERROR;
//
// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
//
// if (current_status->hil_state == new_state) {
// warnx("Hil state not changed");
// valid_transition = true;
//
// } else {
//
// switch (new_state) {
//
// case HIL_STATE_OFF:
//
// if (current_status->arming_state == ARMING_STATE_INIT
// || current_status->arming_state == ARMING_STATE_STANDBY) {
//
// current_status->flag_hil_enabled = false;
// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
// valid_transition = true;
// }
// break;
//
// case HIL_STATE_ON:
//
// if (current_status->arming_state == ARMING_STATE_INIT
// || current_status->arming_state == ARMING_STATE_STANDBY) {
//
// current_status->flag_hil_enabled = true;
// mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
// valid_transition = true;
// }
// break;
//
// default:
// warnx("Unknown hil state");
// break;
// }
// }
//
// if (valid_transition) {
// current_status->hil_state = new_state;
// state_machine_publish(status_pub, current_status, mavlink_fd);
// ret = OK;
// } else {
// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
// }
//
// return ret;
//}

4
apps/commander/state_machine_helper.h
View File

@ -47,9 +47,9 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);

89
apps/uORB/topics/vehicle_status.h
View File

@ -60,27 +60,48 @@
/* State Machine */
typedef enum {
NAVIGATION_STATE_STANDBY=0,
NAVIGATION_STATE_MANUAL,
NAVIGATION_STATE_SEATBELT,
NAVIGATION_STATE_DESCENT,
NAVIGATION_STATE_LOITER,
NAVIGATION_STATE_AUTO_READY,
NAVIGATION_STATE_MISSION,
NAVIGATION_STATE_RTL,
NAVIGATION_STATE_LAND,
NAVIGATION_STATE_TAKEOFF
} navigation_state_t;
SYSTEM_STATE_INIT=0,
SYSTEM_STATE_MANUAL,
SYSTEM_STATE_SEATBELT,
SYSTEM_STATE_AUTO,
SYSTEM_STATE_REBOOT,
SYSTEM_STATE_IN_AIR_RESTORE
} system_state_t;
typedef enum {
ARMING_STATE_INIT = 0,
ARMING_STATE_STANDBY,
ARMING_STATE_ARMED,
ARMING_STATE_ABORT,
ARMING_STATE_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE
} arming_state_t;
MANUAL_STANDBY = 0,
MANUAL_READY,
MANUAL_IN_AIR
} manual_state_t;
typedef enum {
SEATBELT_STANDBY,
SEATBELT_READY,
SEATBELT,
SEATBELT_ASCENT,
SEATBELT_DESCENT,
} seatbelt_state_t;
typedef enum {
AUTO_STANDBY,
AUTO_READY,
AUTO_LOITER,
AUTO_MISSION,
AUTO_RTL,
AUTO_TAKEOFF,
AUTO_LAND,
} auto_state_t;
//typedef enum {
// ARMING_STATE_INIT = 0,
// ARMING_STATE_STANDBY,
// ARMING_STATE_ARMED_GROUND,
// ARMING_STATE_ARMED_AIRBORNE,
// ARMING_STATE_ERROR_GROUND,
// ARMING_STATE_ERROR_AIRBORNE,
// ARMING_STATE_REBOOT,
// ARMING_STATE_IN_AIR_RESTORE
//} arming_state_t;
typedef enum {
HIL_STATE_OFF = 0,
@ -100,7 +121,7 @@ enum VEHICLE_MODE_FLAG {
typedef enum {
MODE_SWITCH_MANUAL = 0,
MODE_SWITCH_ASSISTED,
MODE_SWITCH_SEATBELT,
MODE_SWITCH_AUTO
} mode_switch_pos_t;
@ -114,26 +135,6 @@ typedef enum {
MISSION_SWITCH_MISSION
} mission_switch_pos_t;
//enum VEHICLE_FLIGHT_MODE {
// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */
// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */
// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */
//};
//
//enum VEHICLE_MANUAL_CONTROL_MODE {
// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */
// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */
// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */
//};
//
//enum VEHICLE_MANUAL_SAS_MODE {
// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */
// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */
// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */
// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */
//};
/**
* Should match 1:1 MAVLink's MAV_TYPE ENUM
*/
@ -180,8 +181,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 navigation state */
arming_state_t arming_state; /**< current arming state */
system_state_t system_state; /**< current system state */
// arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
@ -194,6 +195,7 @@ struct vehicle_status_s
return_switch_pos_t return_switch;
mission_switch_pos_t mission_switch;
bool flag_system_armed; /**< true is motors / actuators are armed */
bool flag_system_emergency;
bool flag_system_in_air_restore; /**< true if we can restore in mid air */
@ -212,9 +214,6 @@ struct vehicle_status_s
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_land_requested; /**< true if land requested */
bool flag_mission_activated; /**< true if in mission mode */
bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
bool flag_preflight_accel_calibration;