AP_Math: add rotateXY
AP_AHRS: use rotateXY for speed

Saves 0.1ms at 100hz