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

State machine / switching improvements

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
apps/commander/commander.c
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) {
/* check auto mode switch for correct mode */
if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
/* enable stabilized mode */
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
/* enable guided mode */
update_state_machine_mode_guided(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_hold(stat_pub, &current_status, mavlink_fd);
}
} else {
/* center stick position, set SAS for all vehicle types */
current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status.flag_control_attitude_enabled = true;
current_status.flag_control_rates_enabled = true;
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
}
/* handle the case where RC signal was regained */

45
apps/commander/state_machine_helper.c
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)
{
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) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE");
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
tune_error();
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");
printf("[cmd] position guided mode\n");
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
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)
{
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);
} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
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) {
update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
}

4
apps/commander/state_machine_helper.h
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);
/**
* 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 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);
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

60
apps/fixedwing_att_control/fixedwing_att_control_main.c
View File

@ -185,55 +185,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
/* attitude control */
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
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* attitude control */
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 (vstatus.rc_signal_lost) {
// XXX define failsafe throttle param
//param_get(failsafe_throttle_handle, &failsafe_throttle);
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value */
if (isfinite(manual_sp.throttle)) {
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;