AP_Compass: AK8963: Use MPU9250 auxiliary bus

libraries/AP_Compass/AP_Compass_AK8963.cpp

@@ -22,29 +22,6 @@
 #include "AP_Compass_AK8963.h"
 #include <AP_InertialSensor/AP_InertialSensor_MPU9250.h>
 
-#define READ_FLAG                   0x80
-#define MPUREG_I2C_SLV0_ADDR        0x25
-#define MPUREG_I2C_SLV0_REG         0x26
-#define MPUREG_I2C_SLV0_CTRL        0x27
-#define MPUREG_EXT_SENS_DATA_00     0x49
-#define MPUREG_I2C_SLV0_DO          0x63
-
-/* bit definitions for MPUREG_USER_CTRL */
-#define MPUREG_USER_CTRL                                0x6A
-/* Enable MPU to act as the I2C Master to external slave sensors */
-#       define BIT_USER_CTRL_I2C_MST_EN                 0x20
-#       define BIT_USER_CTRL_I2C_IF_DIS                 0x10
-
-/* bit definitions for MPUREG_MST_CTRL */
-#define MPUREG_I2C_MST_CTRL                             0x24
-#        define I2C_SLV0_EN                             0x80
-#        define I2C_MST_CLOCK_400KHZ                    0x0D
-#        define I2C_MST_CLOCK_258KHZ                    0x08
-
-#define MPUREG_I2C_SLV4_CTRL                            0x34
-#define MPUREG_I2C_MST_DELAY_CTRL                       0x67
-#        define I2C_SLV0_DLY_EN                         0x01
-
 #define AK8963_I2C_ADDR                                 0x0c
 
 #define AK8963_WIA                                      0x00
@@ -93,9 +70,10 @@ AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus)
     _reset_filter();
 }
 
-AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250(Compass &compass, AP_HAL::SPIDeviceDriver *spi)
+AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250(Compass &compass, uint8_t mpu9250_instance)
 {
-    AP_AK8963_SerialBus *bus = new AP_AK8963_SerialBus_MPU9250(spi);
+    AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
+    AP_AK8963_SerialBus *bus = new AP_AK8963_SerialBus_MPU9250(ins, AK8963_I2C_ADDR, mpu9250_instance);
     if (!bus)
         return nullptr;
     return _detect(compass, bus);
@@ -148,11 +126,6 @@ bool AP_Compass_AK8963::init()
         goto fail_sem;
     }
 
-    if (!_bus->configure()) {
-        hal.console->printf("AK8963: Could not configure bus for AK8963\n");
-        goto fail;
-    }
-
     if (!_check_id()) {
         hal.console->printf("AK8963: Wrong id\n");
         goto fail;
@@ -403,94 +376,57 @@ void AP_Compass_AK8963::_dump_registers()
 }
 
 /* MPU9250 implementation of the AK8963 */
-AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250(AP_HAL::SPIDeviceDriver *spi)
+AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins,
+                                                         uint8_t addr,
+                                                         uint8_t mpu9250_instance)
 {
-    _spi = spi;
+    // Only initialize members. Fails are handled by configure or while
+    // getting the semaphore
+    _bus = ins.get_auxiliary_bus(HAL_INS_MPU9250, mpu9250_instance);
+    if (!_bus)
+        hal.scheduler->panic("Cannot get MPU9250 auxiliary bus");
 
-    if (_spi == NULL) {
+    _slave = _bus->request_next_slave(addr);
-        hal.console->printf("Cannot get SPIDevice_MPU9250\n");
-        return;
 }
+
+AP_AK8963_SerialBus_MPU9250::~AP_AK8963_SerialBus_MPU9250()
+{
+    /* After started it's owned by AuxiliaryBus */
+    if (!_started)
+        delete _slave;
 }
 
 void AP_AK8963_SerialBus_MPU9250::register_write(uint8_t reg, uint8_t value)
 {
-    const uint8_t count = 1;
+    _slave->passthrough_write(reg, value);
-    _write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR);
-    _write(MPUREG_I2C_SLV0_REG, reg);
-    _write(MPUREG_I2C_SLV0_DO, value);
-    _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count);
 }
 
 void AP_AK8963_SerialBus_MPU9250::register_read(uint8_t reg, uint8_t *value, uint8_t count)
 {
-    _write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG);
+    _slave->passthrough_read(reg, value, count);
-    _write(MPUREG_I2C_SLV0_REG, reg);
-    _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count);
-
-    hal.scheduler->delay(10);
-    _read(MPUREG_EXT_SENS_DATA_00, value, count);
-}
-
-void AP_AK8963_SerialBus_MPU9250::_read(uint8_t reg, uint8_t *buf, uint32_t count)
-{
-    ASSERT(count < 32);
-
-    reg |= READ_FLAG;
-    uint8_t tx[32] = { reg, };
-    uint8_t rx[32] = { };
-
-    _spi->transaction(tx, rx, count + 1);
-    memcpy(buf, rx + 1, count);
-}
-
-void AP_AK8963_SerialBus_MPU9250::_write(uint8_t reg, const uint8_t *buf, uint32_t count)
-{
-    ASSERT(count < 2);
-    uint8_t tx[2] = { reg, };
-
-    memcpy(tx+1, buf, count);
-    _spi->transaction(tx, NULL, count + 1);
-}
-
-bool AP_AK8963_SerialBus_MPU9250::configure()
-{
-    if (!AP_InertialSensor_MPU9250::initialize_driver_state(_spi))
-        return false;
-
-    uint8_t user_ctrl;
-    register_read(MPUREG_USER_CTRL, &user_ctrl, 1);
-    _write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
-    _write(MPUREG_I2C_MST_CTRL, I2C_MST_CLOCK_400KHZ);
-
-    return true;
 }
 
 void AP_AK8963_SerialBus_MPU9250::read_raw(struct raw_value *rv)
 {
-    _read(MPUREG_EXT_SENS_DATA_00, (uint8_t *) rv, sizeof(*rv));
+    if (_started) {
+        _slave->read((uint8_t*)rv);
+        return;
+    }
+
+    _slave->passthrough_read(0x03, (uint8_t*)rv, sizeof(*rv));
 }
 
 AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore()
 {
-    return _spi->get_semaphore();
+    return _bus ? _bus->get_semaphore() : nullptr;
 }
 
 bool AP_AK8963_SerialBus_MPU9250::start_measurements()
 {
-    const uint8_t count = sizeof(struct raw_value);
+    if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(struct raw_value)) < 0)
+        return false;
 
-    /* Don't sample AK8963 at MPU9250's sample rate. See MPU9250's datasheet
+    _started = true;
-     * about registers below and registers 73-96, External Sensor Data */
-    _write(MPUREG_I2C_SLV4_CTRL, 31);
-    _write(MPUREG_I2C_MST_DELAY_CTRL, I2C_SLV0_DLY_EN);
-
-    /* Configure the registers from AK8963 that will be read by MPU9250's
-     * master: we will get the result directly from MPU9250's registers starting
-     * from MPUREG_EXT_SENS_DATA_00 when read_raw() is called */
-    _write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG);
-    _write(MPUREG_I2C_SLV0_REG, AK8963_HXL);
-    _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count);
 
     return true;
 }

libraries/AP_Compass/AP_Compass_AK8963.h

@@ -9,6 +9,10 @@
 #include "Compass.h"
 #include "AP_Compass_Backend.h"
 
+class AuxiliaryBus;
+class AuxiliaryBusSlave;
+class AP_InertialSensor;
+
 class AP_AK8963_SerialBus
 {
 public:
@@ -26,7 +30,6 @@ public:
     }
     virtual void register_write(uint8_t reg, uint8_t value) = 0;
     virtual AP_HAL::Semaphore* get_semaphore() = 0;
-    virtual bool configure() = 0;
     virtual bool start_measurements() = 0;
     virtual void read_raw(struct raw_value *rv) = 0;
     virtual uint32_t get_dev_id() = 0;
@@ -35,7 +38,7 @@ public:
 class AP_Compass_AK8963 : public AP_Compass_Backend
 {
 public:
-    static AP_Compass_Backend *detect_mpu9250(Compass &compass, AP_HAL::SPIDeviceDriver *spi);
+    static AP_Compass_Backend *detect_mpu9250(Compass &compass, uint8_t mpu9250_instance);
     static AP_Compass_Backend *detect_i2c(Compass &compass,
                                           AP_HAL::I2CDriver *i2c,
                                           uint8_t addr);
@@ -85,21 +88,18 @@ private:
 class AP_AK8963_SerialBus_MPU9250: public AP_AK8963_SerialBus
 {
 public:
-    AP_AK8963_SerialBus_MPU9250(AP_HAL::SPIDeviceDriver *spi);
+    AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins, uint8_t addr, uint8_t mpu9250_instance);
+    ~AP_AK8963_SerialBus_MPU9250();
     void register_read(uint8_t reg, uint8_t *value, uint8_t count);
     void register_write(uint8_t reg, uint8_t value);
     AP_HAL::Semaphore* get_semaphore();
-    bool configure();
     bool start_measurements();
     void read_raw(struct raw_value *rv);
     uint32_t get_dev_id();
 private:
-    void _read(uint8_t reg, uint8_t *value, uint32_t count);
+    AuxiliaryBus *_bus = nullptr;
-    void _write(uint8_t reg, const uint8_t *value,  uint32_t count);
+    AuxiliaryBusSlave *_slave = nullptr;
-    void _write(uint8_t reg, const uint8_t value) {
+    bool _started;
-        _write(reg, &value, 1);
-    }
-    AP_HAL::SPIDeviceDriver *_spi;
 };
 
 class AP_AK8963_SerialBus_I2C: public AP_AK8963_SerialBus
@@ -109,7 +109,6 @@ public:
     void register_read(uint8_t reg, uint8_t *value, uint8_t count);
     void register_write(uint8_t reg, uint8_t value);
     AP_HAL::Semaphore* get_semaphore();
-    bool configure(){ return true; }
     bool start_measurements() { return true; }
     void read_raw(struct raw_value *rv);
     uint32_t get_dev_id();

libraries/AP_Compass/Compass.cpp

@@ -432,7 +432,7 @@ void Compass::_detect_backends(void)
       CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_BEBOP && \
       CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_MINLURE
     _add_backend(AP_Compass_HMC5843::detect_i2c(*this, hal.i2c));
-    _add_backend(AP_Compass_AK8963::detect_mpu9250(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250)));
+    _add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0));
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
     _add_backend(AP_Compass_HIL::detect(*this));
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
@@ -444,6 +444,8 @@ void Compass::_detect_backends(void)
                                                HAL_COMPASS_AK8963_I2C_ADDR));
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
     _add_backend(AP_Compass_PX4::detect(*this));
+#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
+    _add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0));
 #else
     #error Unrecognised HAL_COMPASS_TYPE setting
 #endif