diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-30 11:16:01 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-30 11:16:01 +0100 |
commit | 4db0ec03ce7c599621915a92b3ad0d2e8fa17b71 (patch) | |
tree | f70b8b30aaeeaed79a2faabe5ba7e8a458b533d1 /apps/multirotor_att_control/multirotor_att_control_main.c | |
parent | 01932a2dc35c0dc4c6a45e9eb4b46660e7e4136a (diff) | |
download | px4-firmware-4db0ec03ce7c599621915a92b3ad0d2e8fa17b71.tar.gz px4-firmware-4db0ec03ce7c599621915a92b3ad0d2e8fa17b71.tar.bz2 px4-firmware-4db0ec03ce7c599621915a92b3ad0d2e8fa17b71.zip |
Better yaw position control, but not quite there yet
Diffstat (limited to 'apps/multirotor_att_control/multirotor_att_control_main.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 10 |
1 files changed, 7 insertions, 3 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index b77281dc2..77adaf217 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -139,6 +139,7 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; + bool flag_system_armed = false; while (!thread_should_exit) { @@ -208,15 +209,16 @@ mc_thread_main(int argc, char *argv[]) /* 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) { + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { att_sp.yaw_body = att.yaw; } att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; /* 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; + if (manual.yaw < -0.02f || 0.02f < manual.yaw) { + att_sp.yaw_body = att_sp.yaw_body + manual.yaw * -0.0025f; } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); @@ -264,8 +266,10 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } + /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; + flag_system_armed = state.flag_system_armed; perf_end(mc_loop_perf); } |