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

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