Copter: AC_ATT correct yaw error calculation

This commit is contained in:
lthall 2014-08-05 21:30:24 +09:30 committed by Randy Mackay
parent 90dc9411a5
commit 329118b7c9
1 changed files with 2 additions and 2 deletions

View File

@ -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;
}
//