AP_InertialSensor: run MPU6000 sensor register reads at 8MHz

run other register IO at 500kHz

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -179,10 +179,10 @@ const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3;
 
 AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() : 
 	AP_InertialSensor(),
-    _mpu6000_product_id(AP_PRODUCT_ID_NONE),
     _drdy_pin(NULL),
     _temp(0),
-    _initialised(false)
+    _initialised(false),
+    _mpu6000_product_id(AP_PRODUCT_ID_NONE)
 {
 }
 
@@ -308,7 +308,9 @@ bool AP_InertialSensor_MPU6000::update( void )
 
     if (_last_filter_hz != _mpu6000_filter) {
         if (_spi_sem->take(10)) {
+            _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
             _set_filter_register(_mpu6000_filter, 0);
+            _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
             _spi_sem->give();
         }
     }
@@ -484,6 +486,9 @@ bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
         hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
     }
 
+    // initially run the bus at low speed (500kHz on APM2)
+    _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
+
     // Chip reset
     uint8_t tries;
     for (tries = 0; tries<5; tries++) {
@@ -575,6 +580,10 @@ bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
     register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN);
     hal.scheduler->delay(1);
 
+    // now that we have initialised, we set the SPI bus speed to high
+    // (8MHz on APM2)
+    _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
+
     _spi_sem->give();
 
     return true;