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
Split 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[]) @@ -1791,50 +1791,22 @@ int commander_thread_main(int argc, char *argv[])
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)) {
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} else {
/* no valid stick position, go to default */
/* 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) {
/* 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)) {
/* 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;
}
/* bottom stick position, go to manual mode */
current_status.mode_switch = MODE_SWITCH_MANUAL;
} 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;
current_status.mode_switch = MODE_SWITCH_AUTO;
} 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;
current_status.mode_switch = MODE_SWITCH_SEATBELT;
}
// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
@ -1845,43 +1817,58 @@ int commander_thread_main(int argc, char *argv[]) @@ -1845,43 +1817,58 @@ int commander_thread_main(int argc, char *argv[])
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,7 +1877,9 @@ int commander_thread_main(int argc, char *argv[]) @@ -1890,7 +1877,9 @@ 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!");
}
}
/*

592
apps/commander/state_machine_helper.c
Unescape View File

@ -54,422 +54,330 @@ @@ -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;
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) {
case NAVIGATION_STATE_STANDBY:
/* Always accept manual mode */
if (current_status->mode_switch == MODE_SWITCH_MANUAL) {
new_system_state = SYSTEM_STATE_MANUAL;
if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
|| current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
/* Accept SEATBELT if there is a local position estimate */
} else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) {
if (!current_status->flag_system_armed) {
mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
valid_transition = true;
if (current_status->flag_local_position_valid) {
new_system_state = SYSTEM_STATE_SEATBELT;
} 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:
if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
/* 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;
/* 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, "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;
}
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");
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, "Switched to AUTO READY state");
valid_transition = true;
mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
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
|| current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
/*
* Next up, check for
*/
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;
/* Update the system state in case it has changed */
if (current_status->system_state != new_system_state) {
/* 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;
/* 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, "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 */
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;
return;
}
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 */
mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state");
valid_transition = true;
valid_path = true;
}
break;
int ret = ERROR;
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) {
/* only check transition if the new state is actually different from the current one */
if (new_system_state != current_system_state) {
mavlink_log_critical(mavlink_fd, "Switched to LAND state");
valid_transition = true;
valid_path = true;
}
break;
switch (new_system_state) {
case SYSTEM_STATE_INIT:
default:
warnx("Unknown navigation state");
break;
}
}
/* 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 (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;
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.");
}
case SYSTEM_STATE_MANUAL:
} 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;
mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
valid_transition = true;
ret = OK;
}
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:
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");
valid_transition = true;
ret = OK;
}
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:
case SYSTEM_STATE_AUTO:
if (current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_ERROR) {
/* 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) {
valid_transition = true;
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
usleep(500000);
up_systemreset();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
ret = OK;
}
break;
case ARMING_STATE_IN_AIR_RESTORE:
case SYSTEM_STATE_REBOOT:
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;
}
}
/* from INIT you can go straight to REBOOT */
if (current_system_state == SYSTEM_STATE_INIT) {
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:
case SYSTEM_STATE_IN_AIR_RESTORE:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY) {
/* from INIT you can go straight to an IN AIR RESTORE */
if (current_system_state == SYSTEM_STATE_INIT) {
current_status->flag_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
ret = OK;
}
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 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)

4
apps/commander/state_machine_helper.h
Unescape View File

@ -47,9 +47,9 @@ @@ -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
Unescape View File

@ -60,27 +60,48 @@ @@ -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 { @@ -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 { @@ -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 @@ -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 @@ -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 @@ -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;

Write Preview
Loading…
Cancel
Save