From 1fc29a265488f15d6bbc6419d147399cfa2c174e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Roberto=20de=20Souza?= Date: Mon, 28 Sep 2015 11:21:31 -0300 Subject: [PATCH] AP_Compass: AK8963: Use MPU9250 auxiliary bus --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 126 +++++---------------- libraries/AP_Compass/AP_Compass_AK8963.h | 21 ++-- libraries/AP_Compass/Compass.cpp | 4 +- 3 files changed, 44 insertions(+), 107 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 3c1edd31c6..6744d133b1 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -22,29 +22,6 @@ #include "AP_Compass_AK8963.h" #include -#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) { - hal.console->printf("Cannot get SPIDevice_MPU9250\n"); - return; - } + _slave = _bus->request_next_slave(addr); +} + +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; - _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); + _slave->passthrough_write(reg, value); } 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); - _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; + _slave->passthrough_read(reg, value, count); } 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 - * 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); + _started = true; return true; } diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 267c7b1171..e979b956d9 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/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); - void _write(uint8_t reg, const uint8_t *value, uint32_t count); - void _write(uint8_t reg, const uint8_t value) { - _write(reg, &value, 1); - } - AP_HAL::SPIDeviceDriver *_spi; + AuxiliaryBus *_bus = nullptr; + AuxiliaryBusSlave *_slave = nullptr; + bool _started; }; 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(); diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index f866bc8dec..f7ccb2c9f5 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/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