AP_InertialSensor: MPU9250: Add BusDriver abstraction

This adds a similar abstraction to the one in MPU60XX so it's possible to use
MPU9250 in other bus. The I2C implementation will come later.

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -176,7 +176,111 @@ extern const AP_HAL::HAL& hal;
  *  variants however
  */
 
-AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) :
+/*
+ * 2 bytes for each in this order: ACC_X, ACC_Y, ACC_Z, TEMP, GYRO_X, GYRO_Y
+ * and GYRO_Z
+ */
+#define MPU9250_SAMPLE_SIZE 14
+
+/* SPI bus driver implementation */
+AP_MPU9250_BusDriver_SPI::AP_MPU9250_BusDriver_SPI(AP_HAL::SPIDeviceDriver *spi)
+{
+    _spi = spi;
+}
+
+void AP_MPU9250_BusDriver_SPI::init()
+{
+    // disable I2C as recommended by the datasheet
+    write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
+}
+
+void AP_MPU9250_BusDriver_SPI::read8(uint8_t reg, uint8_t *val)
+{
+    uint8_t addr = reg | 0x80; // Set most significant bit
+
+    uint8_t tx[2];
+    uint8_t rx[2];
+
+    tx[0] = addr;
+    tx[1] = 0;
+    _spi->transaction(tx, rx, 2);
+
+    *val = rx[1];
+}
+
+void AP_MPU9250_BusDriver_SPI::write8(uint8_t reg, uint8_t val)
+{
+    uint8_t tx[2];
+    uint8_t rx[2];
+
+    tx[0] = reg;
+    tx[1] = val;
+    _spi->transaction(tx, rx, 2);
+}
+
+void AP_MPU9250_BusDriver_SPI::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed)
+{
+    _spi->set_bus_speed(speed);
+}
+
+bool AP_MPU9250_BusDriver_SPI::read_data_transaction(uint8_t *samples, uint8_t &n_samples)
+{
+    /* one register address followed by seven 2-byte registers */
+    struct PACKED {
+        uint8_t cmd;
+        uint8_t int_status;
+        uint8_t v[MPU9250_SAMPLE_SIZE];
+    } rx, tx = { cmd : MPUREG_INT_STATUS | 0x80, };
+
+    _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
+
+    if (!(rx.int_status & BIT_RAW_RDY_INT)) {
+        n_samples = 0;
+#if MPU9250_DEBUG
+        hal.console->printf("MPU9250: No sample available.\n");
+#endif
+        return false;
+    }
+
+    n_samples = 1;
+    memcpy(&samples[0], &rx.v[0], MPU9250_SAMPLE_SIZE);
+
+    return true;
+}
+
+AP_HAL::Semaphore* AP_MPU9250_BusDriver_SPI::get_semaphore()
+{
+    return _spi->get_semaphore();
+}
+
+void AP_MPU9250_BusDriver_SPI::get_driver_state(bool &initialized, bool &working)
+{
+    AP_HAL::SPIDeviceDriver::State state = _spi->get_state();
+
+    if (state == AP_HAL::SPIDeviceDriver::State::FAILED) {
+        initialized = true;
+        working = false;
+        return;
+    }
+    if (state == AP_HAL::SPIDeviceDriver::State::RUNNING) {
+        initialized = true;
+        working = true;
+        return;
+    }
+
+    initialized = false;
+}
+
+void AP_MPU9250_BusDriver_SPI::set_driver_state(bool working)
+{
+    if (working) {
+        _spi->set_state(AP_HAL::SPIDeviceDriver::State::RUNNING);
+    } else {
+        _spi->set_state(AP_HAL::SPIDeviceDriver::State::FAILED);
+    }
+}
+
+AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_MPU9250_BusDriver *bus) :
 	AP_InertialSensor_Backend(imu),
     _last_accel_filter_hz(-1),
     _last_gyro_filter_hz(-1),
@@ -184,6 +288,7 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) :
     _accel_filter(DEFAULT_SAMPLE_RATE, 15),
     _gyro_filter(DEFAULT_SAMPLE_RATE, 15),
     _have_sample_available(false),
+    _bus(bus),
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
     _default_rotation(ROTATION_ROLL_180_YAW_270)
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
@@ -199,86 +304,110 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) :
 {
 }
 
-
 /*
   detect the sensor
  */
 AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu, AP_HAL::SPIDeviceDriver *spi)
 {
-    AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu);
+    AP_MPU9250_BusDriver *bus = new AP_MPU9250_BusDriver_SPI(spi);
+    if (!bus)
+        return NULL;
+    return _detect(_imu, bus, HAL_INS_MPU9250);
+}
+
+/* Common detection method - it takes ownership of the bus, freeing it if it's
+ * not possible to return an AP_InertialSensor_Backend */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::_detect(AP_InertialSensor &_imu,
+                                                              AP_MPU9250_BusDriver *bus,
+                                                              int16_t id)
+{
+    AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu, bus);
     if (sensor == NULL) {
+        delete bus;
         return NULL;
     }
-    if (!sensor->_init_sensor(spi)) {
+    if (!sensor->_init_sensor()) {
         delete sensor;
+        delete bus;
         return NULL;
     }
 
+    sensor->_id = id;
+
     return sensor;
 }
 
-bool AP_InertialSensor_MPU9250::initialize_driver_state(AP_HAL::SPIDeviceDriver *spi) {
+bool AP_InertialSensor_MPU9250::initialize_driver_state(AP_HAL::SPIDeviceDriver *spi)
-
+{
     if (!spi)
         return false;
 
-    AP_HAL::SPIDeviceDriver::State state = spi->get_state();
+    AP_MPU9250_BusDriver_SPI *bus = new AP_MPU9250_BusDriver_SPI(spi);
-    if (state == AP_HAL::SPIDeviceDriver::State::FAILED)
+    if (!bus)
         return false;
-    if (state == AP_HAL::SPIDeviceDriver::State::RUNNING)
-        return true;
 
-    /* First time trying the initialization: if it fails from now on we will
+    bool initialized = initialize_driver_state(bus);
-     * set the device state to State::Failed so we don't try again if another
+    delete bus;
-     * driver asks it */
+    return initialized;
-    spi->set_state(AP_HAL::SPIDeviceDriver::State::FAILED);
+}
 
-    uint8_t whoami = _register_read(spi, MPUREG_WHOAMI);
+bool AP_InertialSensor_MPU9250::initialize_driver_state(AP_MPU9250_BusDriver *bus)
+{
+    bool initialized, working;
+
+    bus->get_driver_state(initialized, working);
+    if (initialized) {
+        return working;
+    }
+
+    bus->set_driver_state(false);
+
+    // initially run the bus at low speed
+    bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
+
+    uint8_t whoami = _register_read(bus, MPUREG_WHOAMI);
     if (whoami != MPUREG_WHOAMI_MPU9250 && whoami != MPUREG_WHOAMI_MPU9255) {
         hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
         goto fail_whoami;
     }
 
-    // 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++) {
-        uint8_t user_ctrl = _register_read(spi, MPUREG_USER_CTRL);
+        uint8_t user_ctrl = _register_read(bus, MPUREG_USER_CTRL);
 
         /* First disable the master I2C to avoid hanging the slaves on the
-         * aulixiliar I2C bus */
+         * auxiliary I2C bus */
         if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
-            _register_write(spi, MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN);
+            _register_write(bus, MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN);
             hal.scheduler->delay(10);
         }
 
         // reset device
-        _register_write(spi, MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
+        _register_write(bus, MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
         hal.scheduler->delay(100);
 
-        // disable I2C as recommended by the datasheet
+        // bus-dependent initialization
-        _register_write(spi, MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
+        bus->init();
 
         // Wake up device and select GyroZ clock. Note that the
         // MPU9250 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);
+        _register_write(bus, 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) {
+        if (_register_read(bus, MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) {
             break;
         }
 
         hal.scheduler->delay(10);
-        uint8_t status = _register_read(spi, MPUREG_INT_STATUS);
+        uint8_t status = _register_read(bus, MPUREG_INT_STATUS);
         if ((status & BIT_RAW_RDY_INT) != 0) {
             break;
         }
 #if MPU9250_DEBUG
-        _dump_registers(spi);
+        _dump_registers(bus);
 #endif
     }
 
@@ -287,13 +416,13 @@ bool AP_InertialSensor_MPU9250::initialize_driver_state(AP_HAL::SPIDeviceDriver
         goto fail_tries;
     }
 
-    spi->set_state(AP_HAL::SPIDeviceDriver::State::RUNNING);
+    bus->set_driver_state(true);
 
     return true;
 
 fail_tries:
 fail_whoami:
-    spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
+    bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
 
     return false;
 }
@@ -301,10 +430,9 @@ fail_whoami:
 /*
   initialise the sensor
  */
-bool AP_InertialSensor_MPU9250::_init_sensor(AP_HAL::SPIDeviceDriver *spi)
+bool AP_InertialSensor_MPU9250::_init_sensor()
 {
-    _spi = spi;
+    _bus_sem = _bus->get_semaphore();
-    _spi_sem = _spi->get_semaphore();
 
     if (!_hardware_init())
         return false;
@@ -321,7 +449,7 @@ bool AP_InertialSensor_MPU9250::_init_sensor(AP_HAL::SPIDeviceDriver *spi)
     _set_gyro_raw_sample_rate(_gyro_instance, DEFAULT_SAMPLE_RATE);
 
 #if MPU9250_DEBUG
-    _dump_registers(_spi);
+    _dump_registers(_bus);
 #endif
     return true;
 }
@@ -361,7 +489,7 @@ bool AP_InertialSensor_MPU9250::update( void )
  */
 void AP_InertialSensor_MPU9250::_poll_data(void)
 {
-    if (!_spi_sem->take_nonblocking()) {
+    if (!_bus_sem->take_nonblocking()) {
         /*
           the semaphore being busy is an expected condition when the
           mainline code is calling wait_for_sample() which will
@@ -371,39 +499,36 @@ void AP_InertialSensor_MPU9250::_poll_data(void)
         return;
     }
     _read_data_transaction();
-    _spi_sem->give();
+    _bus_sem->give();
 }
 
-
 /*
   read from the data registers and update filtered data
  */
 void AP_InertialSensor_MPU9250::_read_data_transaction() 
 {
-    /* one register address followed by seven 2-byte registers */
+    uint8_t n_samples;
-    struct PACKED {
+    uint8_t rx[MPU9250_SAMPLE_SIZE];
-        uint8_t cmd;
-        uint8_t int_status;
-        uint8_t v[14];
-    } rx, tx = { cmd : MPUREG_INT_STATUS | 0x80, };
 
     Vector3f accel, gyro;
 
-    _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
+    if (!_bus->read_data_transaction(rx, n_samples)) {
+        return;
+    }
 
 #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
 
-    accel = Vector3f(int16_val(rx.v, 1),
+    accel = Vector3f(int16_val(rx, 1),
-                     int16_val(rx.v, 0),
+                     int16_val(rx, 0),
-                     -int16_val(rx.v, 2));
+                     -int16_val(rx, 2));
     accel *= MPU9250_ACCEL_SCALE_1G;
     accel.rotate(_default_rotation);
     _rotate_and_correct_accel(_accel_instance, accel);
     _notify_new_accel_raw_sample(_accel_instance, accel);
 
-    gyro = Vector3f(int16_val(rx.v, 5),
+    gyro = Vector3f(int16_val(rx, 5),
-                    int16_val(rx.v, 4),
+                    int16_val(rx, 4),
-                    -int16_val(rx.v, 6));
+                    -int16_val(rx, 6));
     gyro *= GYRO_SCALE;
     gyro.rotate(_default_rotation);
     _rotate_and_correct_gyro(_gyro_instance, gyro);
@@ -422,42 +547,31 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
 /*
   read an 8 bit register
  */
-uint8_t AP_InertialSensor_MPU9250::_register_read(AP_HAL::SPIDeviceDriver *spi,
+uint8_t AP_InertialSensor_MPU9250::_register_read(AP_MPU9250_BusDriver *bus,
                                                   uint8_t reg)
 {
-    uint8_t addr = reg | 0x80; // Set most significant bit
+    uint8_t val;
-    uint8_t tx[2];
+    bus->read8(reg, &val);
-    uint8_t rx[2];
+    return val;
-
-    tx[0] = addr;
-    tx[1] = 0;
-    spi->transaction(tx, rx, 2);
-
-    return rx[1];
 }
 
 /*
   write an 8 bit register
  */
-void AP_InertialSensor_MPU9250::_register_write(AP_HAL::SPIDeviceDriver *spi,
+void AP_InertialSensor_MPU9250::_register_write(AP_MPU9250_BusDriver *bus,
                                                 uint8_t reg, uint8_t val)
 {
-    uint8_t tx[2];
+    bus->write8(reg, val);
-    uint8_t rx[2];
-
-    tx[0] = reg;
-    tx[1] = val;
-    spi->transaction(tx, rx, 2);
 }
 
 inline uint8_t AP_InertialSensor_MPU9250::_register_read(uint8_t reg)
 {
-    return _register_read(_spi, reg);
+    return _register_read(_bus, reg);
 }
 
 inline void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
 {
-    _register_write(_spi, reg, val);
+    _register_write(_bus, reg, val);
 }
 
 /*
@@ -486,12 +600,12 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
     // the bus while we do the long initialisation
     hal.scheduler->suspend_timer_procs();
 
-    if (!_spi_sem->take(100)) {
+    if (!_bus_sem->take(100)) {
         hal.console->printf("MPU9250: Unable to get semaphore");
         return false;
     }
 
-    if (!initialize_driver_state(_spi))
+    if (!initialize_driver_state(_bus))
         return false;
 
     _register_write(MPUREG_PWR_MGMT_2, 0x00);            // only used for wake-up in accelerometer only low power mode
@@ -515,10 +629,10 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
     // until we clear the interrupt
     _register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN);
 
-    // now that we have initialised, we set the SPI bus speed to high
+    // now that we have initialized, we set the SPI bus speed to high
-    _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
+    _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
 
-    _spi_sem->give();
+    _bus_sem->give();
 
     hal.scheduler->resume_timer_procs();
 
@@ -527,11 +641,11 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
 
 #if MPU9250_DEBUG
 // dump all config registers - used for debug
-void AP_InertialSensor_MPU9250::_dump_registers(AP_HAL::SPIDeviceDriver *spi)
+void AP_InertialSensor_MPU9250::_dump_registers(AP_MPU9250_BusDriver *bus)
 {
     hal.console->println("MPU9250 registers");
     for (uint8_t reg=0; reg<=126; reg++) {
-        uint8_t v = _register_read(spi, reg);
+        uint8_t v = _register_read(bus, reg);
         hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
         if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) {
             hal.console->println();

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h

@@ -13,11 +13,26 @@
 // enable debug to see a register dump on startup
 #define MPU9250_DEBUG 0
 
+class AP_MPU9250_BusDriver
+{
+public:
+    virtual ~AP_MPU9250_BusDriver() { };
+    virtual void init() = 0;
+    virtual void read8(uint8_t reg, uint8_t *val) = 0;
+    virtual void write8(uint8_t reg, uint8_t val) = 0;
+    virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) = 0;
+    virtual bool read_data_transaction(uint8_t* samples,
+                                       uint8_t &n_samples) = 0;
+    virtual AP_HAL::Semaphore* get_semaphore() = 0;
+    virtual void get_driver_state(bool &initialized, bool &working) = 0;
+    virtual void set_driver_state(bool working) = 0;
+};
+
 class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
 {
 public:
 
-    AP_InertialSensor_MPU9250(AP_InertialSensor &imu);
+    AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_MPU9250_BusDriver *bus);
 
     /* update accel and gyro state */
     bool update();
@@ -28,7 +43,7 @@ public:
     /* 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.
+     * The bus 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
@@ -40,12 +55,18 @@ public:
     static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, AP_HAL::SPIDeviceDriver *spi);
 
 private:
+    static AP_InertialSensor_Backend *_detect(AP_InertialSensor &_imu,
+                                              AP_MPU9250_BusDriver *bus,
+                                              int16_t id);
 
-    static uint8_t _register_read(AP_HAL::SPIDeviceDriver *spi, uint8_t reg);
+
-    static void _register_write(AP_HAL::SPIDeviceDriver *spi, uint8_t reg,
+    static bool initialize_driver_state(AP_MPU9250_BusDriver *bus);
+
+    static uint8_t _register_read(AP_MPU9250_BusDriver *bus, uint8_t reg);
+    static void _register_write(AP_MPU9250_BusDriver *bus, uint8_t reg,
                                 uint8_t val);
 
-    bool                 _init_sensor(AP_HAL::SPIDeviceDriver *spi);
+    bool                 _init_sensor();
     void                 _read_data_transaction();
     bool                 _data_ready();
     void                 _poll_data(void);
@@ -54,8 +75,8 @@ private:
     bool                 _hardware_init(void);
     bool                 _sample_available();
 
-    AP_HAL::SPIDeviceDriver *_spi;
+    AP_MPU9250_BusDriver *_bus;
-    AP_HAL::Semaphore *_spi_sem;
+    AP_HAL::Semaphore *_bus_sem;
 
     // support for updating filter at runtime
     int16_t _last_gyro_filter_hz;
@@ -91,8 +112,25 @@ private:
     enum Rotation _default_rotation;
 
 #if MPU9250_DEBUG
-    static void _dump_registers(AP_HAL::SPIDeviceDriver *spi);
+    static void _dump_registers(AP_MPU9250_BusDriver *bus);
 #endif
 };
 
+class AP_MPU9250_BusDriver_SPI : public AP_MPU9250_BusDriver
+{
+public:
+    AP_MPU9250_BusDriver_SPI(AP_HAL::SPIDeviceDriver *spi);
+    void init();
+    void read8(uint8_t reg, uint8_t *val);
+    void write8(uint8_t reg, uint8_t val);
+    void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
+    bool read_data_transaction(uint8_t* samples, uint8_t &n_samples);
+    AP_HAL::Semaphore* get_semaphore();
+    void get_driver_state(bool &initialized, bool &working);
+    void set_driver_state(bool working);
+
+private:
+    AP_HAL::SPIDeviceDriver *_spi;
+};
+
 #endif // __AP_INERTIAL_SENSOR_MPU9250_H__