From 7a417d11512d26b5963d056f314bd9c026315577 Mon Sep 17 00:00:00 2001 From: Staroselskii Georgii Date: Thu, 2 Jul 2015 16:37:37 +0300 Subject: [PATCH] AP_Compass: AK8963 rework Got rid of extra abstraction layer. There is no need for that now. --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 661 +++++++++------------ libraries/AP_Compass/AP_Compass_AK8963.h | 131 ++-- 2 files changed, 323 insertions(+), 469 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index d95d549392..8bf3a0ddfa 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -18,7 +18,7 @@ * AP_Compass_AK8963.cpp * Code by Georgii Staroselskii. Emlid.com * - * Sensor is conected to SPI port + * Sensor is connected to SPI port * */ @@ -33,8 +33,6 @@ #define MPUREG_EXT_SENS_DATA_00 0x49 #define MPUREG_I2C_SLV0_DO 0x63 -#define MPU9250_SPI_BACKEND 1 - #define MPUREG_PWR_MGMT_1 0x6B # define BIT_PWR_MGMT_1_CLK_INTERNAL 0x00 // clock set to internal 8Mhz oscillator # define BIT_PWR_MGMT_1_CLK_XGYRO 0x01 // PLL with X axis gyroscope reference @@ -57,6 +55,7 @@ #define MPUREG_I2C_MST_CTRL 0x24 # define I2C_SLV0_EN 0x80 # define I2C_MST_CLOCK_400KHZ 0x0D +# define I2C_MST_CLOCK_258KHZ 0x08 #define AK8963_I2C_ADDR 0x0c @@ -90,41 +89,266 @@ #define AK8963_ASAX 0x10 #define AK8963_DEBUG 0 -#define AK8963_SELFTEST 0 #if AK8963_DEBUG -#define error(...) fprintf(stderr, __VA_ARGS__) -#define debug(...) hal.console->printf(__VA_ARGS__) +#include +#define error(...) do { fprintf(stderr, __VA_ARGS__); } while (0) #define ASSERT(x) assert(x) #else -#define error(...) -#define debug(...) +#define error(...) do { } while (0) +#ifndef ASSERT #define ASSERT(x) #endif +#endif extern const AP_HAL::HAL& hal; -AK8963_MPU9250_SPI_Backend::AK8963_MPU9250_SPI_Backend() +AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass) : + AP_Compass_Backend(compass), + _state(STATE_UNKNOWN), + _initialized(false), + _last_update_timestamp(0), + _last_accum_time(0) +{ + _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; +} + +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) { } -bool AK8963_MPU9250_SPI_Backend::sem_take_blocking() +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(); + + if (!_configure_mpu9250()) { + hal.console->printf_P(PSTR("AK8963: MPU9250 not configured for AK8963\n")); + return false; + } + + if (!_configure()) { + hal.console->printf_P(PSTR("AK8963: not configured\n")); + return false; + } + + if (!_check_id()) { + hal.console->printf_P(PSTR("AK8963: wrong id\n")); + return false; + } + + if (!_calibrate()) { + hal.console->printf_P(PSTR("AK8963: not calibrated\n")); + return false; + } + + if (!_start_conversion()) { + hal.console->printf_P(PSTR("AK8963: conversion not started\n")); + return false; + } + + + _state = STATE_SAMPLE; + _initialized = true; + + hal.scheduler->suspend_timer_procs(); + /* register the compass instance in the frontend */ + _compass_instance = register_compass(); + + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void)); + hal.scheduler->resume_timer_procs(); + + return true; +} + +void AP_Compass_AK8963::read() +{ + if (!_initialized) { + return; + } + + if (_accum_count == 0) { + /* We're not ready to publish*/ + return; + } + + /* Update */ + Vector3f field(_mag_x_accum * _magnetometer_ASA[0], + _mag_y_accum * _magnetometer_ASA[1], + _mag_z_accum * _magnetometer_ASA[2]); + + field /= _accum_count; + _mag_x_accum = _mag_y_accum = _mag_z_accum = 0; + _accum_count = 0; + + publish_field(field, _compass_instance); +} + +void AP_Compass_AK8963::_update() +{ + if (hal.scheduler->micros() - _last_update_timestamp < 10000) { + return; + } + + if (!_sem_take_nonblocking()) { + return; + } + + switch (_state) + { + case STATE_SAMPLE: + if (!_collect_samples()) { + _state = STATE_ERROR; + } + break; + case STATE_ERROR: + if (_start_conversion()) { + _state = STATE_SAMPLE; + } + break; + default: + break; + } + + _last_update_timestamp = hal.scheduler->micros(); + _sem_give(); +} + +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 */ + + if (deviceid == AK8963_Device_ID) { + return true; + } + } + + return false; +} + +bool AP_Compass_AK8963::_configure_mpu9250() +{ + _bus_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS | 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); + return true; +} + +bool AP_Compass_AK8963::_reset() +{ + _register_write(AK8963_CNTL2, AK8963_RESET); + + return true; +} + + +bool AP_Compass_AK8963::_calibrate() +{ + uint8_t cntl1 = _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 */ + + uint8_t response[3]; + _register_read(AK8963_ASAX, response, 3); + + for (int i = 0; i < 3; i++) { + float data = response[i]; + _magnetometer_ASA[i] = ((data - 128) / 256 + 1); + 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 */ + + return true; +} + +bool AP_Compass_AK8963::_collect_samples() +{ + if (!_initialized) { + return false; + } + + if (!_read_raw()) { + return false; + } else { + _mag_x_accum += _mag_x; + _mag_y_accum += _mag_y; + _mag_z_accum += _mag_z; + _accum_count++; + if (_accum_count == 10) { + _mag_x_accum /= 2; + _mag_y_accum /= 2; + _mag_z_accum /= 2; + _accum_count = 5; + } + } + + return true; +} + +bool AP_Compass_AK8963::_sem_take_blocking() { return _spi_sem->take(10); } -bool AK8963_MPU9250_SPI_Backend::sem_give() +bool AP_Compass_AK8963::_sem_give() { return _spi_sem->give(); } -bool AK8963_MPU9250_SPI_Backend::sem_take_nonblocking() +bool AP_Compass_AK8963::_sem_take_nonblocking() { - /** - * Take nonblocking from a TimerProcess context & - * monitor for bad failures - */ static int _sem_failure_count = 0; + bool got = _spi_sem->take_nonblocking(); + if (!got) { if (!hal.scheduler->system_initializing()) { _sem_failure_count++; @@ -141,75 +365,17 @@ bool AK8963_MPU9250_SPI_Backend::sem_take_nonblocking() return got; } -bool AK8963_MPU9250_SPI_Backend::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(); - - // start at low speed for - // initialisation. AP_InertialSensor_MPU9250 driver will raise - // speed - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - - return true; -} - -void AK8963_MPU9250_SPI_Backend::read(uint8_t address, uint8_t *buf, uint32_t count) -{ - ASSERT(count < 10); - uint8_t tx[11]; - uint8_t rx[11]; - - tx[0] = address | READ_FLAG; - tx[1] = 0; - _spi->transaction(tx, rx, count + 1); - - memcpy(buf, rx + 1, count); -} - -void AK8963_MPU9250_SPI_Backend::write(uint8_t address, const uint8_t *buf, uint32_t count) -{ - ASSERT(count < 2); - uint8_t tx[2]; - - tx[0] = address; - memcpy(tx+1, buf, count); - - _spi->transaction(tx, NULL, count + 1); -} - -AP_Compass_AK8963_MPU9250::AP_Compass_AK8963_MPU9250(Compass &compass): - AP_Compass_AK8963(compass) -{ -} - -// detect the sensor -AP_Compass_Backend *AP_Compass_AK8963_MPU9250::detect(Compass &compass) -{ - AP_Compass_AK8963_MPU9250 *sensor = new AP_Compass_AK8963_MPU9250(compass); - if (sensor == NULL) { - return NULL; - } - if (!sensor->init()) { - delete sensor; - return NULL; - } - return sensor; -} - -void AP_Compass_AK8963_MPU9250::_dump_registers() +void AP_Compass_AK8963::_dump_registers() { #if AK8963_DEBUG - error(PSTR("MPU9250 registers\n")); + error("MPU9250 registers\n"); + static uint8_t regs[0x7e]; + + _bus_read(0x0, regs, 0x7e); + for (uint8_t reg=0x00; reg<=0x7E; reg++) { - uint8_t v = _backend->read(reg); - error(("%02x:%02x "), (unsigned)reg, (unsigned)v); + uint8_t v = regs[reg]; + error(("%d:%02x "), (unsigned)reg, (unsigned)v); if (reg % 16 == 0) { error("\n"); } @@ -218,74 +384,18 @@ void AP_Compass_AK8963_MPU9250::_dump_registers() #endif } -void AP_Compass_AK8963_MPU9250::_backend_reset() -{ - _backend->write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); -} - -bool AP_Compass_AK8963_MPU9250::_backend_init() -{ - _backend->write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_MST_EN); /* I2C Master mode */ - _backend->write(MPUREG_I2C_MST_CTRL, I2C_MST_CLOCK_400KHZ); /* I2C configuration multi-master IIC 400KHz */ - - return true; -} - -bool AP_Compass_AK8963_MPU9250::init() -{ -#if MPU9250_SPI_BACKEND - _backend = new AK8963_MPU9250_SPI_Backend(); - if (_backend == NULL) { - hal.scheduler->panic(PSTR("_backend coudln't be allocated")); - } - if (!_backend->init()) { - delete _backend; - _backend = NULL; - return false; - } - return AP_Compass_AK8963::init(); -#else -#error Wrong backend for AK8963 is selected - /* other backends not implented yet */ - return false; -#endif -} - -void AP_Compass_AK8963_MPU9250::_register_write(uint8_t address, uint8_t value) -{ - _backend->write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR); /* Set the I2C slave addres of AK8963 and set for _register_write. */ - _backend->write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */ - _backend->write(MPUREG_I2C_SLV0_DO, value); /* Register value to continuous measurement in 16-bit */ - _backend->write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | 0x01); /* Enable I2C and set 1 byte */ -} - -void AP_Compass_AK8963_MPU9250::_register_read(uint8_t address, uint8_t count, uint8_t *value) -{ - _backend->write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG); /* Set the I2C slave addres of AK8963 and set for read. */ - _backend->write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */ - _backend->write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */ - - hal.scheduler->delay(10); - _backend->read(MPUREG_EXT_SENS_DATA_00, value, count); -} - -uint8_t AP_Compass_AK8963_MPU9250::_read_id() -{ - return 1; -} - -bool AP_Compass_AK8963_MPU9250::read_raw() +bool AP_Compass_AK8963::_read_raw() { uint8_t rx[14] = {0}; const uint8_t count = 9; - _backend->read(MPUREG_EXT_SENS_DATA_00, rx, count); + _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)) { + if(!(st2 & 0x08)) { _mag_x = (float) int16_val(rx, 1); _mag_y = (float) int16_val(rx, 2); _mag_z = (float) int16_val(rx, 3); @@ -300,263 +410,44 @@ bool AP_Compass_AK8963_MPU9250::read_raw() } } - -AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass) : - AP_Compass_Backend(compass), - _backend(NULL), - _initialised(false), - _state(STATE_CONVERSION), - _last_update_timestamp(0), - _last_accum_time(0) +void AP_Compass_AK8963::_register_write(uint8_t address, uint8_t value) { - _initialised = false; - _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; + _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 */ } - -/* stub to satisfy Compass API*/ -void AP_Compass_AK8963::accumulate(void) +void AP_Compass_AK8963::_register_read(uint8_t address, uint8_t *value, uint8_t count) { -} - -bool AP_Compass_AK8963::_self_test() -{ - bool success = false; - - /* see AK8963.pdf p.19 */ - - /* Set power-down mode */ - _register_write(AK8963_CNTL1, AK8963_POWERDOWN_MODE | _magnetometer_adc_resolution); - - /* Turn the internal magnetic field on */ - _register_write(AK8963_ASTC, AK8983_SELFTEST_MAGNETIC_FIELD_ON); - - /* Register value to self-test mode in 14-bit */ - _register_write(AK8963_CNTL1, AK8963_SELFTEST_MODE | _magnetometer_adc_resolution); - - _start_conversion(); - hal.scheduler->delay(20); - read_raw(); - - float hx = _mag_x; - float hy = _mag_y; - float hz = _mag_z; - - error("AK8963's SELF-TEST STARTED\n"); - - switch (_magnetometer_adc_resolution) { - bool hx_is_in_range; - bool hy_is_in_range; - bool hz_is_in_range; - case AK8963_14BIT_ADC: - hx_is_in_range = (hx >= - 50) && (hx <= 50); - hy_is_in_range = (hy >= - 50) && (hy <= 50); - hz_is_in_range = (hz >= - 800) && (hz <= -200); - if (hx_is_in_range && hy_is_in_range && hz_is_in_range) { - success = true; - } - break; - case AK8963_16BIT_ADC: - hx_is_in_range = (hx >= -200) && (hx <= 200); - hy_is_in_range = (hy >= -200) && (hy <= 200); - hz_is_in_range = (hz >= -3200) && (hz <= -800); - if (hx_is_in_range && hy_is_in_range && hz_is_in_range) { - success = true; - } - break; - default: - success = false; - hal.scheduler->panic(PSTR("Wrong AK8963's ADC resolution selected")); - break; - } - - error("AK8963's SELF-TEST ENDED: %f %f %f\n", hx, hy, hz); - - /* Turn the internal magnetic field off */ - _register_write(AK8963_ASTC, 0x0); - - /* Register value to continuous measurement in 14-bit */ - _register_write(AK8963_CNTL1, AK8963_POWERDOWN_MODE | _magnetometer_adc_resolution); - - return success; -} - -bool AP_Compass_AK8963::init() -{ - hal.scheduler->suspend_timer_procs(); - if (!_backend->sem_take_blocking()) { - error("_spi_sem->take failed\n"); - return false; - } - - - if (!_backend_init()) { - _backend->sem_give(); - return false; - } - - _register_write(AK8963_CNTL2, AK8963_RESET); /* Reset AK8963 */ - - hal.scheduler->delay(1000); - - int id_mismatch_count; - uint8_t deviceid; - for (id_mismatch_count = 0; id_mismatch_count < 5; id_mismatch_count++) { - _register_read(AK8963_WIA, 0x01, &deviceid); /* Read AK8963's id */ - - if (deviceid == AK8963_Device_ID) { - break; - } - - error("trying to read AK8963's ID once more...\n"); - _backend_reset(); - hal.scheduler->delay(100); - _dump_registers(); - } - - if (id_mismatch_count == 5) { - _initialised = false; - hal.console->printf("WRONG AK8963 DEVICE ID: 0x%x\n", (unsigned)deviceid); - hal.scheduler->panic(PSTR("AK8963: bad DEVICE ID")); - } - - _calibrate(); - - _initialised = true; - -#if AK8963_SELFTEST - if (_self_test()) { - _initialised = true; - } else { - _initialised = false; - } -#endif - - /* Register value to continuous measurement */ - _register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | _magnetometer_adc_resolution); - - _backend->sem_give(); - - // register the compass instance in the frontend - _compass_instance = register_compass(); - - hal.scheduler->resume_timer_procs(); - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void)); - - _start_conversion(); - - _initialised = true; - return _initialised; -} - -void AP_Compass_AK8963::_update() -{ - if (hal.scheduler->micros() - _last_update_timestamp < 10000) { - return; - } - - if (!_backend->sem_take_nonblocking()) { - return; - } - - switch (_state) - { - case STATE_CONVERSION: - _start_conversion(); - _state = STATE_SAMPLE; - break; - case STATE_SAMPLE: - _collect_samples(); - _state = STATE_CONVERSION; - break; - case STATE_ERROR: - break; - default: - break; - } - - _last_update_timestamp = hal.scheduler->micros(); - _backend->sem_give(); -} - -bool AP_Compass_AK8963::_calibrate() -{ - error("CALIBRATTION START\n"); - _register_write(AK8963_CNTL1, AK8963_FUSE_MODE | _magnetometer_adc_resolution); /* Enable FUSE-mode in order to be able to read calibreation data */ + _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 */ hal.scheduler->delay(10); - - uint8_t response[3]; - _register_read(AK8963_ASAX, 0x03, response); - - for (int i = 0; i < 3; i++) { - float data = response[i]; - magnetometer_ASA[i] = ((data-128)/256+1); - error("%d: %lf\n", i, magnetometer_ASA[i]); - } - - error("CALIBRATTION END\n"); - - return true; + _bus_read(MPUREG_EXT_SENS_DATA_00, value, count); } -void AP_Compass_AK8963::read() +void AP_Compass_AK8963::_bus_read(uint8_t address, uint8_t *buf, uint32_t count) { - if (!_initialised) { - return; - } + ASSERT(count < 150); + uint8_t tx[150]; + uint8_t rx[150]; - if (_accum_count == 0) { - /* We're not ready to publish*/ - return; - } + tx[0] = address | READ_FLAG; + tx[1] = 0; + _spi->transaction(tx, rx, count + 1); - /* Update */ - Vector3f field(_mag_x_accum * magnetometer_ASA[0], - _mag_y_accum * magnetometer_ASA[1], - _mag_z_accum * magnetometer_ASA[2]); - - field /= _accum_count; - _mag_x_accum = _mag_y_accum = _mag_z_accum = 0; - _accum_count = 0; - - publish_field(field, _compass_instance); + memcpy(buf, rx + 1, count); } -void AP_Compass_AK8963::_start_conversion() +void AP_Compass_AK8963::_bus_write(uint8_t address, const uint8_t *buf, uint32_t count) { - static const uint8_t address = AK8963_INFO; - /* Read registers from INFO through ST2 */ - static const uint8_t count = 0x09; + ASSERT(count < 2); + uint8_t tx[2]; - _backend_init(); - _backend->write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_MST_EN); /* I2C Master mode */ - _backend->write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG); /* Set the I2C slave addres of AK8963 and set for read. */ - _backend->write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */ - _backend->write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */ -} - -void AP_Compass_AK8963::_collect_samples() -{ - if (!_initialised) { - return; - } - - if (!read_raw()) { - error("read_raw failed\n"); - } else { - _mag_x_accum += _mag_x; - _mag_y_accum += _mag_y; - _mag_z_accum += _mag_z; - _accum_count++; - if (_accum_count == 10) { - _mag_x_accum /= 2; - _mag_y_accum /= 2; - _mag_z_accum /= 2; - _accum_count = 5; - } - } + tx[0] = address; + memcpy(tx+1, buf, count); + + _spi->transaction(tx, NULL, count + 1); } diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index c5c5a2aa4e..033b8b580f 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -9,117 +9,80 @@ #include "Compass.h" #include "AP_Compass_Backend.h" -class AK8963_Backend -{ - public: - virtual ~AK8963_Backend() {} - virtual void read(uint8_t address, uint8_t *buf, uint32_t count) = 0; - virtual void write(uint8_t address, const uint8_t *buf, uint32_t count) = 0; - virtual bool sem_take_nonblocking() = 0; - virtual bool sem_take_blocking() = 0; - virtual bool sem_give() = 0; - virtual bool init() = 0; - virtual uint8_t read(uint8_t address) - { - uint8_t value; - read(address, &value, 1); - return value; - } - - virtual void write(uint8_t address, uint8_t value) - { - write(address, &value, 1); - } -}; - class AP_Compass_AK8963 : public AP_Compass_Backend { public: AP_Compass_AK8963(Compass &compass); + static AP_Compass_Backend *detect(Compass &compass); + bool init(void); void read(void); void accumulate(void); -protected: - AK8963_Backend *_backend; // Not to be confused with Compass (frontend) "_backends" attribute. - float magnetometer_ASA[3]; - float _mag_x; - float _mag_y; - float _mag_z; - uint8_t _compass_instance; - - virtual bool read_raw() = 0; - private: - typedef enum + typedef enum { + STATE_UNKNOWN, STATE_CONVERSION, STATE_SAMPLE, STATE_ERROR } state_t; - virtual bool _backend_init() = 0; - virtual void _register_read(uint8_t address, uint8_t count, uint8_t *value) = 0; - virtual void _register_write(uint8_t address, uint8_t value) = 0; - virtual void _backend_reset() = 0; - virtual uint8_t _read_id() = 0; - virtual void _dump_registers() {} + bool _read_raw(); - bool _register_read(uint8_t address, uint8_t *value); - bool _calibrate(); - bool _self_test(); - void _update(); - void _start_conversion(); - void _collect_samples(); + 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_give(); + + state_t _state; + + float _magnetometer_ASA[3] {0, 0, 0}; + float _mag_x; + float _mag_y; + float _mag_z; + uint8_t _compass_instance; float _mag_x_accum; float _mag_y_accum; float _mag_z_accum; uint32_t _accum_count; - bool _initialised; - state_t _state; + bool _initialized; uint8_t _magnetometer_adc_resolution; uint32_t _last_update_timestamp; uint32_t _last_accum_time; -}; -class AK8963_MPU9250_SPI_Backend: public AK8963_Backend -{ - public: - AK8963_MPU9250_SPI_Backend(); - void read(uint8_t address, uint8_t *buf, uint32_t count); - void write(uint8_t address, const uint8_t *buf, uint32_t count); - bool sem_take_nonblocking(); - bool sem_take_blocking(); - bool sem_give(); - bool init() ; - ~AK8963_MPU9250_SPI_Backend() {} - - private: - AP_HAL::SPIDeviceDriver *_spi; - AP_HAL::Semaphore *_spi_sem; -}; - -class AP_Compass_AK8963_MPU9250: public AP_Compass_AK8963 -{ - public: - AP_Compass_AK8963_MPU9250(Compass &compass); - ~AP_Compass_AK8963_MPU9250() {} - bool init(); - - // detect the sensor - static AP_Compass_Backend *detect(Compass &compass); - - private: - bool _backend_init(); - void _backend_reset(); - void _register_read(uint8_t address, uint8_t count, uint8_t *value); - void _register_write(uint8_t address, uint8_t value); - void _dump_registers(); - bool read_raw(); - uint8_t _read_id(); + AP_HAL::Semaphore *_spi_sem; + AP_HAL::SPIDeviceDriver *_spi; }; #endif