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

State machine / switching improvements

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-31 00:41:11 +01:00
parent 1b82dbb58d
commit 7972a56076
4 changed files with 36 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+3 -7
View File
@@ -1778,20 +1778,16 @@ int commander_thread_main(int argc, char *argv[])
} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
/* check auto mode switch for correct mode */ /* check auto mode switch for correct mode */
if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
/* enable stabilized mode */ /* enable guided mode */
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd); update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd); update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else {
update_state_machine_mode_hold(stat_pub, &current_status, mavlink_fd);
} }
} else { } else {
/* center stick position, set SAS for all vehicle types */ /* center stick position, set SAS for all vehicle types */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
} }
/* handle the case where RC signal was regained */ /* handle the case where RC signal was regained */
apps/commander/state_machine_helper.c
+24 -21
View File
@@ -547,14 +547,34 @@ 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) void update_state_machine_mode_stabilized(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_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[cmd] att stabilized mode\n");
int old_mode = current_status->flight_mode;
int old_manual_control_mode = current_status->manual_control_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
current_status->flag_control_manual_enabled = true;
if (old_mode != current_status->flight_mode ||
old_manual_control_mode != current_status->manual_control_mode) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
}
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
if (!current_status->flag_vector_flight_mode_ok) { if (!current_status->flag_vector_flight_mode_ok) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE"); mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
tune_error(); tune_error();
return; return;
} }
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { 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"); printf("[cmd] position guided mode\n");
int old_mode = current_status->flight_mode; int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
current_status->flag_control_manual_enabled = false; current_status->flag_control_manual_enabled = false;
@@ -566,25 +586,6 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
} }
} }
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) void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{ {
if (!current_status->flag_vector_flight_mode_ok) { if (!current_status->flag_vector_flight_mode_ok) {
@@ -634,6 +635,8 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
} }
apps/commander/state_machine_helper.h
+2 -2
View File
@@ -128,13 +128,13 @@ 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); 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 * Handle state machine if mode switch is guided
* *
* @param status_pub file descriptor for state update topic publication * @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on * @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages * @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); void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/** /**
* Handle state machine if mode switch is auto * Handle state machine if mode switch is auto
apps/fixedwing_att_control/fixedwing_att_control_main.c
+7 -53
View File
@@ -185,55 +185,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* control */ /* control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO) { if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
/* attitude control */ vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
} else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
// XXX define failsafe throttle param
//param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0;
/* limit throttle to 60 % of last value */
if (isfinite(manual_sp.throttle)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
// XXX: Stop copying setpoint / reference from bus, instead keep position
// and mix RC inputs in.
// XXX: For now just stabilize attitude, not anything else
// proper implementation should do stabilization in position controller
// and just check for stabilized or auto state
/* attitude control */ /* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
@@ -252,13 +205,14 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* if the RC signal is lost, try to stay level and go slowly back down to ground */ /* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) { if (vstatus.rc_signal_lost) {
// XXX define failsafe throttle param /* put plane into loiter */
//param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.3f; att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f; att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value */ /* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle)) { if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle; att_sp.thrust = 0.6f * manual_sp.throttle;
} else { } else {
att_sp.thrust = 0.0f; att_sp.thrust = 0.0f;