From ec2308bcd23da2b876d16a7ce186461377d9390b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 23 Aug 2014 21:42:23 +0900 Subject: [PATCH] AC_AttControl: bug fix for ef target during acro --- 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 45a5112c61..4a2aae82ca 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -387,12 +387,12 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_ } if (_angle_ef_target.y > 9000.0f) { _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); - _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x); + _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.y); _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); } if (_angle_ef_target.y < -9000.0f) { _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); - _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x); + _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.y); _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); } }