AP_AHRS: use a common function for updating the CD values

this ensures the wrapping of yaw is consistent between the 3 use cases