Revert "AP_AHRS: use rotateXY for speed"

This reverts commit b0c5d9efd0dabcaed278b00b7a996c41bb9cc2b1.