zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Checkpoint: New hierarchical states

sbg
Julian Oes 12 years ago
parent
aab6214cdc
commit
0e29f2505a
5 changed files with 367 additions and 471 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. BIN
      Documentation/flight_mode_state_machine.odg
  2. 73
      apps/commander/commander.c
  3. 592
      apps/commander/state_machine_helper.c
  4. 4
      apps/commander/state_machine_helper.h
  5. 89
      apps/uORB/topics/vehicle_status.h

BIN
Documentation/flight_mode_state_machine.odg
View File

Binary file not shown.

73
apps/commander/commander.c
Unescape View File

@ -1791,50 +1791,22 @@ int commander_thread_main(int argc, char *argv[])
if (!isfinite(sp_man.mode_switch)) { if (!isfinite(sp_man.mode_switch)) {
warnx("mode sw not finite"); warnx("mode sw not finite");
/* this switch is not properly mapped, set attitude stabilized */ /* no valid stick position, go to default */
if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
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) {
/* this switch is not properly mapped, set attitude stabilized */ /* bottom stick position, go to manual mode */
if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || current_status.mode_switch = MODE_SWITCH_MANUAL;
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
/* assuming a rotary wing, fall back to m */
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else {
/* 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) { } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
/* top stick position, set auto/mission for all vehicle types */ /* top stick position, set auto/mission for all vehicle types */
current_status.flag_control_position_enabled = true; current_status.mode_switch = MODE_SWITCH_AUTO;
current_status.flag_control_velocity_enabled = true;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else { } else {
/* center stick position, set seatbelt/simple control */ /* center stick position, set seatbelt/simple control */
current_status.flag_control_velocity_enabled = true; current_status.mode_switch = MODE_SWITCH_SEATBELT;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} }
// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
@ -1845,43 +1817,58 @@ int commander_thread_main(int argc, char *argv[])
if (!isfinite(sp_man.return_switch)) { if (!isfinite(sp_man.return_switch)) {
/* this switch is not properly mapped, set default */ /* 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) { } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set altitude hold */ /* 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) { } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) {
/* top stick position */ /* top stick position */
current_status.flag_land_requested = true; current_status.return_switch = RETURN_SWITCH_RETURN;
} else { } else {
/* center stick position, set default */ /* center stick position, set default */
current_status.flag_land_requested = false; // XXX default? current_status.return_switch = RETURN_SWITCH_NONE;
} }
/* check mission switch */ /* check mission switch */
if (!isfinite(sp_man.mission_switch)) { if (!isfinite(sp_man.mission_switch)) {
/* this switch is not properly mapped, set default */ /* 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) { } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) {
/* top switch position */ /* 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) { } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) {
/* bottom switch position */ /* bottom switch position */
current_status.flag_mission_activated = false; current_status.mission_switch = MISSION_SWITCH_NONE;
} else { } else {
/* center switch position, set default */ /* 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 */ /* handle the case where RC signal was regained */
@ -1890,7 +1877,9 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
} else { } 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!");
}
} }
/* /*

592
apps/commander/state_machine_helper.c
Unescape View File

@ -54,422 +54,330 @@
/** /**
* 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; int ret = ERROR;
system_state_t new_system_state;
if (current_status->navigation_state == new_state) {
warnx("Navigation state not changed");
} else { /*
* Firstly evaluate mode switch position
* */
switch (new_state) { /* Always accept manual mode */
case NAVIGATION_STATE_STANDBY: if (current_status->mode_switch == MODE_SWITCH_MANUAL) {
new_system_state = SYSTEM_STATE_MANUAL;
if (current_status->navigation_state == NAVIGATION_STATE_MANUAL /* Accept SEATBELT if there is a local position estimate */
|| current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) {
if (!current_status->flag_system_armed) { if (current_status->flag_local_position_valid) {
mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); new_system_state = SYSTEM_STATE_SEATBELT;
valid_transition = true;
} else { } else {
mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed"); /* or just fall back to manual */
mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
new_system_state = SYSTEM_STATE_MANUAL);
} }
valid_path = true;
}
break;
case NAVIGATION_STATE_MANUAL: /* Accept AUTO if there is a global position estimate */
} else if (current_status->mode_switch == MODE_SWITCH_AUTO) {
if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { if (current_status->flag_local_position_valid) {
new_system_state = SYSTEM_STATE_SEATBELT);
/* 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 { } else {
mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed"); mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position");
}
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;
}
break;
case NAVIGATION_STATE_SEATBELT:
if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_LOITER
|| current_status->navigation_state == NAVIGATION_STATE_MISSION) {
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");
/* otherwise fallback to seatbelt or even manual */
if (current_status->flag_local_position_valid) {
new_system_state = SYSTEM_STATE_SEATBELT;
} else { } else {
mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
valid_transition = true; new_system_state = SYSTEM_STATE_MANUAL;
} }
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 * Next up, check for
|| 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 {
} /* Update the system state in case it has changed */
valid_path = true; if (current_status->system_state != new_system_state) {
/* coming from loiter a home lock is needed */ /* Check if the transition is valid */
} else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { if (system_state_update(current_status->system_state, new_system_state) == OK) {
if (current_status->flag_valid_launch_position) { current_status->system_state = new_system_state;
mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); state_machine_publish(status_pub, current_status, mavlink_fd);
valid_transition = true;
} else { } else {
mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); mavlink_log_critical(mavlink_fd, "System state transition not valid");
} }
valid_path = true;
} }
break;
case NAVIGATION_STATE_RTL:
/* coming from MISSION is always possible */ return;
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) { /*
* 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) {
/* TAKEOFF is straight forward from AUTO READY */ int ret = ERROR;
mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state");
valid_transition = true;
valid_path = true;
}
break;
case NAVIGATION_STATE_LAND: /* only check transition if the new state is actually different from the current one */
if (current_status->navigation_state == NAVIGATION_STATE_RTL if (new_system_state != current_system_state) {
|| current_status->navigation_state == NAVIGATION_STATE_LOITER
|| current_status->navigation_state == NAVIGATION_STATE_MISSION) {
mavlink_log_critical(mavlink_fd, "Switched to LAND state"); switch (new_system_state) {
valid_transition = true; case SYSTEM_STATE_INIT:
valid_path = true;
}
break;
default: /* transitions back to INIT are possible for calibration */
warnx("Unknown navigation state"); if (current_system_state == SYSTEM_STATE_MANUAL
break; || current_system_state == SYSTEM_STATE_SEATBELT
} || current_system_state == SYSTEM_STATE_AUTO) {
}
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; 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;
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; break;
case ARMING_STATE_STANDBY: case SYSTEM_STATE_MANUAL:
// 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) { /* 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) {
current_status->flag_system_armed = false; ret = OK;
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; break;
case ARMING_STATE_ABORT: case SYSTEM_STATE_SEATBELT:
if (current_status->arming_state == ARMING_STATE_ARMED) { /* 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) {
mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); ret = OK;
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; break;
case ARMING_STATE_REBOOT: case SYSTEM_STATE_AUTO:
if (current_status->arming_state == ARMING_STATE_STANDBY /* transitions from INIT or from other modes */
|| current_status->arming_state == ARMING_STATE_ERROR) { if (current_system_state == SYSTEM_STATE_INIT
|| current_system_state == SYSTEM_STATE_MANUAL
|| current_system_state == SYSTEM_STATE_SEATBELT) {
valid_transition = true; ret = OK;
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
usleep(500000);
up_systemreset();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} }
break; break;
case ARMING_STATE_IN_AIR_RESTORE: case SYSTEM_STATE_REBOOT:
if (current_status->arming_state == ARMING_STATE_INIT) { /* from INIT you can go straight to REBOOT */
mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); if (current_system_state == SYSTEM_STATE_INIT) {
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; 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; break;
case HIL_STATE_ON: case SYSTEM_STATE_IN_AIR_RESTORE:
if (current_status->arming_state == ARMING_STATE_INIT /* from INIT you can go straight to an IN AIR RESTORE */
|| current_status->arming_state == ARMING_STATE_STANDBY) { if (current_system_state == SYSTEM_STATE_INIT) {
current_status->flag_hil_enabled = true; ret = OK;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
} }
break; break;
default: default:
warnx("Unknown hil state");
break; 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; 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;
//
// 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;
//}
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)

4
apps/commander/state_machine_helper.h
Unescape View File

@ -47,9 +47,9 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.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); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);

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

@ -60,27 +60,48 @@
/* State Machine */ /* State Machine */
typedef enum { typedef enum {
NAVIGATION_STATE_STANDBY=0, SYSTEM_STATE_INIT=0,
NAVIGATION_STATE_MANUAL, SYSTEM_STATE_MANUAL,
NAVIGATION_STATE_SEATBELT, SYSTEM_STATE_SEATBELT,
NAVIGATION_STATE_DESCENT, SYSTEM_STATE_AUTO,
NAVIGATION_STATE_LOITER, SYSTEM_STATE_REBOOT,
NAVIGATION_STATE_AUTO_READY, SYSTEM_STATE_IN_AIR_RESTORE
NAVIGATION_STATE_MISSION, } system_state_t;
NAVIGATION_STATE_RTL,
NAVIGATION_STATE_LAND,
NAVIGATION_STATE_TAKEOFF
} navigation_state_t;
typedef enum { typedef enum {
ARMING_STATE_INIT = 0, MANUAL_STANDBY = 0,
ARMING_STATE_STANDBY, MANUAL_READY,
ARMING_STATE_ARMED, MANUAL_IN_AIR
ARMING_STATE_ABORT, } manual_state_t;
ARMING_STATE_ERROR,
ARMING_STATE_REBOOT, typedef enum {
ARMING_STATE_IN_AIR_RESTORE SEATBELT_STANDBY,
} arming_state_t; 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 { typedef enum {
HIL_STATE_OFF = 0, HIL_STATE_OFF = 0,
@ -100,7 +121,7 @@ enum VEHICLE_MODE_FLAG {
typedef enum { typedef enum {
MODE_SWITCH_MANUAL = 0, MODE_SWITCH_MANUAL = 0,
MODE_SWITCH_ASSISTED, MODE_SWITCH_SEATBELT,
MODE_SWITCH_AUTO MODE_SWITCH_AUTO
} mode_switch_pos_t; } mode_switch_pos_t;
@ -114,26 +135,6 @@ typedef enum {
MISSION_SWITCH_MISSION MISSION_SWITCH_MISSION
} mission_switch_pos_t; } 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 * 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_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */
// uint64_t failsave_highlevel_begin; TO BE COMPLETED // uint64_t failsave_highlevel_begin; TO BE COMPLETED
navigation_state_t navigation_state; /**< current navigation state */ system_state_t system_state; /**< current system state */
arming_state_t arming_state; /**< current arming state */ // arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */ hil_state_t hil_state; /**< current hil state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ 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; return_switch_pos_t return_switch;
mission_switch_pos_t mission_switch; mission_switch_pos_t mission_switch;
bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_armed; /**< true is motors / actuators are armed */
bool flag_system_emergency; bool flag_system_emergency;
bool flag_system_in_air_restore; /**< true if we can restore in mid air */ 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_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position 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_gyro_calibration; /**< true if gyro calibration is requested */
bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
bool flag_preflight_accel_calibration; bool flag_preflight_accel_calibration;

Write Preview
Loading…
Cancel
Save