diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index c07790913c..1ebab5f48b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/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;