Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: AK8963: use AP_HAL::I2CDevice abstraction 

We still need the BusDriver (in some places called SerialBus) interface
since this driver can also be used on an AuxiliaryBus and that has a
different interface.
This commit is contained in:
Lucas De Marchi 2016-03-10 02:42:11 -03:00
parent 41c1209169
commit d3831dbb98
3 changed files with 244 additions and 266 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

331
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -13,6 +13,8 @@
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <assert.h>
#include <utility>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
@ -44,97 +46,91 @@
#define AK8963_ASAX 0x10 #define AK8963_ASAX 0x10
#define AK8963_DEBUG 0
#if AK8963_DEBUG
#include <stdio.h>
#define error(...) do { fprintf(stderr, __VA_ARGS__); } while (0)
#define ASSERT(x) assert(x)
#else
#define error(...) do { } while (0)
#ifndef ASSERT
#define ASSERT(x)
#endif
#endif
#define AK8963_MILLIGAUSS_SCALE 10.0f #define AK8963_MILLIGAUSS_SCALE 10.0f
extern const AP_HAL::HAL& hal; struct PACKED sample_regs {
int16_t val[3];
uint8_t st2;
};
AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus) : extern const AP_HAL::HAL &hal;
AP_Compass_Backend(compass),
_initialized(false), AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
_last_update_timestamp(0), uint32_t dev_id)
_last_accum_time(0), : AP_Compass_Backend(compass)
_bus(bus) , _bus(bus)
, _dev_id(dev_id)
{ {
_reset_filter(); _reset_filter();
} }
AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250(Compass &compass, uint8_t mpu9250_instance)
{
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);
}
AP_Compass_Backend *AP_Compass_AK8963::detect_i2c(Compass &compass,
AP_HAL::I2CDriver *i2c,
uint8_t addr)
{
AP_AK8963_SerialBus *bus = new AP_AK8963_SerialBus_I2C(i2c, addr);
if (!bus)
return nullptr;
return _detect(compass, bus);
}
AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250_i2c(Compass &compass,
AP_HAL::I2CDriver *i2c,
uint8_t addr)
{
AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
ins.detect_backends();
return detect_i2c(compass, i2c, addr);
}
AP_Compass_Backend *AP_Compass_AK8963::_detect(Compass &compass,
AP_AK8963_SerialBus *bus)
{
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus);
if (sensor == nullptr) {
delete bus;
return nullptr;
}
if (!sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
AP_Compass_AK8963::~AP_Compass_AK8963() AP_Compass_AK8963::~AP_Compass_AK8963()
{ {
delete _bus; delete _bus;
} }
/* stub to satisfy Compass API*/ AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass,
void AP_Compass_AK8963::accumulate(void) AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{ {
AP_AK8963_BusDriver *bus = new AP_AK8963_BusDriver_HALDevice(std::move(dev));
if (!bus) {
return nullptr;
}
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, AP_COMPASS_TYPE_AK8963_I2C);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
/* Allow MPU9250 to shortcut auxiliary bus and host bus */
ins.detect_backends();
return probe(compass, std::move(dev));
}
AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t mpu9250_instance)
{
AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
AP_AK8963_BusDriver *bus =
new AP_AK8963_BusDriver_Auxiliary(ins, HAL_INS_MPU9250_SPI, mpu9250_instance, AK8963_I2C_ADDR);
if (!bus) {
return nullptr;
}
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, AP_COMPASS_TYPE_AK8963_MPU9250);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
} }
bool AP_Compass_AK8963::init() bool AP_Compass_AK8963::init()
{ {
_bus_sem = _bus->get_semaphore();
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
if (!_bus_sem->take(100)) { if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("AK8963: Unable to get bus semaphore"); hal.console->printf("AK8963: Unable to get bus semaphore\n");
goto fail_sem; goto fail_sem;
} }
if (!_bus->configure()) {
hal.console->printf("AK8963: Could not configure the bus\n");
goto fail;
}
if (!_check_id()) { if (!_check_id()) {
hal.console->printf("AK8963: Wrong id\n"); hal.console->printf("AK8963: Wrong id\n");
goto fail; goto fail;
@ -159,17 +155,18 @@ bool AP_Compass_AK8963::init()
/* register the compass instance in the frontend */ /* register the compass instance in the frontend */
_compass_instance = register_compass(); _compass_instance = register_compass();
set_dev_id(_compass_instance, _bus->get_dev_id()); set_dev_id(_compass_instance, _dev_id);
/* timer needs to be called every 10ms so set the freq_div to 10 */ /* timer needs to be called every 10ms so set the freq_div to 10 */
_timesliced = hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void), 10); _timesliced = hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void), 10);
_bus_sem->give(); bus_sem->give();
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
return true; return true;
fail: fail:
_bus_sem->give(); bus_sem->give();
fail_sem: fail_sem:
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
@ -225,9 +222,7 @@ void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) co
void AP_Compass_AK8963::_update() void AP_Compass_AK8963::_update()
{ {
struct AP_AK8963_SerialBus::raw_value rv; struct sample_regs regs;
float mag_x, mag_y, mag_z;
// get raw_field - sensor frame, uncorrected
Vector3f raw_field; Vector3f raw_field;
uint32_t time_us = AP_HAL::micros(); uint32_t time_us = AP_HAL::micros();
@ -236,28 +231,26 @@ void AP_Compass_AK8963::_update()
goto end; goto end;
} }
if (!_bus_sem->take_nonblocking()) { if (!_bus->get_semaphore()->take_nonblocking()) {
goto end; goto end;
} }
_bus->read_raw(&rv); if (!_bus->block_read(AK8963_HXL, (uint8_t *) &regs, sizeof(regs))) {
goto fail;
}
/* Check for overflow. See AK8963's datasheet, section /* Check for overflow. See AK8963's datasheet, section
* 6.4.3.6 - Magnetic Sensor Overflow. */ * 6.4.3.6 - Magnetic Sensor Overflow. */
if ((rv.st2 & 0x08)) { if ((regs.st2 & 0x08)) {
goto fail; goto fail;
} }
mag_x = (float) rv.val[0]; raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);
mag_y = (float) rv.val[1];
mag_z = (float) rv.val[2];
if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) { if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
goto fail; goto fail;
} }
raw_field = Vector3f(mag_x, mag_y, mag_z);
_make_factory_sensitivity_adjustment(raw_field); _make_factory_sensitivity_adjustment(raw_field);
_make_adc_sensitivity_adjustment(raw_field); _make_adc_sensitivity_adjustment(raw_field);
raw_field *= AK8963_MILLIGAUSS_SCALE; raw_field *= AK8963_MILLIGAUSS_SCALE;
@ -286,8 +279,9 @@ void AP_Compass_AK8963::_update()
} }
_last_update_timestamp = AP_HAL::micros(); _last_update_timestamp = AP_HAL::micros();
fail: fail:
_bus_sem->give(); _bus->get_semaphore()->give();
end: end:
return; return;
} }
@ -296,9 +290,10 @@ bool AP_Compass_AK8963::_check_id()
{ {
for (int i = 0; i < 5; i++) { for (int i = 0; i < 5; i++) {
uint8_t deviceid = 0; uint8_t deviceid = 0;
_bus->register_read(AK8963_WIA, &deviceid, 0x01); /* Read AK8963's id */
if (deviceid == AK8963_Device_ID) { /* Read AK8963's id */
if (_bus->register_read(AK8963_WIA, &deviceid) &&
deviceid == AK8963_Device_ID) {
return true; return true;
} }
} }
@ -307,14 +302,12 @@ bool AP_Compass_AK8963::_check_id()
} }
bool AP_Compass_AK8963::_setup_mode() { bool AP_Compass_AK8963::_setup_mode() {
_bus->register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); return _bus->register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
return true;
} }
bool AP_Compass_AK8963::_reset() bool AP_Compass_AK8963::_reset()
{ {
_bus->register_write(AK8963_CNTL2, AK8963_RESET); return _bus->register_write(AK8963_CNTL2, AK8963_RESET);
return true;
} }
@ -324,127 +317,117 @@ bool AP_Compass_AK8963::_calibrate()
_bus->register_write(AK8963_CNTL1, AK8963_FUSE_MODE | AK8963_16BIT_ADC); _bus->register_write(AK8963_CNTL1, AK8963_FUSE_MODE | AK8963_16BIT_ADC);
uint8_t response[3]; uint8_t response[3];
_bus->register_read(AK8963_ASAX, response, 3);
_bus->block_read(AK8963_ASAX, response, 3);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
float data = response[i]; float data = response[i];
_magnetometer_ASA[i] = ((data - 128) / 256 + 1); _magnetometer_ASA[i] = ((data - 128) / 256 + 1);
error("%d: %lf\n", i, _magnetometer_ASA[i]);
} }
return true; return true;
} }
void AP_Compass_AK8963::_dump_registers() /* AP_HAL::I2CDevice implementation of the AK8963 */
AP_AK8963_BusDriver_HALDevice::AP_AK8963_BusDriver_HALDevice(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
: _dev(std::move(dev))
{ {
#if AK8963_DEBUG
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 = regs[reg];
error(("%d:%02x "), (unsigned)reg, (unsigned)v);
if (reg % 16 == 0) {
error("\n");
}
}
error("\n");
#endif
} }
/* MPU9250 implementation of the AK8963 */ bool AP_AK8963_BusDriver_HALDevice::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins,
uint8_t addr,
uint8_t mpu9250_instance)
{ {
// Only initialize members. Fails are handled by configure or while return _dev->read_registers(reg, buf, size);
// getting the semaphore }
_bus = ins.get_auxiliary_bus(HAL_INS_MPU9250_SPI, mpu9250_instance);
if (!_bus) bool AP_AK8963_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
AP_HAL::panic("Cannot get MPU9250 auxiliary bus"); {
return _dev->read_registers(reg, val, 1);
}
bool AP_AK8963_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
{
return _dev->write_register(reg, val);
}
AP_HAL::Semaphore *AP_AK8963_BusDriver_HALDevice::get_semaphore()
{
return _dev->get_semaphore();
}
/* AK8963 on an auxiliary bus of IMU driver */
AP_AK8963_BusDriver_Auxiliary::AP_AK8963_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); _slave = _bus->request_next_slave(addr);
} }
AP_AK8963_SerialBus_MPU9250::~AP_AK8963_SerialBus_MPU9250() AP_AK8963_BusDriver_Auxiliary::~AP_AK8963_BusDriver_Auxiliary()
{ {
/* After started it's owned by AuxiliaryBus */ /* After started it's owned by AuxiliaryBus */
if (!_started) if (!_started) {
delete _slave; delete _slave;
}
} }
void AP_AK8963_SerialBus_MPU9250::register_write(uint8_t reg, uint8_t value) bool AP_AK8963_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
{
_slave->passthrough_write(reg, value);
}
void AP_AK8963_SerialBus_MPU9250::register_read(uint8_t reg, uint8_t *value, uint8_t count)
{
_slave->passthrough_read(reg, value, count);
}
void AP_AK8963_SerialBus_MPU9250::read_raw(struct raw_value *rv)
{ {
if (_started) { if (_started) {
_slave->read((uint8_t*)rv); /*
return; * We can only read a block when reading the block of sample values -
* calling with any other value is a mistake
*/
assert(reg == AK8963_HXL);
int n = _slave->read(buf);
return n == static_cast<int>(size);
} }
_slave->passthrough_read(0x03, (uint8_t*)rv, sizeof(*rv)); int r = _slave->passthrough_read(reg, buf, size);
return r > 0 && static_cast<uint32_t>(r) == size;
} }
AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore() bool AP_AK8963_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
{
return _slave->passthrough_read(reg, val, 1) == 1;
}
bool AP_AK8963_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
{
return _slave->passthrough_write(reg, val) == 1;
}
AP_HAL::Semaphore *AP_AK8963_BusDriver_Auxiliary::get_semaphore()
{ {
return _bus ? _bus->get_semaphore() : nullptr; return _bus ? _bus->get_semaphore() : nullptr;
} }
bool AP_AK8963_SerialBus_MPU9250::start_measurements() bool AP_AK8963_BusDriver_Auxiliary::configure()
{ {
if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(struct raw_value)) < 0) if (!_bus || !_slave) {
return false; return false;
}
return true;
}
bool AP_AK8963_BusDriver_Auxiliary::start_measurements()
{
if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(sample_regs)) < 0) {
return false;
}
_started = true; _started = true;
return true; return true;
} }
uint32_t AP_AK8963_SerialBus_MPU9250::get_dev_id()
{
return AP_COMPASS_TYPE_AK8963_MPU9250;
}
/* I2C implementation of the AK8963 */
AP_AK8963_SerialBus_I2C::AP_AK8963_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr) :
_i2c(i2c),
_addr(addr)
{
}
void AP_AK8963_SerialBus_I2C::register_write(uint8_t reg, uint8_t value)
{
_i2c->writeRegister(_addr, reg, value);
}
void AP_AK8963_SerialBus_I2C::register_read(uint8_t reg, uint8_t *value, uint8_t count)
{
_i2c->readRegisters(_addr, reg, count, value);
}
void AP_AK8963_SerialBus_I2C::read_raw(struct raw_value *rv)
{
_i2c->readRegisters(_addr, AK8963_HXL, sizeof(*rv), (uint8_t *) rv);
}
AP_HAL::Semaphore * AP_AK8963_SerialBus_I2C::get_semaphore()
{
return _i2c->get_semaphore();
}
uint32_t AP_AK8963_SerialBus_I2C::get_dev_id()
{
return AP_COMPASS_TYPE_AK8963_I2C;
}
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

167
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -1,8 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/SPIDevice.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include "Compass.h" #include "Compass.h"
@ -11,113 +13,108 @@
class AuxiliaryBus; class AuxiliaryBus;
class AuxiliaryBusSlave; class AuxiliaryBusSlave;
class AP_InertialSensor; class AP_InertialSensor;
class AP_AK8963_BusDriver;
class AP_AK8963_SerialBus
{
public:
struct PACKED raw_value {
int16_t val[3];
uint8_t st2;
};
virtual ~AP_AK8963_SerialBus() { }
virtual void register_read(uint8_t reg, uint8_t *value, uint8_t count) = 0;
uint8_t register_read(uint8_t reg) {
uint8_t value;
register_read(reg, &value, 1);
return value;
}
virtual void register_write(uint8_t reg, uint8_t value) = 0;
virtual AP_HAL::Semaphore* get_semaphore() = 0;
virtual bool start_measurements() = 0;
virtual void read_raw(struct raw_value *rv) = 0;
virtual uint32_t get_dev_id() = 0;
};
class AP_Compass_AK8963 : public AP_Compass_Backend class AP_Compass_AK8963 : public AP_Compass_Backend
{ {
public: public:
static AP_Compass_Backend *detect_mpu9250(Compass &compass, uint8_t mpu9250_instance); /* Probe for AK8963 standalone on I2C bus */
static AP_Compass_Backend *detect_mpu9250_i2c(Compass &compass, static AP_Compass_Backend *probe(Compass &compass,
AP_HAL::I2CDriver *i2c, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
uint8_t addr);
static AP_Compass_Backend *detect_i2c(Compass &compass,
AP_HAL::I2CDriver *i2c,
uint8_t addr);
AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus); /* Probe for AK8963 on auxiliary bus of MPU9250, connected through I2C */
~AP_Compass_AK8963(); static AP_Compass_Backend *probe_mpu9250(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
bool init(void); /* Probe for AK8963 on auxiliary bus of MPU9250, connected through SPI */
void read(void); static AP_Compass_Backend *probe_mpu9250(Compass &compass, uint8_t mpu9250_instance);
void accumulate(void);
virtual ~AP_Compass_AK8963();
bool init() override;
void read() override;
private: private:
static AP_Compass_Backend *_detect(Compass &compass, AP_AK8963_SerialBus *bus); AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
uint32_t dev_id);
void _make_factory_sensitivity_adjustment(Vector3f& field) const; void _make_factory_sensitivity_adjustment(Vector3f &field) const;
void _make_adc_sensitivity_adjustment(Vector3f& field) const; void _make_adc_sensitivity_adjustment(Vector3f &field) const;
Vector3f _get_filtered_field() const; Vector3f _get_filtered_field() const;
void _reset_filter();
void _reset_filter();
bool _reset(); bool _reset();
bool _setup_mode(); bool _setup_mode();
bool _check_id(); bool _check_id();
bool _calibrate(); bool _calibrate();
void _update(); void _update();
void _dump_registers();
float _magnetometer_ASA[3] {0, 0, 0}; AP_AK8963_BusDriver *_bus;
uint8_t _compass_instance;
float _mag_x_accum;
float _mag_y_accum;
float _mag_z_accum;
uint32_t _accum_count;
bool _initialized;
uint32_t _last_update_timestamp;
uint32_t _last_accum_time;
bool _timesliced;
AP_AK8963_SerialBus *_bus = nullptr;
AP_HAL::Semaphore *_bus_sem; AP_HAL::Semaphore *_bus_sem;
float _magnetometer_ASA[3] {0, 0, 0};
float _mag_x_accum;
float _mag_y_accum;
float _mag_z_accum;
uint32_t _accum_count;
uint32_t _last_update_timestamp;
uint32_t _last_accum_time;
uint32_t _dev_id;
uint8_t _compass_instance;
bool _initialized;
bool _timesliced;
}; };
class AP_AK8963_SerialBus_MPU9250: public AP_AK8963_SerialBus class AP_AK8963_BusDriver
{ {
public: public:
AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins, uint8_t addr, uint8_t mpu9250_instance); virtual ~AP_AK8963_BusDriver() { }
~AP_AK8963_SerialBus_MPU9250();
void register_read(uint8_t reg, uint8_t *value, uint8_t count); virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) = 0;
void register_write(uint8_t reg, uint8_t value); virtual bool register_read(uint8_t reg, uint8_t *val) = 0;
AP_HAL::Semaphore* get_semaphore(); virtual bool register_write(uint8_t reg, uint8_t val) = 0;
bool start_measurements();
void read_raw(struct raw_value *rv); virtual AP_HAL::Semaphore *get_semaphore() = 0;
uint32_t get_dev_id();
virtual bool configure() { return true; }
virtual bool start_measurements() { return true; }
};
class AP_AK8963_BusDriver_HALDevice: public AP_AK8963_BusDriver
{
public:
AP_AK8963_BusDriver_HALDevice(AP_HAL::OwnPtr<AP_HAL::I2CDevice> 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;
private: private:
AuxiliaryBus *_bus = nullptr; AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
AuxiliaryBusSlave *_slave = nullptr; };
class AP_AK8963_BusDriver_Auxiliary : public AP_AK8963_BusDriver
{
public:
AP_AK8963_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
uint8_t backend_instance, uint8_t addr);
~AP_AK8963_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::Semaphore *get_semaphore() override;
bool configure();
bool start_measurements();
private:
AuxiliaryBus *_bus;
AuxiliaryBusSlave *_slave;
bool _started; bool _started;
}; };
class AP_AK8963_SerialBus_I2C: public AP_AK8963_SerialBus
{
public:
AP_AK8963_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
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 start_measurements() { return true; }
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::I2CDriver *_i2c;
uint8_t _addr;
};

12
libraries/AP_Compass/Compass.cpp
View File

@ -440,25 +440,23 @@ void Compass::_detect_backends(void)
if (backend) { if (backend) {
_add_backend(backend); _add_backend(backend);
} else { } else {
_add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0)); _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0));
} }
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT
_add_backend(AP_Compass_QFLIGHT::detect(*this)); _add_backend(AP_Compass_QFLIGHT::detect(*this));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
_add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0)); _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR))); _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000
_add_backend(AP_Compass_HMC5843::probe_mpu6000(*this)); _add_backend(AP_Compass_HMC5843::probe_mpu6000(*this));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C
_add_backend(AP_Compass_AK8963::detect_i2c(*this, hal.i2c1, _add_backend(AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)));
HAL_COMPASS_AK8963_I2C_ADDR));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250_I2C #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250_I2C
_add_backend(AP_Compass_AK8963::detect_mpu9250_i2c(*this, HAL_COMPASS_AK8963_I2C_POINTER, _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)));
HAL_COMPASS_AK8963_I2C_ADDR));
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
_add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR))); _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)));
_add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0)); _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0));
#else #else
#error Unrecognised HAL_COMPASS_TYPE setting #error Unrecognised HAL_COMPASS_TYPE setting
#endif #endif