This might be a good start but a cursory glance leads me to believe the code quality is attrocious: https://os.mbed.com/users/onehorse/code/MPU9250AHRS/