mirror of https://github.com/ArduPilot/ardupilot
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
parent
41c1209169
commit
d3831dbb98
|
|
@ -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,97 +46,91 @@
|
|||
|
||||
#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;
|
||||
};
|
||||
|
||||
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)
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
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_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()
|
||||
{
|
||||
delete _bus;
|
||||
}
|
||||
|
||||
/* stub to satisfy Compass API*/
|
||||
void AP_Compass_AK8963::accumulate(void)
|
||||
AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass,
|
||||
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()
|
||||
{
|
||||
_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()
|
|||
|
||||
/* 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
|
|||
|
||||
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()
|
|||
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 *) ®s, 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()
|
|||
}
|
||||
|
||||
_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()
|
|||
{
|
||||
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()
|
|||
}
|
||||
|
||||
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()
|
|||
_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");
|
||||
return _dev->read_registers(reg, buf, size);
|
||||
}
|
||||
|
||||
bool AP_AK8963_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
AP_AK8963_SerialBus_MPU9250::~AP_AK8963_SerialBus_MPU9250()
|
||||
AP_AK8963_BusDriver_Auxiliary::~AP_AK8963_BusDriver_Auxiliary()
|
||||
{
|
||||
/* After started it's owned by AuxiliaryBus */
|
||||
if (!_started)
|
||||
if (!_started) {
|
||||
delete _slave;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_AK8963_SerialBus_MPU9250::register_write(uint8_t reg, uint8_t value)
|
||||
{
|
||||
_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)
|
||||
bool AP_AK8963_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
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 true;
|
||||
}
|
||||
|
||||
bool AP_AK8963_BusDriver_Auxiliary::start_measurements()
|
||||
{
|
||||
if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(sample_regs)) < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_started = 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
|
||||
|
|
|
|||
|
|
@ -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 @@
|
|||
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);
|
||||
|
||||
AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus);
|
||||
~AP_Compass_AK8963();
|
||||
/* 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);
|
||||
|
||||
bool init(void);
|
||||
void read(void);
|
||||
void accumulate(void);
|
||||
/* Probe for AK8963 on auxiliary bus of MPU9250, connected through SPI */
|
||||
static AP_Compass_Backend *probe_mpu9250(Compass &compass, uint8_t mpu9250_instance);
|
||||
|
||||
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;
|
||||
|
||||
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_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_SerialBus_MPU9250: public AP_AK8963_SerialBus
|
||||
class 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();
|
||||
virtual ~AP_AK8963_BusDriver() { }
|
||||
|
||||
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;
|
||||
|
||||
virtual AP_HAL::Semaphore *get_semaphore() = 0;
|
||||
|
||||
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:
|
||||
AuxiliaryBus *_bus = nullptr;
|
||||
AuxiliaryBusSlave *_slave = nullptr;
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
|
||||
};
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue