From 7972a56076058331e43a8a1e06c3b2c87e833bce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 31 Dec 2012 00:41:11 +0100 Subject: [PATCH] State machine / switching improvements --- apps/commander/commander.c | 10 +--- apps/commander/state_machine_helper.c | 45 +++++++------- apps/commander/state_machine_helper.h | 4 +- .../fixedwing_att_control_main.c | 60 +++---------------- 4 files changed, 36 insertions(+), 83 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f47e481916..3ad90c029e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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, ¤t_status, mavlink_fd); + /* enable guided mode */ + update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - } else { - update_state_machine_mode_hold(stat_pub, ¤t_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, ¤t_status, mavlink_fd); } /* handle the case where RC signal was regained */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index fdd32ca40a..18337a4ee1 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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); } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 815645089a..2f2ccc7298 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -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 diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 646deb62eb..a36f1ce7d8 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -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;