AP_InertialSensor: factor out MPU9250 initialization

Now we have the initialization code split in 2 parts:

1) Making sure the MPU9250 chip is alive and working: this is now in a
static function that may be called by other drivers that use MPU9250 as
backend.

2) The configuration of gyro and accel. Once the first part is completed
successfully the AP_InertialSensor_MPU9250 finishes the configuration of
the sensors it uses.

The only change in behavior here is that before we would try 25 time (5x
inside _hardware_init time 5x inside _init_sensor() that calls the first
function) to "boot the chip" and now we are doing "only" 5.

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;
+    // 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;
         }
-            uint8_t status = _register_read(MPUREG_INT_STATUS);
+
+        hal.scheduler->delay(10);
+        uint8_t status = _register_read(spi, MPUREG_INT_STATUS);
         if ((status & BIT_RAW_RDY_INT) != 0) {
-                _spi_sem->give();
             break;
         }
-            _spi_sem->give();
+#if MPU9250_DEBUG
+        _dump_registers(_spi);
+#endif
+    }
+
+    if (tries == 5) {
+        hal.console->println_P(PSTR("Failed to boot MPU9250 5 times"));
+        goto fail_tries;
     }
-        if (tries++ > 5) {
-            hal.console->printf("MPU9250: 5 unsuccessful attempts to initialize\n");
+
+    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;
 }
-    } while (1);
 
-    hal.scheduler->resume_timer_procs();
+/*
+  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;
 }
 

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);