diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-30 07:20:23 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-30 07:20:23 +0100 |
commit | fedf5470d6adb1945f48c4b08b2485fd7842e6c9 (patch) | |
tree | 5b1aa420a3d0a17d0b78044cc8958b48cb9325ff /apps/multirotor_att_control/multirotor_att_control_main.c | |
parent | d5af511f8da247fa75ed48b537445302d3946034 (diff) | |
download | px4-firmware-fedf5470d6adb1945f48c4b08b2485fd7842e6c9.tar.gz px4-firmware-fedf5470d6adb1945f48c4b08b2485fd7842e6c9.tar.bz2 px4-firmware-fedf5470d6adb1945f48c4b08b2485fd7842e6c9.zip |
Correctly initializing and updating yaw setpoint, sign still to be checked
Diffstat (limited to 'apps/multirotor_att_control/multirotor_att_control_main.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 20 |
1 files changed, 18 insertions, 2 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 0cdb53554..b77281dc2 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -136,6 +136,10 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ printf("[multirotor_att_control] starting\n"); + /* store last control mode to detect mode switches */ + bool flag_control_manual_enabled = false; + bool flag_control_attitude_enabled = false; + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -197,14 +201,23 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = manual.pitch; rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.throttle; - //printf("rates\n"); rates_sp.timestamp = hrt_absolute_time(); } if (state.flag_control_attitude_enabled) { + + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled) { + att_sp.yaw_body = att.yaw; + } + att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - att_sp.yaw_body = att.yaw + manual.yaw * -2.0f; + /* only move setpoint if manual input is != 0 */ + if (-3.0f * FLT_EPSILON < manual.yaw || manual.yaw > 3.0f * FLT_EPSILON) { + att_sp.yaw_body = att.yaw + manual.yaw * -2.0f; + } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); } @@ -251,6 +264,9 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } + flag_control_attitude_enabled = state.flag_control_attitude_enabled; + flag_control_manual_enabled = state.flag_control_manual_enabled; + perf_end(mc_loop_perf); } |