From cff8bf14b76af78b2a945f1a491e846a3760e387 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Fri, 15 Feb 2019 17:33:04 +0530 Subject: [PATCH] AP_Compass: add support for AK09916 via invensensev2 aux bus --- libraries/AP_Compass/AP_Compass.cpp | 3 +- libraries/AP_Compass/AP_Compass_AK09916.cpp | 418 +++++++++++++------- libraries/AP_Compass/AP_Compass_AK09916.h | 145 +++++-- 3 files changed, 388 insertions(+), 178 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 5089cc529f..cbe0630e48 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -622,13 +622,11 @@ void Compass::_probe_external_i2c_compasses(void) // AK09916 on ICM20948 FOREACH_I2C_EXTERNAL(i) { ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR), true, ROTATION_PITCH_180_YAW_90)); } FOREACH_I2C_INTERNAL(i) { ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR), all_external, ROTATION_PITCH_180_YAW_90)); } @@ -791,6 +789,7 @@ void Compass::_detect_backends(void) // we run the AK8963 only on the 2nd MPU9250, which leaves the // first MPU9250 to run without disturbance at high rate ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(1, ROTATION_YAW_270)); + ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe_ICM20948(0, ROTATION_ROLL_180_YAW_270)); break; case AP_BoardConfig::PX4_BOARD_FMUV5: diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index 5b6f82fd85..a79c68cbff 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -17,10 +17,12 @@ */ #include "AP_Compass_AK09916.h" +#include #include #include #include #include +#include #define REG_COMPANY_ID 0x00 #define REG_DEVICE_ID 0x01 @@ -41,23 +43,43 @@ #define REG_ICM_PWR_MGMT_1 0x06 #define REG_ICM_INT_PIN_CFG 0x0f -#define ICM_WHOAMI_VAL 0xEA +#define AK09916_Device_ID 0x09 +#define AK09916_MILLIGAUSS_SCALE 10.0f extern const AP_HAL::HAL &hal; -/* - probe for AK09916 directly on I2C - */ +struct PACKED sample_regs { + int16_t val[3]; + uint8_t st2; +}; + +AP_Compass_AK09916::AP_Compass_AK09916(AP_AK09916_BusDriver *bus, + bool force_external, + enum Rotation rotation) + : _bus(bus) + , _force_external(force_external) + , _rotation(rotation) +{ +} + +AP_Compass_AK09916::~AP_Compass_AK09916() +{ + delete _bus; +} + AP_Compass_Backend *AP_Compass_AK09916::probe(AP_HAL::OwnPtr dev, - bool force_external, - enum Rotation rotation) + bool force_external, + enum Rotation rotation) { if (!dev) { return nullptr; } - AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(std::move(dev), nullptr, - force_external, - rotation, AK09916_I2C); + AP_AK09916_BusDriver *bus = new AP_AK09916_BusDriver_HALDevice(std::move(dev)); + if (!bus) { + return nullptr; + } + + AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(bus, force_external, rotation); if (!sensor || !sensor->init()) { delete sensor; return nullptr; @@ -66,21 +88,33 @@ AP_Compass_Backend *AP_Compass_AK09916::probe(AP_HAL::OwnPtr return sensor; } - -/* - probe for AK09916 connected via an ICM20948 - */ AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr dev, - AP_HAL::OwnPtr dev_icm, - bool force_external, - enum Rotation rotation) + bool force_external, + enum Rotation rotation) { - if (!dev || !dev_icm) { + if (!dev) { return nullptr; } - AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(std::move(dev), std::move(dev_icm), - force_external, - rotation, AK09916_ICM20948_I2C); + AP_InertialSensor &ins = AP::ins(); + + /* Allow MPU9250 to shortcut auxiliary bus and host bus */ + ins.detect_backends(); + + return probe(std::move(dev), force_external, rotation); +} + +AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(uint8_t inv2_instance, + enum Rotation rotation) +{ + AP_InertialSensor &ins = AP::ins(); + + AP_AK09916_BusDriver *bus = + new AP_AK09916_BusDriver_Auxiliary(ins, HAL_INS_INV2_SPI, inv2_instance, HAL_COMPASS_AK09916_I2C_ADDR); + if (!bus) { + return nullptr; + } + + AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(bus, false, rotation); if (!sensor || !sensor->init()) { delete sensor; return nullptr; @@ -89,152 +123,246 @@ AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr _dev, - AP_HAL::OwnPtr _dev_icm, - bool _force_external, - enum Rotation _rotation, - enum bus_type _bus_type) - : bus_type(_bus_type) - , dev(std::move(_dev)) - , dev_icm(std::move(_dev_icm)) - , force_external(_force_external) - , rotation(_rotation) -{ -} - bool AP_Compass_AK09916::init() { - if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + AP_HAL::Semaphore *bus_sem = _bus->get_semaphore(); + + if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + hal.console->printf("AK09916: Unable to get bus semaphore\n"); return false; } - if (bus_type == AK09916_ICM20948_I2C && dev_icm) { - uint8_t rval; - if (!dev_icm->read_registers(REG_ICM_WHOAMI, &rval, 1) || - rval != ICM_WHOAMI_VAL) { - // not an ICM_WHOAMI - goto fail; - } - uint8_t retries = 5; - do { - // reset then bring sensor out of sleep mode - if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x80)) { - goto fail; - } - hal.scheduler->delay(10); - - if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x00)) { - goto fail; - } - hal.scheduler->delay(10); - - // see if ICM20948 is sleeping - if (!dev_icm->read_registers(REG_ICM_PWR_MGMT_1, &rval, 1)) { - goto fail; - } - if ((rval & 0x40) == 0) { - break; - } - } while (retries--); - - if (rval & 0x40) { - // it didn't come out of sleep - goto fail; - } - - // initially force i2c bypass off - dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x00); - hal.scheduler->delay(1); - - // now check if a AK09916 shows up on the bus. If it does then - // we have both a real AK09916 and a ICM20948 with an embedded - // AK09916. In that case we will fail the driver load and use - // the main AK09916 driver - uint16_t whoami; - if (dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2)) { - // a device is replying on the AK09916 I2C address, don't - // load the ICM20948 - hal.console->printf("ICM20948: AK09916 bus conflict\n"); - goto fail; - } - - // now force bypass on - dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x02); - hal.scheduler->delay(1); - } - - uint16_t whoami; - if (!dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2) || - whoami != 0x0948) { - // not a AK09916 + if (!_bus->configure()) { + hal.console->printf("AK09916: Could not configure the bus\n"); goto fail; } - dev->setup_checked_registers(2); + if (!_check_id()) { + hal.console->printf("AK09916: Wrong id\n"); + goto fail; + } - dev->write_register(REG_CNTL2, 0x08, true); // continuous 100Hz - dev->write_register(REG_CNTL3, 0x00, true); - - dev->get_semaphore()->give(); + if (!_setup_mode()) { + hal.console->printf("AK09916: Could not setup mode\n"); + goto fail; + } + + if (!_bus->start_measurements()) { + hal.console->printf("AK09916: Could not start measurements\n"); + goto fail; + } + + _initialized = true; /* register the compass instance in the frontend */ - compass_instance = register_compass(); + _compass_instance = register_compass(); - printf("Found a AK09916 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance); - - set_rotation(compass_instance, rotation); - - if (force_external) { - set_external(compass_instance, true); + if (_force_external) { + set_external(_compass_instance, true); } - - dev->set_device_type(bus_type == AK09916_ICM20948_I2C?DEVTYPE_ICM20948:DEVTYPE_AK09916); - set_dev_id(compass_instance, dev->get_bus_id()); - // call timer() at 100Hz - dev->register_periodic_callback(10000, - FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::timer, void)); + set_rotation(_compass_instance, _rotation); + + _bus->set_device_type(DEVTYPE_AK09916); + set_dev_id(_compass_instance, _bus->get_bus_id()); + + bus_sem->give(); + + _bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::_update, void)); return true; fail: - dev->get_semaphore()->give(); + bus_sem->give(); return false; } -void AP_Compass_AK09916::timer() -{ - struct PACKED { - int16_t magx; - int16_t magy; - int16_t magz; - uint8_t tmps; - uint8_t status2; - } data; - const float to_utesla = 4912.0f / 32752.0f; - const float utesla_to_milliGauss = 10; - const float range_scale = to_utesla * utesla_to_milliGauss; - Vector3f field; - - // check data ready - uint8_t st1; - if (!dev->read_registers(REG_ST1, &st1, 1) || !(st1 & 1)) { - goto check_registers; - } - - if (!dev->read_registers(REG_HXL, (uint8_t *)&data, sizeof(data))) { - goto check_registers; - } - - field(data.magx * range_scale, data.magy * range_scale, data.magz * range_scale); - - accumulate_sample(field, compass_instance); - -check_registers: - dev->check_next_register(); -} - void AP_Compass_AK09916::read() { - drain_accumulated_samples(compass_instance); + if (!_initialized) { + return; + } + + drain_accumulated_samples(_compass_instance); +} + +void AP_Compass_AK09916::_make_adc_sensitivity_adjustment(Vector3f& field) const +{ + static const float ADC_16BIT_RESOLUTION = 0.15f; + + field *= ADC_16BIT_RESOLUTION; +} + +void AP_Compass_AK09916::_update() +{ + struct sample_regs regs; + Vector3f raw_field; + + if (!_bus->block_read(REG_HXL, (uint8_t *) ®s, sizeof(regs))) { + return; + } + + /* Check for overflow. See AK09916's datasheet*/ + if ((regs.st2 & 0x08)) { + return; + } + + raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]); + + if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) { + return; + } + + _make_adc_sensitivity_adjustment(raw_field); + raw_field *= AK09916_MILLIGAUSS_SCALE; + + accumulate_sample(raw_field, _compass_instance, 10); +} + +bool AP_Compass_AK09916::_check_id() +{ + for (int i = 0; i < 5; i++) { + uint8_t deviceid = 0; + + /* Read AK09916's id */ + if (_bus->register_read(REG_DEVICE_ID, &deviceid) && + deviceid == AK09916_Device_ID) { + return true; + } + } + + return false; +} + +bool AP_Compass_AK09916::_setup_mode() { + return _bus->register_write(REG_CNTL2, 0x04); //Continuous Mode 2 +} + +bool AP_Compass_AK09916::_reset() +{ + return _bus->register_write(REG_CNTL3, 0x01); //Soft Reset +} + +/* AP_HAL::I2CDevice implementation of the AK09916 */ +AP_AK09916_BusDriver_HALDevice::AP_AK09916_BusDriver_HALDevice(AP_HAL::OwnPtr dev) + : _dev(std::move(dev)) +{ +} + +bool AP_AK09916_BusDriver_HALDevice::block_read(uint8_t reg, uint8_t *buf, uint32_t size) +{ + return _dev->read_registers(reg, buf, size); +} + +bool AP_AK09916_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val) +{ + return _dev->read_registers(reg, val, 1); +} + +bool AP_AK09916_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val) +{ + return _dev->write_register(reg, val); +} + +AP_HAL::Semaphore *AP_AK09916_BusDriver_HALDevice::get_semaphore() +{ + return _dev->get_semaphore(); +} + +AP_HAL::Device::PeriodicHandle AP_AK09916_BusDriver_HALDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + return _dev->register_periodic_callback(period_usec, cb); +} + +/* AK09916 on an auxiliary bus of IMU driver */ +AP_AK09916_BusDriver_Auxiliary::AP_AK09916_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id, + uint8_t backend_instance, uint8_t addr) +{ + /* + * Only initialize members. Fails are handled by configure or while + * getting the semaphore + */ + _bus = ins.get_auxiliary_bus(backend_id, backend_instance); + if (!_bus) { + return; + } + + _slave = _bus->request_next_slave(addr); +} + +AP_AK09916_BusDriver_Auxiliary::~AP_AK09916_BusDriver_Auxiliary() +{ + /* After started it's owned by AuxiliaryBus */ + if (!_started) { + delete _slave; + } +} + +bool AP_AK09916_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size) +{ + if (_started) { + /* + * We can only read a block when reading the block of sample values - + * calling with any other value is a mistake + */ + assert(reg == REG_HXL); + + int n = _slave->read(buf); + return n == static_cast(size); + } + + int r = _slave->passthrough_read(reg, buf, size); + + return r > 0 && static_cast(r) == size; +} + +bool AP_AK09916_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val) +{ + return _slave->passthrough_read(reg, val, 1) == 1; +} + +bool AP_AK09916_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val) +{ + return _slave->passthrough_write(reg, val) == 1; +} + +AP_HAL::Semaphore *AP_AK09916_BusDriver_Auxiliary::get_semaphore() +{ + return _bus ? _bus->get_semaphore() : nullptr; +} + +bool AP_AK09916_BusDriver_Auxiliary::configure() +{ + if (!_bus || !_slave) { + return false; + } + return true; +} + +bool AP_AK09916_BusDriver_Auxiliary::start_measurements() +{ + if (_bus->register_periodic_read(_slave, REG_HXL, sizeof(sample_regs)) < 0) { + return false; + } + + _started = true; + + return true; +} + +AP_HAL::Device::PeriodicHandle AP_AK09916_BusDriver_Auxiliary::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + return _bus->register_periodic_callback(period_usec, cb); +} + +// set device type within a device class +void AP_AK09916_BusDriver_Auxiliary::set_device_type(uint8_t devtype) +{ + _bus->set_device_type(devtype); +} + +// return 24 bit bus identifier +uint32_t AP_AK09916_BusDriver_Auxiliary::get_bus_id(void) const +{ + return _bus->get_bus_id(); } diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h index 8dc63f4f25..b91805039f 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.h +++ b/libraries/AP_Compass/AP_Compass_AK09916.h @@ -26,50 +26,133 @@ # define HAL_COMPASS_AK09916_I2C_ADDR 0x0C #endif -// the AK09916 can be connected via an ICM20948 -#ifndef HAL_COMPASS_ICM20948_I2C_ADDR -# define HAL_COMPASS_ICM20948_I2C_ADDR 0x69 -#endif +class AuxiliaryBus; +class AuxiliaryBusSlave; +class AP_InertialSensor; +class AP_AK09916_BusDriver; class AP_Compass_AK09916 : public AP_Compass_Backend { public: + /* Probe for AK09916 standalone on I2C bus */ static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, - bool force_external = false, + bool force_external, enum Rotation rotation = ROTATION_NONE); - // separate probe function for when behind a ICM20948 IMU + /* Probe for AK09916 on auxiliary bus of ICM20948, connected through I2C */ static AP_Compass_Backend *probe_ICM20948(AP_HAL::OwnPtr dev, - AP_HAL::OwnPtr dev_icm, - bool force_external = false, - enum Rotation rotation = ROTATION_NONE); - - void read() override; + bool force_external, + enum Rotation rotation = ROTATION_NONE); + + /* Probe for AK09916 on auxiliary bus of ICM20948, connected through SPI */ + static AP_Compass_Backend *probe_ICM20948(uint8_t mpu9250_instance, + enum Rotation rotation = ROTATION_NONE); static constexpr const char *name = "AK09916"; + virtual ~AP_Compass_AK09916(); + + void read() override; + private: - enum bus_type { - AK09916_I2C=0, - AK09916_ICM20948_I2C, - } bus_type; - - AP_Compass_AK09916(AP_HAL::OwnPtr dev, - AP_HAL::OwnPtr dev_icm, - bool force_external, - enum Rotation rotation, - enum bus_type bus_type); + AP_Compass_AK09916(AP_AK09916_BusDriver *bus, bool force_external, + enum Rotation rotation = ROTATION_NONE); - AP_HAL::OwnPtr dev; - AP_HAL::OwnPtr dev_icm; - - /** - * Device periodic callback to read data from the sensor. - */ bool init(); - void timer(); + void _make_factory_sensitivity_adjustment(Vector3f &field) const; + void _make_adc_sensitivity_adjustment(Vector3f &field) const; - uint8_t compass_instance; - bool force_external; - enum Rotation rotation; + bool _reset(); + bool _setup_mode(); + bool _check_id(); + bool _calibrate(); + + void _update(); + + AP_AK09916_BusDriver *_bus; + + float _magnetometer_ASA[3] {0, 0, 0}; + bool _force_external; + uint8_t _compass_instance; + bool _initialized; + enum Rotation _rotation; +}; + + +class AP_AK09916_BusDriver +{ +public: + virtual ~AP_AK09916_BusDriver() { } + + virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) = 0; + virtual bool register_read(uint8_t reg, uint8_t *val) = 0; + virtual bool register_write(uint8_t reg, uint8_t val) = 0; + + virtual AP_HAL::Semaphore *get_semaphore() = 0; + + virtual bool configure() { return true; } + virtual bool start_measurements() { return true; } + virtual AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t, AP_HAL::Device::PeriodicCb) = 0; + + // set device type within a device class + virtual void set_device_type(uint8_t devtype) = 0; + + // return 24 bit bus identifier + virtual uint32_t get_bus_id(void) const = 0; +}; + +class AP_AK09916_BusDriver_HALDevice: public AP_AK09916_BusDriver +{ +public: + AP_AK09916_BusDriver_HALDevice(AP_HAL::OwnPtr dev); + + virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) override; + virtual bool register_read(uint8_t reg, uint8_t *val) override; + virtual bool register_write(uint8_t reg, uint8_t val) override; + + virtual AP_HAL::Semaphore *get_semaphore() override; + AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + + // set device type within a device class + void set_device_type(uint8_t devtype) override { + _dev->set_device_type(devtype); + } + + // return 24 bit bus identifier + uint32_t get_bus_id(void) const override { + return _dev->get_bus_id(); + } + +private: + AP_HAL::OwnPtr _dev; +}; + +class AP_AK09916_BusDriver_Auxiliary : public AP_AK09916_BusDriver +{ +public: + AP_AK09916_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id, + uint8_t backend_instance, uint8_t addr); + ~AP_AK09916_BusDriver_Auxiliary(); + + bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) override; + bool register_read(uint8_t reg, uint8_t *val) override; + bool register_write(uint8_t reg, uint8_t val) override; + + AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + + AP_HAL::Semaphore *get_semaphore() override; + + bool configure() override; + bool start_measurements() override; + + // set device type within a device class + void set_device_type(uint8_t devtype) override; + + // return 24 bit bus identifier + uint32_t get_bus_id(void) const override; + +private: + AuxiliaryBus *_bus; + AuxiliaryBusSlave *_slave; + bool _started; };