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

Cleaned up control mode state machine / flight mode / navigation state machine still needs a bit cleaning up

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-27 18:27:08 +01:00
parent 61d7e1d285
commit f5bad08bd0
10 changed files with 625 additions and 300 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+73 -3
View File
@@ -1268,6 +1268,8 @@ int commander_thread_main(int argc, char *argv[])
current_status.offboard_control_signal_lost = true;
/* allow manual override initially */
current_status.flag_external_manual_override_ok = true;
/* flag position info as bad, do not allow auto mode */
current_status.flag_vector_flight_mode_ok = false;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@@ -1654,6 +1656,72 @@ 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.manual_mode_switch)) {
/* this switch is not properly mapped, set default */
if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
(current_status.system_type == MAV_TYPE_OCTOROTOR)) {
/* assuming a rotary wing, fall back to SAS */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
} else {
/* assuming a fixed wing, fall back to direct pass-through */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
}
} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set direct mode for vehicles supporting it */
if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
(current_status.system_type == MAV_TYPE_OCTOROTOR)) {
/* assuming a rotary wing, fall back to SAS */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
} else {
/* assuming a fixed wing, fall back to direct pass-through */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
}
} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
/* top stick position, set SAS for all vehicle types */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
} else {
/* center stick position, set rate control */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
}
/*
* Check if manual stability control modes have to be switched
*/
if (!isfinite(sp_man.manual_sas_switch)) {
/* this switch is not properly mapped, set default */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set altitude hold */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
/* top stick position */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
} else {
/* center stick position, set default */
current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
}
/*
* Check if left stick is in lower left position --> switch to standby state.
* Do this only for multirotors, not for fixed wing aircraft.
@@ -1686,19 +1754,21 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* check manual override switch - switch to manual or auto mode */
if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
/* enable manual override */
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else {
/* check auto mode switch for correct mode */
if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
//update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
// XXX hack
/* enable stabilized mode */
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else {
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
update_state_machine_mode_hold(stat_pub, &current_status, mavlink_fd);
}
}
apps/commander/state_machine_helper.c
+91 -55
View File
@@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = true;
fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
fprintf(stderr, "[cmd] EMERGENCY LANDING!\n");
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY LANDING!");
break;
case SYSTEM_STATE_EMCY_CUTOFF:
@@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = false;
fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
fprintf(stderr, "[cmd] EMERGENCY MOTOR CUTOFF!\n");
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY MOTOR CUTOFF!");
break;
case SYSTEM_STATE_GROUND_ERROR:
@@ -114,8 +114,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* prevent actuators from arming */
current_status->flag_system_armed = false;
fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
fprintf(stderr, "[cmd] GROUND ERROR, locking down propulsion system\n");
mavlink_log_critical(mavlink_fd, "[cmd] GROUND ERROR, locking down propulsion system");
break;
case SYSTEM_STATE_PREFLIGHT:
@@ -123,10 +123,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
/* set system flags according to state */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
mavlink_log_critical(mavlink_fd, "[cmd] Switched to PREFLIGHT state");
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to switch to PREFLIGHT state");
}
break;
@@ -136,13 +136,13 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
invalid_state = false;
/* set system flags according to state */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
mavlink_log_critical(mavlink_fd, "[cmd] REBOOTING SYSTEM");
usleep(500000);
up_systemreset();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to REBOOT");
}
break;
@@ -152,7 +152,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* standby enforces disarmed */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
mavlink_log_critical(mavlink_fd, "[cmd] Switched to STANDBY state");
break;
case SYSTEM_STATE_GROUND_READY:
@@ -162,7 +162,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* ground ready has motors / actuators armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
mavlink_log_critical(mavlink_fd, "[cmd] Switched to GROUND READY state");
break;
case SYSTEM_STATE_AUTO:
@@ -172,7 +172,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* auto is airborne and in auto mode, motors armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / AUTO mode");
break;
case SYSTEM_STATE_STABILIZED:
@@ -180,7 +180,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / STABILIZED mode");
break;
case SYSTEM_STATE_MANUAL:
@@ -188,7 +188,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / MANUAL mode");
break;
default:
@@ -203,7 +203,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
ret = OK;
}
if (invalid_state) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
mavlink_log_critical(mavlink_fd, "[cmd] REJECTING invalid state transition");
ret = ERROR;
}
return ret;
@@ -232,7 +232,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
}
void publish_armed_status(const struct vehicle_status_s *current_status) {
@@ -250,7 +250,7 @@ void publish_armed_status(const struct vehicle_status_s *current_status) {
*/
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
fprintf(stderr, "[commander] EMERGENCY HANDLER\n");
fprintf(stderr, "[cmd] EMERGENCY HANDLER\n");
/* Depending on the current state go to one of the error states */
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
@@ -262,7 +262,7 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
} else {
fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
fprintf(stderr, "[cmd] Unknown system state: #%d\n", current_status->state_machine);
}
}
@@ -272,7 +272,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
} else {
//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
}
}
@@ -497,7 +497,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
printf("[commander] arming\n");
printf("[cmd] arming\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
}
}
@@ -505,11 +505,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
printf("[commander] going standby\n");
printf("[cmd] going standby\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] MISSION ABORT!\n");
printf("[cmd] MISSION ABORT!\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
}
}
@@ -518,6 +518,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->flag_control_manual_enabled = true;
/* enable attitude control per default */
current_status->flag_control_attitude_enabled = true;
@@ -525,58 +526,91 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] manual mode\n");
printf("[cmd] manual mode\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
}
}
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (!current_status->flag_vector_flight_mode_ok) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE");
return;
}
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] stabilized mode\n");
printf("[cmd] stabilized mode\n");
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
current_status->flag_control_manual_enabled = false;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
void update_state_machine_mode_hold(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (!current_status->flag_vector_flight_mode_ok) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. HOLD MODE");
return;
}
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[cmd] stabilized mode\n");
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_HOLD;
current_status->flag_control_manual_enabled = false;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (!current_status->flag_vector_flight_mode_ok) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
return;
}
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
printf("[commander] auto mode\n");
printf("[cmd] auto mode\n");
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = false;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
{
printf("[commander] Requested new mode: %d\n", (int)mode);
uint8_t ret = 1;
/* Switch on HIL if in standby and not already in HIL mode */
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
/* Switch on HIL if in standby and not already in HIL mode */
if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
&& !current_status->flag_hil_enabled) {
/* Enable HIL on request */
current_status->flag_hil_enabled = true;
ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd);
publish_armed_status(current_status);
printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
} else if (current_status->state_machine != SYSTEM_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING HIL, not in standby.")
if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
/* Enable HIL on request */
current_status->flag_hil_enabled = true;
ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd);
publish_armed_status(current_status);
printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
current_status->flag_system_armed) {
mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, disarm first!")
} else {
mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, not in standby.")
}
}
/* switch manual / auto */
@@ -595,7 +629,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
ret = OK;
printf("[commander] arming due to command request\n");
printf("[cmd] arming due to command request\n");
}
}
@@ -605,13 +639,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
ret = OK;
printf("[commander] disarming due to command request\n");
printf("[cmd] disarming due to command request\n");
}
}
/* NEVER actually switch off HIL without reboot */
if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
fprintf(stderr, "[commander] DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
fprintf(stderr, "[cmd] DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
mavlink_log_critical(mavlink_fd, "[cmd] Power-cycle to exit HIL");
ret = ERROR;
}
@@ -636,7 +671,8 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
printf("system will reboot\n");
//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "[cmd] Rebooting..");
usleep(200000);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
ret = 0;
}
apps/commander/state_machine_helper.h
+9
View File
@@ -127,6 +127,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
*/
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is hold
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_mode_hold(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is auto
*