diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 6768e29ca4..1723d71901 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -512,7 +512,7 @@ AP_InertialSensor::_detect_backends(void) #elif HAL_INS_DEFAULT == HAL_INS_OILPAN _add_backend(AP_InertialSensor_Oilpan::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_MPU9250 - _add_backend(AP_InertialSensor_MPU9250::detect(*this)); + _add_backend(AP_InertialSensor_MPU9250::detect(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250))); #elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE _add_backend(AP_InertialSensor_Flymaple::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 2b53ef4770..03dba81717 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -201,13 +201,13 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) : /* detect the sensor */ -AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu) +AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu, AP_HAL::SPIDeviceDriver *spi) { AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu); if (sensor == NULL) { return NULL; } - if (!sensor->_init_sensor()) { + if (!sensor->_init_sensor(spi)) { delete sensor; return NULL; } @@ -215,8 +215,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor & return sensor; } -bool AP_InertialSensor_MPU9250::initialize_driver_state() { - AP_HAL::SPIDeviceDriver *spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); +bool AP_InertialSensor_MPU9250::initialize_driver_state(AP_HAL::SPIDeviceDriver *spi) { + if (!spi) return false; @@ -276,7 +276,7 @@ bool AP_InertialSensor_MPU9250::initialize_driver_state() { break; } #if MPU9250_DEBUG - _dump_registers(_spi); + _dump_registers(spi); #endif } @@ -299,9 +299,9 @@ fail_whoami: /* initialise the sensor */ -bool AP_InertialSensor_MPU9250::_init_sensor(void) +bool AP_InertialSensor_MPU9250::_init_sensor(AP_HAL::SPIDeviceDriver *spi) { - _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); + _spi = spi; _spi_sem = _spi->get_semaphore(); if (!_hardware_init()) @@ -487,7 +487,7 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) return false; } - if (!initialize_driver_state()) + if (!initialize_driver_state(_spi)) return false; _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index dd2ab8be86..4398395aa5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -35,10 +35,10 @@ public: * successful the bus is left on low speed so the caller can finish the * initialization of its driver. */ - static bool initialize_driver_state(); + static bool initialize_driver_state(AP_HAL::SPIDeviceDriver *spi); // detect the sensor - static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, AP_HAL::SPIDeviceDriver *spi); private: @@ -46,7 +46,7 @@ private: static void _register_write(AP_HAL::SPIDeviceDriver *spi, uint8_t reg, uint8_t val); - bool _init_sensor(void); + bool _init_sensor(AP_HAL::SPIDeviceDriver *spi); void _read_data_transaction(); bool _data_ready(); void _poll_data(void);