zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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.
master
Lucas De Marchi 9 years ago
parent
41c1209169
commit
d3831dbb98
3 changed files with 209 additions and 231 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 273
      libraries/AP_Compass/AP_Compass_AK8963.cpp
  2. 155
      libraries/AP_Compass/AP_Compass_AK8963.h
  3. 12
      libraries/AP_Compass/Compass.cpp

273
libraries/AP_Compass/AP_Compass_AK8963.cpp
Unescape View File

@ -13,6 +13,8 @@ @@ -13,6 +13,8 @@
You should have received a copy of the GNU General Public License
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_HAL/AP_HAL.h>
@ -44,69 +46,69 @@ @@ -44,69 +46,69 @@
#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
extern const AP_HAL::HAL& hal;
struct PACKED sample_regs {
int16_t val[3];
uint8_t st2;
};
extern const AP_HAL::HAL &hal;
AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus) :
AP_Compass_Backend(compass),
_initialized(false),
_last_update_timestamp(0),
_last_accum_time(0),
_bus(bus)
AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
uint32_t dev_id)
: AP_Compass_Backend(compass)
, _bus(bus)
, _dev_id(dev_id)
{
_reset_filter();
}
AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250(Compass &compass, uint8_t mpu9250_instance)
AP_Compass_AK8963::~AP_Compass_AK8963()
{
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);
delete _bus;
}
AP_Compass_Backend *AP_Compass_AK8963::detect_i2c(Compass &compass,
AP_HAL::I2CDriver *i2c,
uint8_t addr)
AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
AP_AK8963_SerialBus *bus = new AP_AK8963_SerialBus_I2C(i2c, addr);
if (!bus)
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 _detect(compass, bus);
}
return sensor;
}
AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250_i2c(Compass &compass,
AP_HAL::I2CDriver *i2c,
uint8_t addr)
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 detect_i2c(compass, i2c, addr);
return probe(compass, std::move(dev));
}
AP_Compass_Backend *AP_Compass_AK8963::_detect(Compass &compass,
AP_AK8963_SerialBus *bus)
AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t mpu9250_instance)
{
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus);
if (sensor == nullptr) {
delete bus;
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;
}
if (!sensor->init()) {
AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, AP_COMPASS_TYPE_AK8963_MPU9250);
if (!sensor || !sensor->init()) {
delete sensor;
return nullptr;
}
@ -114,27 +116,21 @@ AP_Compass_Backend *AP_Compass_AK8963::_detect(Compass &compass, @@ -114,27 +116,21 @@ AP_Compass_Backend *AP_Compass_AK8963::_detect(Compass &compass,
return sensor;
}
AP_Compass_AK8963::~AP_Compass_AK8963()
{
delete _bus;
}
/* stub to satisfy Compass API*/
void AP_Compass_AK8963::accumulate(void)
{
}
bool AP_Compass_AK8963::init()
{
_bus_sem = _bus->get_semaphore();
hal.scheduler->suspend_timer_procs();
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
if (!_bus_sem->take(100)) {
hal.console->printf("AK8963: Unable to get bus semaphore");
if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("AK8963: Unable to get bus semaphore\n");
goto fail_sem;
}
if (!_bus->configure()) {
hal.console->printf("AK8963: Could not configure the bus\n");
goto fail;
}
if (!_check_id()) {
hal.console->printf("AK8963: Wrong id\n");
goto fail;
@ -159,17 +155,18 @@ bool AP_Compass_AK8963::init() @@ -159,17 +155,18 @@ bool AP_Compass_AK8963::init()
/* register the compass instance in the frontend */
_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 */
_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();
return true;
fail:
_bus_sem->give();
bus_sem->give();
fail_sem:
hal.scheduler->resume_timer_procs();
@ -225,9 +222,7 @@ void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) co @@ -225,9 +222,7 @@ void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) co
void AP_Compass_AK8963::_update()
{
struct AP_AK8963_SerialBus::raw_value rv;
float mag_x, mag_y, mag_z;
// get raw_field - sensor frame, uncorrected
struct sample_regs regs;
Vector3f raw_field;
uint32_t time_us = AP_HAL::micros();
@ -236,28 +231,26 @@ void AP_Compass_AK8963::_update() @@ -236,28 +231,26 @@ void AP_Compass_AK8963::_update()
goto end;
}
if (!_bus_sem->take_nonblocking()) {
if (!_bus->get_semaphore()->take_nonblocking()) {
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
* 6.4.3.6 - Magnetic Sensor Overflow. */
if ((rv.st2 & 0x08)) {
if ((regs.st2 & 0x08)) {
goto fail;
}
mag_x = (float) rv.val[0];
mag_y = (float) rv.val[1];
mag_z = (float) rv.val[2];
raw_field = Vector3f(regs.val[0], regs.val[1], regs.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;
}
raw_field = Vector3f(mag_x, mag_y, mag_z);
_make_factory_sensitivity_adjustment(raw_field);
_make_adc_sensitivity_adjustment(raw_field);
raw_field *= AK8963_MILLIGAUSS_SCALE;
@ -286,8 +279,9 @@ void AP_Compass_AK8963::_update() @@ -286,8 +279,9 @@ void AP_Compass_AK8963::_update()
}
_last_update_timestamp = AP_HAL::micros();
fail:
_bus_sem->give();
_bus->get_semaphore()->give();
end:
return;
}
@ -296,9 +290,10 @@ bool AP_Compass_AK8963::_check_id() @@ -296,9 +290,10 @@ bool AP_Compass_AK8963::_check_id()
{
for (int i = 0; i < 5; i++) {
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;
}
}
@ -307,14 +302,12 @@ bool AP_Compass_AK8963::_check_id() @@ -307,14 +302,12 @@ bool AP_Compass_AK8963::_check_id()
}
bool AP_Compass_AK8963::_setup_mode() {
_bus->register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
return true;
return _bus->register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
}
bool AP_Compass_AK8963::_reset()
{
_bus->register_write(AK8963_CNTL2, AK8963_RESET);
return true;
return _bus->register_write(AK8963_CNTL2, AK8963_RESET);
}
@ -324,127 +317,117 @@ bool AP_Compass_AK8963::_calibrate() @@ -324,127 +317,117 @@ bool AP_Compass_AK8963::_calibrate()
_bus->register_write(AK8963_CNTL1, AK8963_FUSE_MODE | AK8963_16BIT_ADC);
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++) {
float data = response[i];
_magnetometer_ASA[i] = ((data - 128) / 256 + 1);
error("%d: %lf\n", i, _magnetometer_ASA[i]);
}
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 */
AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins,
uint8_t addr,
uint8_t mpu9250_instance)
bool AP_AK8963_BusDriver_HALDevice::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
{
// Only initialize members. Fails are handled by configure or while
// getting the semaphore
_bus = ins.get_auxiliary_bus(HAL_INS_MPU9250_SPI, mpu9250_instance);
if (!_bus)
AP_HAL::panic("Cannot get MPU9250 auxiliary bus");
_slave = _bus->request_next_slave(addr);
return _dev->read_registers(reg, buf, size);
}
AP_AK8963_SerialBus_MPU9250::~AP_AK8963_SerialBus_MPU9250()
bool AP_AK8963_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
{
/* After started it's owned by AuxiliaryBus */
if (!_started)
delete _slave;
return _dev->read_registers(reg, val, 1);
}
void AP_AK8963_SerialBus_MPU9250::register_write(uint8_t reg, uint8_t value)
bool AP_AK8963_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
{
_slave->passthrough_write(reg, value);
return _dev->write_register(reg, val);
}
void AP_AK8963_SerialBus_MPU9250::register_read(uint8_t reg, uint8_t *value, uint8_t count)
AP_HAL::Semaphore *AP_AK8963_BusDriver_HALDevice::get_semaphore()
{
_slave->passthrough_read(reg, value, count);
return _dev->get_semaphore();
}
void AP_AK8963_SerialBus_MPU9250::read_raw(struct raw_value *rv)
/* 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)
{
if (_started) {
_slave->read((uint8_t*)rv);
/*
* 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->passthrough_read(0x03, (uint8_t*)rv, sizeof(*rv));
_slave = _bus->request_next_slave(addr);
}
AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore()
AP_AK8963_BusDriver_Auxiliary::~AP_AK8963_BusDriver_Auxiliary()
{
return _bus ? _bus->get_semaphore() : nullptr;
/* After started it's owned by AuxiliaryBus */
if (!_started) {
delete _slave;
}
}
bool AP_AK8963_SerialBus_MPU9250::start_measurements()
bool AP_AK8963_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
{
if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(struct raw_value)) < 0)
return false;
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 == AK8963_HXL);
int n = _slave->read(buf);
return n == static_cast<int>(size);
}
_started = true;
int r = _slave->passthrough_read(reg, buf, size);
return true;
return r > 0 && static_cast<uint32_t>(r) == size;
}
uint32_t AP_AK8963_SerialBus_MPU9250::get_dev_id()
bool AP_AK8963_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
{
return AP_COMPASS_TYPE_AK8963_MPU9250;
return _slave->passthrough_read(reg, val, 1) == 1;
}
/* I2C implementation of the AK8963 */
AP_AK8963_SerialBus_I2C::AP_AK8963_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr) :
_i2c(i2c),
_addr(addr)
bool AP_AK8963_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
{
return _slave->passthrough_write(reg, val) == 1;
}
void AP_AK8963_SerialBus_I2C::register_write(uint8_t reg, uint8_t value)
AP_HAL::Semaphore *AP_AK8963_BusDriver_Auxiliary::get_semaphore()
{
_i2c->writeRegister(_addr, reg, value);
return _bus ? _bus->get_semaphore() : nullptr;
}
void AP_AK8963_SerialBus_I2C::register_read(uint8_t reg, uint8_t *value, uint8_t count)
bool AP_AK8963_BusDriver_Auxiliary::configure()
{
_i2c->readRegisters(_addr, reg, count, value);
if (!_bus || !_slave) {
return false;
}
return true;
}
void AP_AK8963_SerialBus_I2C::read_raw(struct raw_value *rv)
bool AP_AK8963_BusDriver_Auxiliary::start_measurements()
{
_i2c->readRegisters(_addr, AK8963_HXL, sizeof(*rv), (uint8_t *) rv);
}
if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(sample_regs)) < 0) {
return false;
}
AP_HAL::Semaphore * AP_AK8963_SerialBus_I2C::get_semaphore()
{
return _i2c->get_semaphore();
}
_started = true;
uint32_t AP_AK8963_SerialBus_I2C::get_dev_id()
{
return AP_COMPASS_TYPE_AK8963_I2C;
return true;
}
#endif // CONFIG_HAL_BOARD

155
libraries/AP_Compass/AP_Compass_AK8963.h
Unescape View File

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

12
libraries/AP_Compass/Compass.cpp
Unescape View File

@ -440,25 +440,23 @@ void Compass::_detect_backends(void) @@ -440,25 +440,23 @@ void Compass::_detect_backends(void)
if (backend) {
_add_backend(backend);
} 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
_add_backend(AP_Compass_QFLIGHT::detect(*this));
#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
_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
_add_backend(AP_Compass_HMC5843::probe_mpu6000(*this));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C
_add_backend(AP_Compass_AK8963::detect_i2c(*this, hal.i2c1,
HAL_COMPASS_AK8963_I2C_ADDR));
_add_backend(AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)));
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250_I2C
_add_backend(AP_Compass_AK8963::detect_mpu9250_i2c(*this, HAL_COMPASS_AK8963_I2C_POINTER,
HAL_COMPASS_AK8963_I2C_ADDR));
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)));
#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_AK8963::detect_mpu9250(*this, 0));
_add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0));
#else
#error Unrecognised HAL_COMPASS_TYPE setting
#endif

Write Preview
Loading…
Cancel
Save