diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-31 00:41:11 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-31 00:41:11 +0100 |
commit | 7972a56076058331e43a8a1e06c3b2c87e833bce (patch) | |
tree | b0ee953ec808131ab60d90a252a160c2a8fc07fa /apps/fixedwing_att_control | |
parent | 1b82dbb58db9b7a279841714fe64c7830f71e290 (diff) | |
download | px4-firmware-7972a56076058331e43a8a1e06c3b2c87e833bce.tar.gz px4-firmware-7972a56076058331e43a8a1e06c3b2c87e833bce.tar.bz2 px4-firmware-7972a56076058331e43a8a1e06c3b2c87e833bce.zip |
State machine / switching improvements
Diffstat (limited to 'apps/fixedwing_att_control')
-rw-r--r-- | apps/fixedwing_att_control/fixedwing_att_control_main.c | 60 |
1 files changed, 7 insertions, 53 deletions
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 646deb62e..a36f1ce7d 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; |