From 7bff8e931268dc5f6cddb5d1a2f2a715cfc5976d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 29 Mar 2014 20:41:22 +0900 Subject: [PATCH] AC_AttControl: constrain earth frame yaw angle error This temporarily resolves the wobble we see in Auto when the target heading is suddenly changed leading to a large yaw angle error --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 0b15148830..a460f53785 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -207,16 +207,17 @@ void AC_AttitudeControl::angle_ef_roll_pitch_yaw(float roll_angle_ef, float pitc angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor); angle_ef_error.z = wrap_180_cd_float(_angle_ef_target.z - _ahrs.yaw_sensor); + // constrain the yaw angle error + if (slew_yaw) { + angle_ef_error.z = constrain_float(angle_ef_error.z,-_slew_yaw,_slew_yaw); + } + // convert earth-frame angle errors to body-frame angle errors frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error); // convert body-frame angle errors to body-frame rate targets update_rate_bf_targets(); - if (slew_yaw) { - _rate_bf_target.z = constrain_float(_rate_bf_target.z,-_slew_yaw,_slew_yaw); - } - // body-frame to motor outputs should be called separately }