AP_InertialSensors: make MPU6000 driver compatible with 400Hz main loop ...

...speed

The code is edited same as done by Andrew Tridgell for MPU9250