From 0b41da0dea2f2b477b156dea12f9e2f5a79bf286 Mon Sep 17 00:00:00 2001 From: Julien BERAUD Date: Mon, 6 Jul 2015 19:50:59 +0200 Subject: [PATCH] AP_Compass: rework to make the bus generic for AK8963 Supporting only MPU9250, prepare to implement another bus, i.e i2c on the bebop --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 215 ++++++++++----------- libraries/AP_Compass/AP_Compass_AK8963.h | 65 ++++--- libraries/AP_Compass/Compass.cpp | 3 +- 3 files changed, 145 insertions(+), 138 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 3f761e51ec..04f1fbc108 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -106,36 +106,21 @@ extern const AP_HAL::HAL& hal; -AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass) : +AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus) : AP_Compass_Backend(compass), _state(STATE_UNKNOWN), _initialized(false), _last_update_timestamp(0), - _last_accum_time(0) + _last_accum_time(0), + _bus(bus) { _mag_x_accum =_mag_y_accum = _mag_z_accum = 0; _mag_x =_mag_y = _mag_z = 0; _accum_count = 0; _magnetometer_adc_resolution = AK8963_16BIT_ADC; + init(); } -AP_Compass_Backend *AP_Compass_AK8963::detect(Compass &compass) -{ - AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass); - - if (sensor == nullptr) { - return nullptr; - } - - if (!sensor->init()) { - delete sensor; - return nullptr; - } - - return sensor; -} - - /* stub to satisfy Compass API*/ void AP_Compass_AK8963::accumulate(void) { @@ -143,24 +128,17 @@ void AP_Compass_AK8963::accumulate(void) bool AP_Compass_AK8963::init() { - _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); - - if (_spi == NULL) { - hal.console->println_P(PSTR("Cannot get SPIDevice_MPU9250")); - return false; - } - - _spi_sem = _spi->get_semaphore(); + _bus_sem = _bus->get_semaphore(); hal.scheduler->suspend_timer_procs(); - if (!_spi_sem->take(100)) { - hal.console->printf("AK8963: Unable to get MPU9250 semaphore"); + if (!_bus_sem->take(100)) { + hal.console->printf("AK8963: Unable to get bus semaphore"); goto fail_sem; } - if (!_configure_mpu9250()) { - hal.console->printf_P(PSTR("AK8963: MPU9250 not configured for AK8963\n")); + if (!_bus->configure()) { + hal.console->printf_P(PSTR("AK8963: Bus not configured for AK8963\n")); goto fail; } @@ -179,7 +157,7 @@ bool AP_Compass_AK8963::init() goto fail; } - if (!_start_conversion()) { + if (!_bus->start_conversion()) { hal.console->printf_P(PSTR("AK8963: conversion not started\n")); goto fail; } @@ -193,14 +171,14 @@ bool AP_Compass_AK8963::init() hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void)); - _spi_sem->give(); + _bus_sem->give(); hal.scheduler->resume_timer_procs(); return true; fail: - _spi_sem->give(); + _bus_sem->give(); fail_sem: hal.scheduler->resume_timer_procs(); @@ -227,9 +205,6 @@ void AP_Compass_AK8963::read() _mag_x_accum = _mag_y_accum = _mag_z_accum = 0; _accum_count = 0; -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP - field.rotate(ROTATION_YAW_90); -#endif publish_field(field, _compass_instance); } @@ -251,7 +226,7 @@ void AP_Compass_AK8963::_update() } break; case STATE_ERROR: - if (_start_conversion()) { + if (_bus->start_conversion()) { _state = STATE_SAMPLE; } break; @@ -267,7 +242,7 @@ bool AP_Compass_AK8963::_check_id() { for (int i = 0; i < 5; i++) { uint8_t deviceid; - _register_read(AK8963_WIA, &deviceid, 0x01); /* Read AK8963's id */ + _bus->register_read(AK8963_WIA, &deviceid, 0x01); /* Read AK8963's id */ if (deviceid == AK8963_Device_ID) { return true; @@ -277,40 +252,26 @@ bool AP_Compass_AK8963::_check_id() return false; } -bool AP_Compass_AK8963::_configure_mpu9250() -{ - if (!AP_InertialSensor_MPU9250::initialize_driver_state()) - return false; - - uint8_t user_ctrl; - _register_read(MPUREG_USER_CTRL, &user_ctrl, 1); - _bus_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN); - _bus_write(MPUREG_I2C_MST_CTRL, I2C_MST_CLOCK_400KHZ); - - return true; -} - bool AP_Compass_AK8963::_configure() { - _register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | _magnetometer_adc_resolution); + _bus->register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | _magnetometer_adc_resolution); return true; } bool AP_Compass_AK8963::_reset() { - _register_write(AK8963_CNTL2, AK8963_RESET); - + _bus->register_write(AK8963_CNTL2, AK8963_RESET); return true; } bool AP_Compass_AK8963::_calibrate() { - uint8_t cntl1 = _register_read(AK8963_CNTL1); + uint8_t cntl1 = _bus->register_read(AK8963_CNTL1); - _register_write(AK8963_CNTL1, AK8963_FUSE_MODE | _magnetometer_adc_resolution); /* Enable FUSE-mode in order to be able to read calibreation data */ + _bus->register_write(AK8963_CNTL1, AK8963_FUSE_MODE | _magnetometer_adc_resolution); /* Enable FUSE-mode in order to be able to read calibreation data */ uint8_t response[3]; - _register_read(AK8963_ASAX, response, 3); + _bus->register_read(AK8963_ASAX, response, 3); for (int i = 0; i < 3; i++) { float data = response[i]; @@ -318,21 +279,7 @@ bool AP_Compass_AK8963::_calibrate() error("%d: %lf\n", i, _magnetometer_ASA[i]); } - _register_write(AK8963_CNTL1, cntl1); - - return true; -} - -bool AP_Compass_AK8963::_start_conversion() -{ - static const uint8_t address = AK8963_INFO; - /* Read registers from INFO through ST2 */ - static const uint8_t count = 0x09; - - _configure_mpu9250(); - _bus_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG); /* Set the I2C slave addres of AK8963 and set for read. */ - _bus_write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */ - _bus_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */ + _bus->register_write(AK8963_CNTL1, cntl1); return true; } @@ -343,7 +290,7 @@ bool AP_Compass_AK8963::_collect_samples() return false; } - if (!_read_raw()) { + if (!_bus->read_raw(_mag_x, _mag_y, _mag_z)) { return false; } else { _mag_x_accum += _mag_x; @@ -363,27 +310,27 @@ bool AP_Compass_AK8963::_collect_samples() bool AP_Compass_AK8963::_sem_take_blocking() { - return _spi_sem->take(10); + return _bus_sem->take(10); } bool AP_Compass_AK8963::_sem_give() { - return _spi_sem->give(); + return _bus_sem->give(); } bool AP_Compass_AK8963::_sem_take_nonblocking() { static int _sem_failure_count = 0; - bool got = _spi_sem->take_nonblocking(); + bool got = _bus_sem->take_nonblocking(); if (!got) { if (!hal.scheduler->system_initializing()) { _sem_failure_count++; if (_sem_failure_count > 100) { - hal.scheduler->panic(PSTR("PANIC: failed to take _spi_sem " + hal.scheduler->panic(PSTR("PANIC: failed to take _bus->sem " "100 times in a row, in " - "AP_Compass_AK8963::_update")); + "AP_Compass_AK8963")); } } return false; /* never reached */ @@ -412,51 +359,35 @@ void AP_Compass_AK8963::_dump_registers() #endif } -bool AP_Compass_AK8963::_read_raw() +AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250() { - uint8_t rx[14] = {0}; - - const uint8_t count = 9; - _bus_read(MPUREG_EXT_SENS_DATA_00, rx, count); - - uint8_t st2 = rx[8]; /* End data read by reading ST2 register */ - -#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx])) - - if(!(st2 & 0x08)) { - _mag_x = (float) int16_val(rx, 1); - _mag_y = (float) int16_val(rx, 2); - _mag_z = (float) int16_val(rx, 3); - - if (is_zero(_mag_x) && is_zero(_mag_y) && is_zero(_mag_z)) { - return false; - } + _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); - return true; - } else { - return false; + if (_spi == NULL) { + hal.console->println_P(PSTR("Cannot get SPIDevice_MPU9250")); + return; } - } -void AP_Compass_AK8963::_register_write(uint8_t address, uint8_t value) + +void AP_AK8963_SerialBus_MPU9250::register_write(uint8_t address, uint8_t value) { - _bus_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR); /* Set the I2C slave addres of AK8963 and set for _register_write. */ - _bus_write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */ - _bus_write(MPUREG_I2C_SLV0_DO, value); /* Register value to continuous measurement in 16-bit */ - _bus_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | 0x01); /* Enable I2C and set 1 byte */ + _write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR); /* Set the I2C slave addres of AK8963 and set for register_write. */ + _write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */ + _write(MPUREG_I2C_SLV0_DO, value); /* Register value to continuous measurement in 16-bit */ + _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | 0x01); /* Enable I2C and set 1 byte */ } -void AP_Compass_AK8963::_register_read(uint8_t address, uint8_t *value, uint8_t count) +void AP_AK8963_SerialBus_MPU9250::register_read(uint8_t address, uint8_t *value, uint8_t count) { - _bus_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG); /* Set the I2C slave addres of AK8963 and set for read. */ - _bus_write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */ - _bus_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */ + _write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG); /* Set the I2C slave addres of AK8963 and set for read. */ + _write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */ + _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */ hal.scheduler->delay(10); - _bus_read(MPUREG_EXT_SENS_DATA_00, value, count); + _read(MPUREG_EXT_SENS_DATA_00, value, count); } -void AP_Compass_AK8963::_bus_read(uint8_t address, uint8_t *buf, uint32_t count) +void AP_AK8963_SerialBus_MPU9250::_read(uint8_t address, uint8_t *buf, uint32_t count) { ASSERT(count < 150); uint8_t tx[150]; @@ -469,7 +400,7 @@ void AP_Compass_AK8963::_bus_read(uint8_t address, uint8_t *buf, uint32_t count) memcpy(buf, rx + 1, count); } -void AP_Compass_AK8963::_bus_write(uint8_t address, const uint8_t *buf, uint32_t count) +void AP_AK8963_SerialBus_MPU9250::_write(uint8_t address, const uint8_t *buf, uint32_t count) { ASSERT(count < 2); uint8_t tx[2]; @@ -479,4 +410,62 @@ void AP_Compass_AK8963::_bus_write(uint8_t address, const uint8_t *buf, uint32_t _spi->transaction(tx, NULL, count + 1); } + +bool AP_AK8963_SerialBus_MPU9250::configure() +{ + if (!AP_InertialSensor_MPU9250::initialize_driver_state()) + 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; +} + +bool AP_AK8963_SerialBus_MPU9250::read_raw(float &mag_x, float &mag_y, float &mag_z) +{ + uint8_t rx[14] = {0}; + + const uint8_t count = 9; + _read(MPUREG_EXT_SENS_DATA_00, rx, count); + + uint8_t st2 = rx[8]; /* End data read by reading ST2 register */ + +#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx])) + + if(!(st2 & 0x08)) { + mag_x = (float) int16_val(rx, 1); + mag_y = (float) int16_val(rx, 2); + mag_z = (float) int16_val(rx, 3); + + if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) { + return false; + } + + return true; + } else { + return false; + } +} + +AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore() +{ + return _spi->get_semaphore(); +} + +bool AP_AK8963_SerialBus_MPU9250::start_conversion() +{ + static const uint8_t address = AK8963_INFO; + /* Read registers from INFO through ST2 */ + static const uint8_t count = 0x09; + + configure(); + _write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG); /* Set the I2C slave addres of AK8963 and set for read. */ + _write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */ + _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */ + + return true; +} #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 9b4a3f0ffc..16ecb5d57f 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -9,10 +9,26 @@ #include "Compass.h" #include "AP_Compass_Backend.h" +class AP_AK8963_SerialBus +{ +public: + virtual void register_read(uint8_t address, uint8_t *value, uint8_t count) = 0; + uint8_t register_read(uint8_t address) { + uint8_t reg; + register_read(address, ®, 1); + return reg; + } + virtual void register_write(uint8_t address, uint8_t value) = 0; + virtual AP_HAL::Semaphore* get_semaphore() = 0; + virtual bool start_conversion() = 0; + virtual bool configure() = 0; + virtual bool read_raw(float &mag_x, float &mag_y, float &mag_z) = 0; +}; + class AP_Compass_AK8963 : public AP_Compass_Backend { public: - AP_Compass_AK8963(Compass &compass); + AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus); static AP_Compass_Backend *detect(Compass &compass); @@ -29,38 +45,16 @@ private: STATE_ERROR } state_t; - bool _read_raw(); - bool _reset(); bool _configure(); bool _check_id(); bool _calibrate(); void _update(); - bool _start_conversion(); bool _collect_samples(); void _dump_registers(); - - bool _configure_mpu9250(); - void _bus_read(uint8_t address, uint8_t *value, uint32_t count); - void _bus_write(uint8_t address, const uint8_t *value, uint32_t count); - - void _bus_write(uint8_t address, const uint8_t value) { - _bus_write(address, &value, 1); - } - - void _register_read(uint8_t address, uint8_t *value, uint8_t count); - - uint8_t _register_read(uint8_t address) { - uint8_t reg; - _register_read(address, ®, 1); - return reg; - } - - void _register_write(uint8_t address, uint8_t value); - - bool _sem_take_nonblocking(); bool _sem_take_blocking(); + bool _sem_take_nonblocking(); bool _sem_give(); state_t _state; @@ -80,6 +74,29 @@ private: uint8_t _magnetometer_adc_resolution; uint32_t _last_update_timestamp; uint32_t _last_accum_time; + + AP_AK8963_SerialBus *_bus; + AP_HAL::Semaphore *_bus_sem; +}; + +class AP_AK8963_SerialBus_MPU9250: public AP_AK8963_SerialBus +{ +public: + AP_AK8963_SerialBus_MPU9250(); + void register_read(uint8_t address, uint8_t *value, uint8_t count); + void register_write(uint8_t address, uint8_t value); + AP_HAL::Semaphore* get_semaphore(); + bool start_conversion(); + bool configure(); + bool read_raw(float &mag_x, float &mag_y, float &mag_z); +private: + void _read(uint8_t address, uint8_t *value, uint32_t count); + void _write(uint8_t address, const uint8_t *value, uint32_t count); + void _write(uint8_t address, const uint8_t value) { + _write(address, &value, 1); + } + AP_HAL::SPIDeviceDriver *_spi; + AP_HAL::Semaphore *_spi_sem; }; #endif diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 9bec08e532..813f29ac6f 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -351,7 +351,8 @@ Compass::_detect_backends(void) #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE _add_backend(AP_Compass_HMC5843::detect); - _add_backend(AP_Compass_AK8963::detect); + _backends[_backend_count++] = new AP_Compass_AK8963(*this, + new AP_AK8963_SerialBus_MPU9250()); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL _add_backend(AP_Compass_HIL::detect); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843