This commit is not buildable. Changed some AHRS timing functions, but still has lots of major bugs.