mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: backport master AK09916 driver
this allows mRoControlZeroF7 to work in copter stable
This commit is contained in:
Andrew Tridgell
parent
3011bc1930
commit
cd178a6c4f
|
|
@ -601,16 +601,14 @@ void Compass::_probe_external_i2c_compasses(void)
|
|||
#if !HAL_MINIMIZE_FEATURES
|
||||
// AK09916 on ICM20948
|
||||
FOREACH_I2C_EXTERNAL(i) {
|
||||
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
|
||||
GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
|
||||
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
|
||||
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR),
|
||||
true, ROTATION_PITCH_180_YAW_90),
|
||||
AP_Compass_AK09916::name, true);
|
||||
}
|
||||
|
||||
FOREACH_I2C_INTERNAL(i) {
|
||||
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
|
||||
GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
|
||||
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
|
||||
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR),
|
||||
both_i2c_external, ROTATION_PITCH_180_YAW_90),
|
||||
AP_Compass_AK09916::name, true);
|
||||
|
|
@ -646,12 +644,12 @@ void Compass::_probe_external_i2c_compasses(void)
|
|||
|
||||
// AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that
|
||||
FOREACH_I2C_EXTERNAL(i) {
|
||||
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
|
||||
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
|
||||
true, ROTATION_YAW_270),
|
||||
AP_Compass_AK09916::name, true);
|
||||
}
|
||||
FOREACH_I2C_INTERNAL(i) {
|
||||
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
|
||||
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
|
||||
both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE),
|
||||
AP_Compass_AK09916::name, both_i2c_external);
|
||||
}
|
||||
|
|
@ -949,6 +947,8 @@ void Compass::_detect_backends(void)
|
|||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BMM150_I2C
|
||||
ADD_BACKEND(DRIVER_BMM150, AP_Compass_BMM150::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)),
|
||||
AP_Compass_BMM150::name, true);
|
||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_MROCONTROLZEROF7
|
||||
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe_ICM20948(0, ROTATION_ROLL_180), AP_Compass_AK09916::name, false);
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NONE
|
||||
// no compass
|
||||
#else
|
||||
|
|
|
|||
|
|
@ -17,10 +17,13 @@
|
|||
*/
|
||||
#include "AP_Compass_AK09916.h"
|
||||
|
||||
#include <assert.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <utility>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <stdio.h>
|
||||
#include <AP_InertialSensor/AP_InertialSensor_Invensensev2.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#define REG_COMPANY_ID 0x00
|
||||
#define REG_DEVICE_ID 0x01
|
||||
|
|
@ -43,22 +46,46 @@
|
|||
|
||||
#define ICM_WHOAMI_VAL 0xEA
|
||||
|
||||
#define AK09916_Device_ID 0x09
|
||||
#define AK09916_MILLIGAUSS_SCALE 10.0f
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
/*
|
||||
probe for AK09916 directly on I2C
|
||||
*/
|
||||
AP_Compass_Backend *AP_Compass_AK09916::probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
struct PACKED sample_regs {
|
||||
uint8_t st1;
|
||||
int16_t val[3];
|
||||
uint8_t tmps;
|
||||
uint8_t st2;
|
||||
};
|
||||
|
||||
AP_Compass_AK09916::AP_Compass_AK09916(AP_AK09916_BusDriver *bus,
|
||||
bool force_external,
|
||||
enum Rotation rotation) :
|
||||
AP_Compass_Backend(AP::compass())
|
||||
, _bus(bus)
|
||||
, _force_external(force_external)
|
||||
, _rotation(rotation)
|
||||
{
|
||||
}
|
||||
|
||||
AP_Compass_AK09916::~AP_Compass_AK09916()
|
||||
{
|
||||
delete _bus;
|
||||
}
|
||||
|
||||
AP_Compass_Backend *AP_Compass_AK09916::probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), nullptr,
|
||||
force_external,
|
||||
rotation, AK09916_I2C);
|
||||
AP_AK09916_BusDriver *bus = new AP_AK09916_BusDriver_HALDevice(std::move(dev));
|
||||
if (!bus) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(bus, force_external, rotation);
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
|
|
@ -67,22 +94,91 @@ AP_Compass_Backend *AP_Compass_AK09916::probe(Compass &compass,
|
|||
return sensor;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
probe for AK09916 connected via an ICM20948
|
||||
*/
|
||||
AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
|
||||
bool force_external,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev || !dev_icm) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), std::move(dev_icm),
|
||||
force_external,
|
||||
rotation, AK09916_ICM20948_I2C);
|
||||
|
||||
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/* Allow ICM20x48 to shortcut auxiliary bus and host bus */
|
||||
uint8_t rval;
|
||||
uint16_t whoami;
|
||||
uint8_t retries = 5;
|
||||
if (!dev_icm->read_registers(REG_ICM_WHOAMI, &rval, 1) ||
|
||||
rval != ICM_WHOAMI_VAL) {
|
||||
// not an ICM_WHOAMI
|
||||
goto fail;
|
||||
}
|
||||
do {
|
||||
// reset then bring sensor out of sleep mode
|
||||
if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x80)) {
|
||||
goto fail;
|
||||
}
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x00)) {
|
||||
goto fail;
|
||||
}
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
// see if ICM20948 is sleeping
|
||||
if (!dev_icm->read_registers(REG_ICM_PWR_MGMT_1, &rval, 1)) {
|
||||
goto fail;
|
||||
}
|
||||
if ((rval & 0x40) == 0) {
|
||||
break;
|
||||
}
|
||||
} while (retries--);
|
||||
|
||||
if (rval & 0x40) {
|
||||
// it didn't come out of sleep
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// initially force i2c bypass off
|
||||
dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x00);
|
||||
hal.scheduler->delay(1);
|
||||
|
||||
// now check if a AK09916 shows up on the bus. If it does then
|
||||
// we have both a real AK09916 and a ICM20948 with an embedded
|
||||
// AK09916. In that case we will fail the driver load and use
|
||||
// the main AK09916 driver
|
||||
if (dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2)) {
|
||||
// a device is replying on the AK09916 I2C address, don't
|
||||
// load the ICM20948
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "ICM20948: AK09916 bus conflict\n");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// now force bypass on
|
||||
dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x02);
|
||||
hal.scheduler->delay(1);
|
||||
dev->get_semaphore()->give();
|
||||
return probe(std::move(dev), force_external, rotation);
|
||||
fail:
|
||||
dev->get_semaphore()->give();
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(uint8_t inv2_instance,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
AP_InertialSensor &ins = AP::ins();
|
||||
|
||||
AP_AK09916_BusDriver *bus =
|
||||
new AP_AK09916_BusDriver_Auxiliary(ins, HAL_INS_INV2_SPI, inv2_instance, HAL_COMPASS_AK09916_I2C_ADDR);
|
||||
if (!bus) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(bus, false, rotation);
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
|
|
@ -91,182 +187,290 @@ AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(Compass &compass,
|
|||
return sensor;
|
||||
}
|
||||
|
||||
AP_Compass_AK09916::AP_Compass_AK09916(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev_icm,
|
||||
bool _force_external,
|
||||
enum Rotation _rotation,
|
||||
enum bus_type _bus_type)
|
||||
: AP_Compass_Backend(compass)
|
||||
, bus_type(_bus_type)
|
||||
, dev(std::move(_dev))
|
||||
, dev_icm(std::move(_dev_icm))
|
||||
, force_external(_force_external)
|
||||
, rotation(_rotation)
|
||||
{
|
||||
}
|
||||
|
||||
bool AP_Compass_AK09916::init()
|
||||
{
|
||||
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
|
||||
|
||||
if (!bus_sem || !_bus->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Unable to get bus semaphore\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (bus_type == AK09916_ICM20948_I2C && dev_icm) {
|
||||
uint8_t rval;
|
||||
if (!dev_icm->read_registers(REG_ICM_WHOAMI, &rval, 1) ||
|
||||
rval != ICM_WHOAMI_VAL) {
|
||||
// not an ICM_WHOAMI
|
||||
goto fail;
|
||||
}
|
||||
uint8_t retries = 5;
|
||||
do {
|
||||
// reset then bring sensor out of sleep mode
|
||||
if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x80)) {
|
||||
goto fail;
|
||||
}
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x00)) {
|
||||
goto fail;
|
||||
}
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
// see if ICM20948 is sleeping
|
||||
if (!dev_icm->read_registers(REG_ICM_PWR_MGMT_1, &rval, 1)) {
|
||||
goto fail;
|
||||
}
|
||||
if ((rval & 0x40) == 0) {
|
||||
break;
|
||||
}
|
||||
} while (retries--);
|
||||
|
||||
if (rval & 0x40) {
|
||||
// it didn't come out of sleep
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// initially force i2c bypass off
|
||||
dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x00);
|
||||
hal.scheduler->delay(1);
|
||||
|
||||
// now check if a AK09916 shows up on the bus. If it does then
|
||||
// we have both a real AK09916 and a ICM20948 with an embedded
|
||||
// AK09916. In that case we will fail the driver load and use
|
||||
// the main AK09916 driver
|
||||
uint16_t whoami;
|
||||
if (dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2)) {
|
||||
// a device is replying on the AK09916 I2C address, don't
|
||||
// load the ICM20948
|
||||
hal.console->printf("ICM20948: AK09916 bus conflict\n");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// now force bypass on
|
||||
dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x02);
|
||||
hal.scheduler->delay(1);
|
||||
}
|
||||
|
||||
uint16_t whoami;
|
||||
if (!dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2) ||
|
||||
whoami != 0x0948) {
|
||||
// not a AK09916
|
||||
if (!_bus->configure()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Could not configure the bus\n");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
dev->setup_checked_registers(2);
|
||||
if (!_reset()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Reset Failed\n");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
dev->write_register(REG_CNTL2, 0x08, true); // continuous 100Hz
|
||||
dev->write_register(REG_CNTL3, 0x00, true);
|
||||
|
||||
dev->get_semaphore()->give();
|
||||
if (!_check_id()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Wrong id\n");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (!_setup_mode()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Could not setup mode\n");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (!_bus->start_measurements()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"AK09916: Could not start measurements\n");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
_initialized = true;
|
||||
|
||||
/* register the compass instance in the frontend */
|
||||
compass_instance = register_compass();
|
||||
_compass_instance = register_compass();
|
||||
|
||||
printf("Found a AK09916 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance);
|
||||
|
||||
set_rotation(compass_instance, rotation);
|
||||
|
||||
if (force_external) {
|
||||
set_external(compass_instance, true);
|
||||
if (_force_external) {
|
||||
set_external(_compass_instance, true);
|
||||
}
|
||||
|
||||
dev->set_device_type(bus_type == AK09916_ICM20948_I2C?DEVTYPE_ICM20948:DEVTYPE_AK09916);
|
||||
set_dev_id(compass_instance, dev->get_bus_id());
|
||||
|
||||
// call timer() at 100Hz
|
||||
dev->register_periodic_callback(10000,
|
||||
FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::timer, void));
|
||||
set_rotation(_compass_instance, _rotation);
|
||||
|
||||
_bus->set_device_type(DEVTYPE_AK09916);
|
||||
set_dev_id(_compass_instance, _bus->get_bus_id());
|
||||
|
||||
bus_sem->give();
|
||||
|
||||
_bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::_update, void));
|
||||
|
||||
return true;
|
||||
|
||||
fail:
|
||||
dev->get_semaphore()->give();
|
||||
bus_sem->give();
|
||||
return false;
|
||||
}
|
||||
|
||||
void AP_Compass_AK09916::timer()
|
||||
{
|
||||
struct PACKED {
|
||||
int16_t magx;
|
||||
int16_t magy;
|
||||
int16_t magz;
|
||||
uint8_t tmps;
|
||||
uint8_t status2;
|
||||
} data;
|
||||
const float to_utesla = 4912.0f / 32752.0f;
|
||||
const float utesla_to_milliGauss = 10;
|
||||
const float range_scale = to_utesla * utesla_to_milliGauss;
|
||||
Vector3f field;
|
||||
|
||||
// check data ready
|
||||
uint8_t st1;
|
||||
if (!dev->read_registers(REG_ST1, &st1, 1) || !(st1 & 1)) {
|
||||
goto check_registers;
|
||||
}
|
||||
|
||||
if (!dev->read_registers(REG_HXL, (uint8_t *)&data, sizeof(data))) {
|
||||
goto check_registers;
|
||||
}
|
||||
|
||||
field(data.magx * range_scale, data.magy * range_scale, data.magz * range_scale);
|
||||
|
||||
/* rotate raw_field from sensor frame to body frame */
|
||||
rotate_field(field, compass_instance);
|
||||
|
||||
/* publish raw_field (uncorrected point sample) for calibration use */
|
||||
publish_raw_field(field, compass_instance);
|
||||
|
||||
/* correct raw_field for known errors */
|
||||
correct_field(field, compass_instance);
|
||||
|
||||
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
accum += field;
|
||||
accum_count++;
|
||||
_sem->give();
|
||||
}
|
||||
|
||||
check_registers:
|
||||
dev->check_next_register();
|
||||
}
|
||||
|
||||
void AP_Compass_AK09916::read()
|
||||
{
|
||||
if (!_sem->take_nonblocking()) {
|
||||
if (!_initialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (accum_count == 0) {
|
||||
_sem->give();
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return;
|
||||
}
|
||||
|
||||
accum /= accum_count;
|
||||
|
||||
publish_filtered_field(accum, compass_instance);
|
||||
publish_filtered_field(accum, _compass_instance);
|
||||
|
||||
accum.zero();
|
||||
accum_count = 0;
|
||||
|
||||
_sem->give();
|
||||
}
|
||||
|
||||
void AP_Compass_AK09916::_make_adc_sensitivity_adjustment(Vector3f& field) const
|
||||
{
|
||||
static const float ADC_16BIT_RESOLUTION = 0.15f;
|
||||
|
||||
field *= ADC_16BIT_RESOLUTION;
|
||||
}
|
||||
|
||||
void AP_Compass_AK09916::_update()
|
||||
{
|
||||
struct sample_regs regs = {0};
|
||||
Vector3f raw_field;
|
||||
|
||||
if (!_bus->block_read(REG_ST1, (uint8_t *) ®s, sizeof(regs))) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!(regs.st1 & 0x01)) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Check for overflow. See AK09916's datasheet*/
|
||||
if ((regs.st2 & 0x08)) {
|
||||
return;
|
||||
}
|
||||
|
||||
raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);
|
||||
|
||||
if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
|
||||
return;
|
||||
}
|
||||
|
||||
_make_adc_sensitivity_adjustment(raw_field);
|
||||
raw_field *= AK09916_MILLIGAUSS_SCALE;
|
||||
|
||||
/* rotate raw_field from sensor frame to body frame */
|
||||
rotate_field(raw_field, _compass_instance);
|
||||
|
||||
/* publish raw_field (uncorrected point sample) for calibration use */
|
||||
publish_raw_field(raw_field, _compass_instance);
|
||||
|
||||
/* correct raw_field for known errors */
|
||||
correct_field(raw_field, _compass_instance);
|
||||
|
||||
if (!field_ok(raw_field)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
accum += raw_field;
|
||||
accum_count++;
|
||||
if (accum_count >= 64) {
|
||||
accum_count /= 2;
|
||||
accum /= 2;
|
||||
}
|
||||
_sem->give();
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_Compass_AK09916::_check_id()
|
||||
{
|
||||
for (int i = 0; i < 5; i++) {
|
||||
uint8_t deviceid = 0;
|
||||
|
||||
/* Read AK09916's id */
|
||||
if (_bus->register_read(REG_DEVICE_ID, &deviceid) &&
|
||||
deviceid == AK09916_Device_ID) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AP_Compass_AK09916::_setup_mode() {
|
||||
return _bus->register_write(REG_CNTL2, 0x08); //Continuous Mode 2
|
||||
}
|
||||
|
||||
bool AP_Compass_AK09916::_reset()
|
||||
{
|
||||
return _bus->register_write(REG_CNTL3, 0x01); //Soft Reset
|
||||
}
|
||||
|
||||
/* AP_HAL::I2CDevice implementation of the AK09916 */
|
||||
AP_AK09916_BusDriver_HALDevice::AP_AK09916_BusDriver_HALDevice(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
|
||||
: _dev(std::move(dev))
|
||||
{
|
||||
}
|
||||
|
||||
bool AP_AK09916_BusDriver_HALDevice::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
|
||||
{
|
||||
return _dev->read_registers(reg, buf, size);
|
||||
}
|
||||
|
||||
bool AP_AK09916_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
|
||||
{
|
||||
return _dev->read_registers(reg, val, 1);
|
||||
}
|
||||
|
||||
bool AP_AK09916_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
return _dev->write_register(reg, val);
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore *AP_AK09916_BusDriver_HALDevice::get_semaphore()
|
||||
{
|
||||
return _dev->get_semaphore();
|
||||
}
|
||||
|
||||
AP_HAL::Device::PeriodicHandle AP_AK09916_BusDriver_HALDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
||||
{
|
||||
return _dev->register_periodic_callback(period_usec, cb);
|
||||
}
|
||||
|
||||
/* AK09916 on an auxiliary bus of IMU driver */
|
||||
AP_AK09916_BusDriver_Auxiliary::AP_AK09916_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_AK09916_BusDriver_Auxiliary::~AP_AK09916_BusDriver_Auxiliary()
|
||||
{
|
||||
/* After started it's owned by AuxiliaryBus */
|
||||
if (!_started) {
|
||||
delete _slave;
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_AK09916_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
|
||||
{
|
||||
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 == REG_ST1);
|
||||
|
||||
int n = _slave->read(buf);
|
||||
return n == static_cast<int>(size);
|
||||
}
|
||||
|
||||
int r = _slave->passthrough_read(reg, buf, size);
|
||||
|
||||
return r > 0 && static_cast<uint32_t>(r) == size;
|
||||
}
|
||||
|
||||
bool AP_AK09916_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
|
||||
{
|
||||
return _slave->passthrough_read(reg, val, 1) == 1;
|
||||
}
|
||||
|
||||
bool AP_AK09916_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
return _slave->passthrough_write(reg, val) == 1;
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore *AP_AK09916_BusDriver_Auxiliary::get_semaphore()
|
||||
{
|
||||
return _bus ? _bus->get_semaphore() : nullptr;
|
||||
}
|
||||
|
||||
bool AP_AK09916_BusDriver_Auxiliary::configure()
|
||||
{
|
||||
if (!_bus || !_slave) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_AK09916_BusDriver_Auxiliary::start_measurements()
|
||||
{
|
||||
if (_bus->register_periodic_read(_slave, REG_ST1, sizeof(sample_regs)) < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_started = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
AP_HAL::Device::PeriodicHandle AP_AK09916_BusDriver_Auxiliary::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
||||
{
|
||||
return _bus->register_periodic_callback(period_usec, cb);
|
||||
}
|
||||
|
||||
// set device type within a device class
|
||||
void AP_AK09916_BusDriver_Auxiliary::set_device_type(uint8_t devtype)
|
||||
{
|
||||
_bus->set_device_type(devtype);
|
||||
}
|
||||
|
||||
// return 24 bit bus identifier
|
||||
uint32_t AP_AK09916_BusDriver_Auxiliary::get_bus_id(void) const
|
||||
{
|
||||
return _bus->get_bus_id();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -26,55 +26,141 @@
|
|||
# define HAL_COMPASS_AK09916_I2C_ADDR 0x0C
|
||||
#endif
|
||||
|
||||
// the AK09916 can be connected via an ICM20948
|
||||
|
||||
#ifndef HAL_COMPASS_ICM20948_I2C_ADDR
|
||||
# define HAL_COMPASS_ICM20948_I2C_ADDR 0x69
|
||||
#endif
|
||||
|
||||
class AuxiliaryBus;
|
||||
class AuxiliaryBusSlave;
|
||||
class AP_InertialSensor;
|
||||
class AP_AK09916_BusDriver;
|
||||
|
||||
class AP_Compass_AK09916 : public AP_Compass_Backend
|
||||
{
|
||||
public:
|
||||
static AP_Compass_Backend *probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external = false,
|
||||
/* Probe for AK09916 standalone on I2C bus */
|
||||
static AP_Compass_Backend *probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
// separate probe function for when behind a ICM20948 IMU
|
||||
static AP_Compass_Backend *probe_ICM20948(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
|
||||
bool force_external = false,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
void read() override;
|
||||
/* Probe for AK09916 on auxiliary bus of ICM20948, connected through I2C */
|
||||
static AP_Compass_Backend *probe_ICM20948(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_icm,
|
||||
bool force_external,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
/* Probe for AK09916 on auxiliary bus of ICM20948, connected through SPI */
|
||||
static AP_Compass_Backend *probe_ICM20948(uint8_t mpu9250_instance,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
static constexpr const char *name = "AK09916";
|
||||
|
||||
virtual ~AP_Compass_AK09916();
|
||||
|
||||
void read() override;
|
||||
|
||||
private:
|
||||
enum bus_type {
|
||||
AK09916_I2C=0,
|
||||
AK09916_ICM20948_I2C,
|
||||
} bus_type;
|
||||
|
||||
AP_Compass_AK09916(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev_icm,
|
||||
bool force_external,
|
||||
enum Rotation rotation,
|
||||
enum bus_type bus_type);
|
||||
AP_Compass_AK09916(AP_AK09916_BusDriver *bus, bool force_external,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev;
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev_icm;
|
||||
|
||||
/**
|
||||
* Device periodic callback to read data from the sensor.
|
||||
*/
|
||||
bool init();
|
||||
void timer();
|
||||
void _make_factory_sensitivity_adjustment(Vector3f &field) const;
|
||||
void _make_adc_sensitivity_adjustment(Vector3f &field) const;
|
||||
|
||||
uint8_t compass_instance;
|
||||
bool _reset();
|
||||
bool _setup_mode();
|
||||
bool _check_id();
|
||||
bool _calibrate();
|
||||
|
||||
void _update();
|
||||
|
||||
AP_AK09916_BusDriver *_bus;
|
||||
|
||||
float _magnetometer_ASA[3] {0, 0, 0};
|
||||
bool _force_external;
|
||||
uint8_t _compass_instance;
|
||||
bool _initialized;
|
||||
enum Rotation _rotation;
|
||||
Vector3f accum;
|
||||
uint16_t accum_count;
|
||||
bool force_external;
|
||||
enum Rotation rotation;
|
||||
uint32_t accum_count;
|
||||
};
|
||||
|
||||
|
||||
class AP_AK09916_BusDriver
|
||||
{
|
||||
public:
|
||||
virtual ~AP_AK09916_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; }
|
||||
virtual AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t, AP_HAL::Device::PeriodicCb) = 0;
|
||||
|
||||
// set device type within a device class
|
||||
virtual void set_device_type(uint8_t devtype) = 0;
|
||||
|
||||
// return 24 bit bus identifier
|
||||
virtual uint32_t get_bus_id(void) const = 0;
|
||||
};
|
||||
|
||||
class AP_AK09916_BusDriver_HALDevice: public AP_AK09916_BusDriver
|
||||
{
|
||||
public:
|
||||
AP_AK09916_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;
|
||||
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
|
||||
|
||||
// set device type within a device class
|
||||
void set_device_type(uint8_t devtype) override {
|
||||
_dev->set_device_type(devtype);
|
||||
}
|
||||
|
||||
// return 24 bit bus identifier
|
||||
uint32_t get_bus_id(void) const override {
|
||||
return _dev->get_bus_id();
|
||||
}
|
||||
|
||||
private:
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
|
||||
};
|
||||
|
||||
class AP_AK09916_BusDriver_Auxiliary : public AP_AK09916_BusDriver
|
||||
{
|
||||
public:
|
||||
AP_AK09916_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
|
||||
uint8_t backend_instance, uint8_t addr);
|
||||
~AP_AK09916_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::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
|
||||
|
||||
AP_HAL::Semaphore *get_semaphore() override;
|
||||
|
||||
bool configure() override;
|
||||
bool start_measurements() override;
|
||||
|
||||
// set device type within a device class
|
||||
void set_device_type(uint8_t devtype) override;
|
||||
|
||||
// return 24 bit bus identifier
|
||||
uint32_t get_bus_id(void) const override;
|
||||
|
||||
private:
|
||||
AuxiliaryBus *_bus;
|
||||
AuxiliaryBusSlave *_slave;
|
||||
bool _started;
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue