diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index f80571b3c5..ff114b538d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -212,47 +212,85 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor & return sensor; } -/* - initialise the sensor - */ -bool AP_InertialSensor_MPU9250::_init_sensor(void) -{ - _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); - _spi_sem = _spi->get_semaphore(); +bool AP_InertialSensor_MPU9250::initialize_driver_state() { + AP_HAL::SPIDeviceDriver *spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); + if (!spi) + return false; - // we need to suspend timers to prevent other SPI drivers grabbing - // the bus while we do the long initialisation - hal.scheduler->suspend_timer_procs(); + AP_HAL::SPIDeviceDriver::State state = spi->get_state(); + if (state == AP_HAL::SPIDeviceDriver::State::FAILED) + return false; + if (state == AP_HAL::SPIDeviceDriver::State::RUNNING) + return true; + + /* First time trying the initialization: if it fails from now on we will + * set the device state to State::Failed so we don't try again if another + * driver asks it */ + spi->set_state(AP_HAL::SPIDeviceDriver::State::FAILED); - uint8_t whoami = _register_read(MPUREG_WHOAMI); + uint8_t whoami = _register_read(spi, MPUREG_WHOAMI); if (whoami != MPUREG_WHOAMI_MPU9250 && whoami != MPUREG_WHOAMI_MPU9255) { hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami); - return false; + goto fail_whoami; } - uint8_t tries = 0; - do { - bool success = _hardware_init(); - if (success) { - hal.scheduler->delay(10); - if (!_spi_sem->take(100)) { - hal.console->printf("MPU9250: Unable to get semaphore"); - return false; - } - uint8_t status = _register_read(MPUREG_INT_STATUS); - if ((status & BIT_RAW_RDY_INT) != 0) { - _spi_sem->give(); - break; - } - _spi_sem->give(); + // initially run the bus at low speed + spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + + + // Chip reset + uint8_t tries; + for (tries = 0; tries < 5; tries++) { + // disable I2C as recommended by the datasheet + _register_write(spi, MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); + + // Wake up device and select GyroZ clock. Note that the + // MPU6000 starts up in sleep mode, and it can take some time + // for it to come out of sleep + _register_write(spi, MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO); + hal.scheduler->delay(5); + + // check it has woken up + if (_register_read(spi, MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) { + break; } - if (tries++ > 5) { - hal.console->printf("MPU9250: 5 unsuccessful attempts to initialize\n"); - return false; + + hal.scheduler->delay(10); + uint8_t status = _register_read(spi, MPUREG_INT_STATUS); + if ((status & BIT_RAW_RDY_INT) != 0) { + break; } - } while (1); +#if MPU9250_DEBUG + _dump_registers(_spi); +#endif + } - hal.scheduler->resume_timer_procs(); + if (tries == 5) { + hal.console->println_P(PSTR("Failed to boot MPU9250 5 times")); + goto fail_tries; + } + + spi->set_state(AP_HAL::SPIDeviceDriver::State::RUNNING); + + return true; + +fail_tries: +fail_whoami: + spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + + return false; +} + +/* + initialise the sensor + */ +bool AP_InertialSensor_MPU9250::_init_sensor(void) +{ + _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); + _spi_sem = _spi->get_semaphore(); + + if (!_hardware_init()) + return false; _gyro_instance = _imu.register_gyro(); _accel_instance = _imu.register_accel(); @@ -418,39 +456,17 @@ void AP_InertialSensor_MPU9250::_set_gyro_filter(uint8_t filter_hz) */ bool AP_InertialSensor_MPU9250::_hardware_init(void) { + // we need to suspend timers to prevent other SPI drivers grabbing + // the bus while we do the long initialisation + hal.scheduler->suspend_timer_procs(); + if (!_spi_sem->take(100)) { hal.console->printf("MPU9250: Unable to get semaphore"); return false; } - // initially run the bus at low speed - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - - // Chip reset - uint8_t tries; - for (tries = 0; tries<5; tries++) { - // disable I2C as recommended by the datasheet - _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); - - // Wake up device and select GyroZ clock. Note that the - // MPU6000 starts up in sleep mode, and it can take some time - // for it to come out of sleep - _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO); - hal.scheduler->delay(5); - - // check it has woken up - if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) { - break; - } -#if MPU9250_DEBUG - _dump_registers(); -#endif - } - if (tries == 5) { - hal.console->println_P(PSTR("Failed to boot MPU9250 5 times")); - _spi_sem->give(); + if (!initialize_driver_state()) return false; - } _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode @@ -479,6 +495,8 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) _spi_sem->give(); + hal.scheduler->resume_timer_procs(); + return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index de6ecf8f5f..ec3664915d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -26,6 +26,17 @@ public: bool gyro_sample_available(void) { return _have_sample_available; } bool accel_sample_available(void) { return _have_sample_available; } + /* Put the MPU9250 in a known state so it can be + * used both for the InertialSensor and as for backend of other drivers. + * + * The SPI semaphore must be taken and timer_procs suspended. + * + * This method puts the bus in low speed. If the initialization is + * successful the bus is left on low speed so the caller can finish the + * initialization of its driver. + */ + static bool initialize_driver_state(); + // detect the sensor static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);