AP_Compass: HMC5843: be agnostic to I2C bus
Like was done to inertial AK8963 and inertial sensor move the decision regarding the I2C bus to the caller. We don't allow changing the address because apparently HMC5843 doesn't support different addresses. Changing only the bus could be more easily done but this prepares the ground for using HMC5843 on an AuxiliarBus. The need for a more generic abstraction is evidenced by this commit: a "SerialBus" in AP_HAL would be a nice addition rather than letting each driver to provide its own. However the methods are a little bit different from what we have in AK8963. It's safer to do the simple conversion now and later on to add the abstraction changing everybody to use it.
This commit is contained in:
Lucas De Marchi
Andrew Tridgell
parent
3eb6be64c9
commit
c66c9c41f2
@ -31,7 +31,7 @@
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define COMPASS_ADDRESS 0x1E
|
||||
#define HMC5843_I2C_ADDR 0x1E
|
||||
#define ConfigRegA 0x00
|
||||
#define ConfigRegB 0x01
|
||||
#define magGain 0x20
|
||||
@ -58,10 +58,10 @@ extern const AP_HAL::HAL& hal;
|
||||
#define DataOutputRate_75HZ 0x06
|
||||
|
||||
// constructor
|
||||
AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass):
|
||||
AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass, AP_HMC5843_SerialBus *bus) :
|
||||
AP_Compass_Backend(compass),
|
||||
_bus(bus),
|
||||
_retry_time(0),
|
||||
_i2c_sem(NULL),
|
||||
_mag_x(0),
|
||||
_mag_y(0),
|
||||
_mag_z(0),
|
||||
@ -74,24 +74,41 @@ AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass):
|
||||
_product_id(0)
|
||||
{}
|
||||
|
||||
// detect the sensor
|
||||
AP_Compass_Backend *AP_Compass_HMC5843::detect(Compass &compass)
|
||||
AP_Compass_HMC5843::~AP_Compass_HMC5843()
|
||||
{
|
||||
AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass);
|
||||
if (sensor == NULL) {
|
||||
return NULL;
|
||||
delete _bus;
|
||||
}
|
||||
|
||||
// detect the sensor
|
||||
AP_Compass_Backend *AP_Compass_HMC5843::detect_i2c(Compass &compass,
|
||||
AP_HAL::I2CDriver *i2c)
|
||||
{
|
||||
AP_HMC5843_SerialBus *bus = new AP_HMC5843_SerialBus_I2C(i2c, HMC5843_I2C_ADDR);
|
||||
if (!bus)
|
||||
return nullptr;
|
||||
return _detect(compass, bus);
|
||||
}
|
||||
|
||||
AP_Compass_Backend *AP_Compass_HMC5843::_detect(Compass &compass,
|
||||
AP_HMC5843_SerialBus *bus)
|
||||
{
|
||||
AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass, bus);
|
||||
if (!sensor) {
|
||||
delete bus;
|
||||
return nullptr;
|
||||
}
|
||||
if (!sensor->init()) {
|
||||
delete sensor;
|
||||
return NULL;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
// read_register - read a register value
|
||||
bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
|
||||
{
|
||||
if (hal.i2c->readRegister((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
|
||||
if (_bus->register_read(address, value) != 0) {
|
||||
_retry_time = hal.scheduler->millis() + 1000;
|
||||
return false;
|
||||
}
|
||||
@ -101,7 +118,7 @@ bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
|
||||
// write_register - update a register value
|
||||
bool AP_Compass_HMC5843::write_register(uint8_t address, uint8_t value)
|
||||
{
|
||||
if (hal.i2c->writeRegister((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
|
||||
if (_bus->register_write(address, value) != 0) {
|
||||
_retry_time = hal.scheduler->millis() + 1000;
|
||||
return false;
|
||||
}
|
||||
@ -113,8 +130,8 @@ bool AP_Compass_HMC5843::read_raw()
|
||||
{
|
||||
uint8_t buff[6];
|
||||
|
||||
if (hal.i2c->readRegisters(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
|
||||
hal.i2c->setHighSpeed(false);
|
||||
if (_bus->register_read(0x03, buff, 6) != 0) {
|
||||
_bus->set_high_speed(false);
|
||||
_retry_time = hal.scheduler->millis() + 1000;
|
||||
return false;
|
||||
}
|
||||
@ -156,12 +173,12 @@ void AP_Compass_HMC5843::accumulate(void)
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_i2c_sem->take(1)) {
|
||||
if (!_bus_sem->take(1)) {
|
||||
// the bus is busy - try again later
|
||||
return;
|
||||
}
|
||||
bool result = read_raw();
|
||||
_i2c_sem->give();
|
||||
_bus_sem->give();
|
||||
|
||||
if (result) {
|
||||
// the _mag_N values are in the range -2048 to 2047, so we can
|
||||
@ -230,8 +247,8 @@ AP_Compass_HMC5843::init()
|
||||
hal.scheduler->suspend_timer_procs();
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
_i2c_sem = hal.i2c->get_semaphore();
|
||||
if (!_i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
_bus_sem = _bus->get_semaphore();
|
||||
if (!_bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
hal.scheduler->panic(PSTR("Failed to get HMC5843 semaphore"));
|
||||
}
|
||||
|
||||
@ -261,7 +278,7 @@ AP_Compass_HMC5843::init()
|
||||
}
|
||||
|
||||
_initialised = true;
|
||||
_i2c_sem->give();
|
||||
_bus_sem->give();
|
||||
hal.scheduler->resume_timer_procs();
|
||||
|
||||
// perform an initial read
|
||||
@ -278,7 +295,7 @@ AP_Compass_HMC5843::init()
|
||||
return true;
|
||||
|
||||
errout:
|
||||
_i2c_sem->give();
|
||||
_bus_sem->give();
|
||||
hal.scheduler->resume_timer_procs();
|
||||
return false;
|
||||
}
|
||||
@ -395,7 +412,7 @@ void AP_Compass_HMC5843::read()
|
||||
}
|
||||
if (!re_initialise()) {
|
||||
_retry_time = hal.scheduler->millis() + 1000;
|
||||
hal.i2c->setHighSpeed(false);
|
||||
_bus->set_high_speed(false);
|
||||
return;
|
||||
}
|
||||
}
|
||||
@ -403,7 +420,7 @@ void AP_Compass_HMC5843::read()
|
||||
if (_accum_count == 0) {
|
||||
accumulate();
|
||||
if (_retry_time != 0) {
|
||||
hal.i2c->setHighSpeed(false);
|
||||
_bus->set_high_speed(false);
|
||||
return;
|
||||
}
|
||||
}
|
||||
@ -424,3 +441,29 @@ void AP_Compass_HMC5843::read()
|
||||
publish_field(field, _compass_instance);
|
||||
_retry_time = 0;
|
||||
}
|
||||
|
||||
AP_HMC5843_SerialBus_I2C::AP_HMC5843_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr)
|
||||
: _i2c(i2c)
|
||||
, _addr(addr)
|
||||
{
|
||||
}
|
||||
|
||||
void AP_HMC5843_SerialBus_I2C::set_high_speed(bool val)
|
||||
{
|
||||
_i2c->setHighSpeed(val);
|
||||
}
|
||||
|
||||
uint8_t AP_HMC5843_SerialBus_I2C::register_read(uint8_t reg, uint8_t *buf, uint8_t size)
|
||||
{
|
||||
return _i2c->readRegisters(_addr, reg, size, buf);
|
||||
}
|
||||
|
||||
uint8_t AP_HMC5843_SerialBus_I2C::register_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
return _i2c->writeRegister(_addr, reg, val);
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore* AP_HMC5843_SerialBus_I2C::get_semaphore()
|
||||
{
|
||||
return _i2c->get_semaphore();
|
||||
}
|
||||
|
||||
@ -9,9 +9,17 @@
|
||||
#include "Compass.h"
|
||||
#include "AP_Compass_Backend.h"
|
||||
|
||||
class AP_HMC5843_SerialBus;
|
||||
|
||||
class AP_Compass_HMC5843 : public AP_Compass_Backend
|
||||
{
|
||||
private:
|
||||
static AP_Compass_Backend *_detect(Compass &compass,
|
||||
AP_HMC5843_SerialBus *bus);
|
||||
|
||||
AP_HMC5843_SerialBus *_bus;
|
||||
AP_HAL::Semaphore *_bus_sem = nullptr;
|
||||
|
||||
float _scaling[3] = {0};
|
||||
bool _initialised;
|
||||
bool read_raw(void);
|
||||
@ -24,7 +32,6 @@ private:
|
||||
bool _detect_version();
|
||||
|
||||
uint32_t _retry_time; // when unhealthy the millis() value to retry at
|
||||
AP_HAL::Semaphore* _i2c_sem;
|
||||
|
||||
int16_t _mag_x;
|
||||
int16_t _mag_y;
|
||||
@ -39,13 +46,45 @@ private:
|
||||
uint8_t _product_id;
|
||||
|
||||
public:
|
||||
AP_Compass_HMC5843(Compass &compass);
|
||||
// detect the sensor
|
||||
static AP_Compass_Backend *detect_i2c(Compass &compass,
|
||||
AP_HAL::I2CDriver *i2c);
|
||||
|
||||
AP_Compass_HMC5843(Compass &compass, AP_HMC5843_SerialBus *bus);
|
||||
~AP_Compass_HMC5843();
|
||||
|
||||
bool init(void);
|
||||
void read(void);
|
||||
void accumulate(void);
|
||||
|
||||
// detect the sensor
|
||||
static AP_Compass_Backend *detect(Compass &compass);
|
||||
|
||||
};
|
||||
|
||||
class AP_HMC5843_SerialBus
|
||||
{
|
||||
public:
|
||||
virtual ~AP_HMC5843_SerialBus() { };
|
||||
virtual void set_high_speed(bool val) = 0;
|
||||
virtual uint8_t register_read(uint8_t reg, uint8_t *buf, uint8_t size) = 0;
|
||||
uint8_t register_read(uint8_t reg, uint8_t *val)
|
||||
{
|
||||
return register_read(reg, val, 1);
|
||||
}
|
||||
|
||||
virtual uint8_t register_write(uint8_t reg, uint8_t val) = 0;
|
||||
virtual AP_HAL::Semaphore* get_semaphore() = 0;
|
||||
};
|
||||
|
||||
class AP_HMC5843_SerialBus_I2C : public AP_HMC5843_SerialBus
|
||||
{
|
||||
public:
|
||||
AP_HMC5843_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
|
||||
void set_high_speed(bool val) override;
|
||||
uint8_t register_read(uint8_t reg, uint8_t *buf, uint8_t size) override;
|
||||
uint8_t register_write(uint8_t reg, uint8_t val) override;
|
||||
AP_HAL::Semaphore* get_semaphore() override;
|
||||
|
||||
private:
|
||||
AP_HAL::I2CDriver *_i2c;
|
||||
uint8_t _addr;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -341,12 +341,12 @@ void Compass::_detect_backends(void)
|
||||
}
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_BEBOP
|
||||
_add_backend(AP_Compass_HMC5843::detect(*this));
|
||||
_add_backend(AP_Compass_HMC5843::detect_i2c(*this, hal.i2c));
|
||||
_add_backend(AP_Compass_AK8963::detect_mpu9250(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
|
||||
_add_backend(AP_Compass_HIL::detect(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843
|
||||
_add_backend(AP_Compass_HMC5843::detect(*this));
|
||||
_add_backend(AP_Compass_HMC5843::detect_i2c(*this, hal.i2c));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C && HAL_INS_AK8963_I2C_BUS == 1
|
||||
_add_backend(AP_Compass_AK8963::detect_i2c(*this, hal.i2c1,
|
||||
HAL_COMPASS_AK8963_I2C_ADDR));
|
||||
|
||||
Loading…
Reference in New Issue
Block a user