mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 06:38:29 -04:00
AP_Compass: AK8963 rework
Got rid of extra abstraction layer. There is no need for that now.
This commit is contained in:
Staroselskii Georgii
Andrew Tridgell
parent
f82344358f
commit
7a417d1151
@ -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 <stdio.h>
|
||||
#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,68 +384,12 @@ 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 */
|
||||
|
||||
@ -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]);
|
||||
_bus_read(MPUREG_EXT_SENS_DATA_00, value, count);
|
||||
}
|
||||
|
||||
error("CALIBRATTION END\n");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
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];
|
||||
|
||||
tx[0] = address | READ_FLAG;
|
||||
tx[1] = 0;
|
||||
_spi->transaction(tx, rx, count + 1);
|
||||
|
||||
memcpy(buf, rx + 1, count);
|
||||
}
|
||||
|
||||
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::_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 */
|
||||
}
|
||||
tx[0] = address;
|
||||
memcpy(tx+1, buf, count);
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
_spi->transaction(tx, NULL, count + 1);
|
||||
}
|
||||
|
||||
@ -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
|
||||
{
|
||||
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 _reset();
|
||||
bool _configure();
|
||||
bool _check_id();
|
||||
bool _calibrate();
|
||||
bool _self_test();
|
||||
|
||||
void _update();
|
||||
void _start_conversion();
|
||||
void _collect_samples();
|
||||
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::SPIDeviceDriver *_spi;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user