diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 45a5112c611816fe1d1c80bcfacaa86ea544a9d1..4a2aae82ca951d62fb070e67560c1f97b8b7fd7a 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); } }