added the Mahoney Algorithm. Also made some minor changes to the IMU.c/h files
Merge branch 'FreeIMU' of github.com:Susurrus/MPU60xx into FreeIMU