From 329118b7c9223dd5663ca43178c669e332f8af65 Mon Sep 17 00:00:00 2001 From: lthall Date: Tue, 5 Aug 2014 21:30:24 +0930 Subject: [PATCH] Copter: AC_ATT correct yaw error calculation --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 78dcf8021c..9350d02b4d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -536,8 +536,8 @@ void AC_AttitudeControl::update_rate_bf_targets() _rate_bf_target.z = constrain_float(_rate_bf_target.z,-_angle_rate_y_max,_angle_rate_y_max); } - _rate_bf_target.x += -_angle_bf_error.y * _ahrs.get_gyro().z; - _rate_bf_target.y += _angle_bf_error.x * _ahrs.get_gyro().z; + _rate_bf_target.x += _angle_bf_error.y * _ahrs.get_gyro().z; + _rate_bf_target.y += -_angle_bf_error.x * _ahrs.get_gyro().z; } //