From 12e1cf3710fa5e9b9b5dd7e0d55e952af9a473fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Oct 2012 17:38:26 +0100 Subject: Fixed error term calculation for yaw position --- apps/multirotor_att_control/multirotor_att_control_main.c | 11 +++++++++-- apps/multirotor_att_control/multirotor_attitude_control.c | 2 +- 2 files changed, 10 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 3466f9842..f2ef13e90 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -140,6 +140,7 @@ mc_thread_main(int argc, char *argv[]) bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; bool flag_system_armed = false; + bool man_yaw_zero_once = false; while (!thread_should_exit) { @@ -212,14 +213,20 @@ mc_thread_main(int argc, char *argv[]) state.flag_control_manual_enabled != flag_control_manual_enabled || state.flag_system_armed != flag_system_armed) { att_sp.yaw_body = att.yaw; + man_yaw_zero_once = false; } att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; + + if ((manual.yaw > -0.1f || 0.1f > manual.yaw)) { + man_yaw_zero_once = true; + } + /* only move setpoint if manual input is != 0 */ // XXX turn into param - if (manual.yaw < -0.01f || 0.01f < manual.yaw) { - att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.01f; + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && man_yaw_zero_once) { + att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f; } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 1258c739e..003180c18 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -217,7 +217,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s att->roll, att->rollspeed, deltaT); /* control yaw rate */ - rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed); + rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed); rates_sp->thrust = att_sp->thrust; -- cgit v1.2.3