mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-23 09:08:30 -04:00
AP_Compass: HMC5843: 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.
Besides the usual conversion some more improvements:
- Use generic function to convert endianness
- Minor cleanups
- Reorder per-board ifdefs in compass instantiation: distinguish when
there's a default compass to when it should probe other compasses
This commit is contained in:
Lucas De Marchi
parent
5411057ec6
commit
0291ad869b
@ -22,9 +22,16 @@
|
||||
* Sensor is initialized in Continuos mode (10Hz)
|
||||
*
|
||||
*/
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#ifdef HAL_COMPASS_HMC5843_I2C_ADDR
|
||||
|
||||
#include <assert.h>
|
||||
#include <utility>
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
|
||||
#include "AP_Compass_HMC5843.h"
|
||||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||
@ -58,56 +65,29 @@ extern const AP_HAL::HAL& hal;
|
||||
#define DataOutputRate_30HZ 0x05
|
||||
#define DataOutputRate_75HZ 0x06
|
||||
|
||||
// constructor
|
||||
AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass, AP_HMC5843_SerialBus *bus) :
|
||||
AP_Compass_Backend(compass),
|
||||
_bus(bus),
|
||||
_retry_time(0),
|
||||
_mag_x(0),
|
||||
_mag_y(0),
|
||||
_mag_z(0),
|
||||
_mag_x_accum(0),
|
||||
_mag_y_accum(0),
|
||||
_mag_z_accum(0),
|
||||
_accum_count(0),
|
||||
_last_accum_time(0),
|
||||
_compass_instance(0),
|
||||
_product_id(0)
|
||||
{}
|
||||
#define REG_DATA_OUTPUT_X_MSB 0x03
|
||||
|
||||
AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass, AP_HMC5843_BusDriver *bus)
|
||||
: AP_Compass_Backend(compass)
|
||||
, _bus(bus)
|
||||
{
|
||||
}
|
||||
|
||||
AP_Compass_HMC5843::~AP_Compass_HMC5843()
|
||||
{
|
||||
delete _bus;
|
||||
}
|
||||
|
||||
// detect the sensor
|
||||
AP_Compass_Backend *AP_Compass_HMC5843::detect_i2c(Compass &compass,
|
||||
AP_HAL::I2CDriver *i2c)
|
||||
AP_Compass_Backend *AP_Compass_HMC5843::probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
|
||||
{
|
||||
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_mpu6000(Compass &compass)
|
||||
{
|
||||
AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
|
||||
AP_HMC5843_SerialBus *bus = new AP_HMC5843_SerialBus_MPU6000(ins, 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;
|
||||
AP_HMC5843_BusDriver *bus = new AP_HMC5843_BusDriver_HALDevice(std::move(dev));
|
||||
if (!bus) {
|
||||
return nullptr;
|
||||
}
|
||||
if (!sensor->init()) {
|
||||
|
||||
AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass, bus);
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
@ -115,166 +95,32 @@ AP_Compass_Backend *AP_Compass_HMC5843::_detect(Compass &compass,
|
||||
return sensor;
|
||||
}
|
||||
|
||||
// read_register - read a register value
|
||||
bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
|
||||
AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(Compass &compass)
|
||||
{
|
||||
if (_bus->register_read(address, value) != 0) {
|
||||
_retry_time = AP_HAL::millis() + 1000;
|
||||
return false;
|
||||
AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
|
||||
|
||||
AP_HMC5843_BusDriver *bus =
|
||||
new AP_HMC5843_BusDriver_Auxiliary(ins, HAL_INS_MPU60XX_SPI,
|
||||
HAL_COMPASS_HMC5843_I2C_ADDR);
|
||||
if (!bus) {
|
||||
return nullptr;
|
||||
}
|
||||
return true;
|
||||
|
||||
AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass, bus);
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
// write_register - update a register value
|
||||
bool AP_Compass_HMC5843::write_register(uint8_t address, uint8_t value)
|
||||
bool AP_Compass_HMC5843::init()
|
||||
{
|
||||
if (_bus->register_write(address, value) != 0) {
|
||||
_retry_time = AP_HAL::millis() + 1000;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
bool AP_Compass_HMC5843::read_raw()
|
||||
{
|
||||
struct AP_HMC5843_SerialBus::raw_value rv;
|
||||
|
||||
if (_bus->read_raw(&rv) != 0) {
|
||||
_bus->set_high_speed(false);
|
||||
_retry_time = AP_HAL::millis() + 1000;
|
||||
return false;
|
||||
}
|
||||
|
||||
int16_t rx, ry, rz;
|
||||
rx = (((int16_t)rv.val[0]) << 8) | rv.val[1];
|
||||
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
|
||||
rz = (((int16_t)rv.val[2]) << 8) | rv.val[3];
|
||||
ry = (((int16_t)rv.val[4]) << 8) | rv.val[5];
|
||||
} else {
|
||||
ry = (((int16_t)rv.val[2]) << 8) | rv.val[3];
|
||||
rz = (((int16_t)rv.val[4]) << 8) | rv.val[5];
|
||||
}
|
||||
if (rx == -4096 || ry == -4096 || rz == -4096) {
|
||||
// no valid data available
|
||||
return false;
|
||||
}
|
||||
|
||||
_mag_x = -rx;
|
||||
_mag_y = ry;
|
||||
_mag_z = -rz;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// accumulate a reading from the magnetometer
|
||||
void AP_Compass_HMC5843::accumulate(void)
|
||||
{
|
||||
if (!_initialised) {
|
||||
// someone has tried to enable a compass for the first time
|
||||
// mid-flight .... we can't do that yet (especially as we won't
|
||||
// have the right orientation!)
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t tnow = AP_HAL::micros();
|
||||
if (_accum_count != 0 && (tnow - _last_accum_time) < 13333) {
|
||||
// the compass gets new data at 75Hz
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_bus_sem->take(1)) {
|
||||
// the bus is busy - try again later
|
||||
return;
|
||||
}
|
||||
bool result = read_raw();
|
||||
_bus_sem->give();
|
||||
|
||||
if (result) {
|
||||
// the _mag_N values are in the range -2048 to 2047, so we can
|
||||
// accumulate up to 15 of them in an int16_t. Let's make it 14
|
||||
// for ease of calculation. We expect to do reads at 10Hz, and
|
||||
// we get new data at most 75Hz, so we don't expect to
|
||||
// accumulate more than 8 before a read
|
||||
// get raw_field - sensor frame, uncorrected
|
||||
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z);
|
||||
raw_field *= _gain_multiple;
|
||||
|
||||
// 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, tnow, _compass_instance);
|
||||
|
||||
// correct raw_field for known errors
|
||||
correct_field(raw_field, _compass_instance);
|
||||
|
||||
// publish raw_field (corrected point sample) for EKF use
|
||||
publish_unfiltered_field(raw_field, tnow, _compass_instance);
|
||||
|
||||
_mag_x_accum += raw_field.x;
|
||||
_mag_y_accum += raw_field.y;
|
||||
_mag_z_accum += raw_field.z;
|
||||
_accum_count++;
|
||||
if (_accum_count == 14) {
|
||||
_mag_x_accum /= 2;
|
||||
_mag_y_accum /= 2;
|
||||
_mag_z_accum /= 2;
|
||||
_accum_count = 7;
|
||||
}
|
||||
_last_accum_time = tnow;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* re-initialise after a IO error
|
||||
*/
|
||||
bool AP_Compass_HMC5843::re_initialise()
|
||||
{
|
||||
if (!write_register(ConfigRegA, _base_config) ||
|
||||
!write_register(ConfigRegB, magGain) ||
|
||||
!write_register(ModeRegister, ContinuousConversion))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool AP_Compass_HMC5843::_detect_version()
|
||||
{
|
||||
_base_config = 0x0;
|
||||
|
||||
if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
|
||||
!read_register(ConfigRegA, &_base_config)) {
|
||||
return false;
|
||||
}
|
||||
if (_base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
|
||||
/* a 5883L supports the sample averaging config */
|
||||
_product_id = AP_COMPASS_TYPE_HMC5883L;
|
||||
return true;
|
||||
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
|
||||
_product_id = AP_COMPASS_TYPE_HMC5843;
|
||||
return true;
|
||||
} else {
|
||||
/* not behaving like either supported compass type */
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
bool
|
||||
AP_Compass_HMC5843::init()
|
||||
{
|
||||
uint8_t calibration_gain = 0x20;
|
||||
uint16_t expected_x = 715;
|
||||
uint16_t expected_yz = 715;
|
||||
_gain_multiple = (1.0f / 1300) * 1000;
|
||||
|
||||
_bus_sem = _bus->get_semaphore();
|
||||
hal.scheduler->suspend_timer_procs();
|
||||
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
|
||||
|
||||
if (!_bus_sem || !_bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
hal.console->printf("HMC5843: Unable to get bus semaphore\n");
|
||||
goto fail_sem;
|
||||
}
|
||||
@ -289,6 +135,239 @@ AP_Compass_HMC5843::init()
|
||||
goto errout;
|
||||
}
|
||||
|
||||
if (!_calibrate()) {
|
||||
hal.console->printf("HMC5843: Could not calibrate sensor\n");
|
||||
goto errout;
|
||||
}
|
||||
|
||||
if (!_re_initialize()) {
|
||||
goto errout;
|
||||
}
|
||||
|
||||
if (!_bus->start_measurements()) {
|
||||
hal.console->printf("HMC5843: Could not start measurements on bus\n");
|
||||
goto errout;
|
||||
}
|
||||
|
||||
_initialised = true;
|
||||
|
||||
bus_sem->give();
|
||||
hal.scheduler->resume_timer_procs();
|
||||
|
||||
// perform an initial read
|
||||
read();
|
||||
|
||||
_compass_instance = register_compass();
|
||||
set_dev_id(_compass_instance, _product_id);
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
||||
// FIXME: wrong way to force external compass
|
||||
set_external(_compass_instance, true);
|
||||
#endif
|
||||
|
||||
return true;
|
||||
|
||||
errout:
|
||||
bus_sem->give();
|
||||
|
||||
fail_sem:
|
||||
hal.scheduler->resume_timer_procs();
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* Accumulate a reading from the magnetometer
|
||||
*
|
||||
* bus semaphore must not be taken
|
||||
*/
|
||||
void AP_Compass_HMC5843::accumulate()
|
||||
{
|
||||
if (!_initialised) {
|
||||
// someone has tried to enable a compass for the first time
|
||||
// mid-flight .... we can't do that yet (especially as we won't
|
||||
// have the right orientation!)
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t tnow = AP_HAL::micros();
|
||||
if (_accum_count != 0 && (tnow - _last_accum_time) < 13333) {
|
||||
// the compass gets new data at 75Hz
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_bus->get_semaphore()->take(1)) {
|
||||
// the bus is busy - try again later
|
||||
return;
|
||||
}
|
||||
|
||||
bool result = _read_sample();
|
||||
|
||||
_bus->get_semaphore()->give();
|
||||
|
||||
if (!result) {
|
||||
return;
|
||||
}
|
||||
|
||||
// the _mag_N values are in the range -2048 to 2047, so we can
|
||||
// accumulate up to 15 of them in an int16_t. Let's make it 14
|
||||
// for ease of calculation. We expect to do reads at 10Hz, and
|
||||
// we get new data at most 75Hz, so we don't expect to
|
||||
// accumulate more than 8 before a read
|
||||
// get raw_field - sensor frame, uncorrected
|
||||
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z);
|
||||
raw_field *= _gain_multiple;
|
||||
|
||||
// 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, tnow, _compass_instance);
|
||||
|
||||
// correct raw_field for known errors
|
||||
correct_field(raw_field, _compass_instance);
|
||||
|
||||
// publish raw_field (corrected point sample) for EKF use
|
||||
publish_unfiltered_field(raw_field, tnow, _compass_instance);
|
||||
|
||||
_mag_x_accum += raw_field.x;
|
||||
_mag_y_accum += raw_field.y;
|
||||
_mag_z_accum += raw_field.z;
|
||||
_accum_count++;
|
||||
if (_accum_count == 14) {
|
||||
_mag_x_accum /= 2;
|
||||
_mag_y_accum /= 2;
|
||||
_mag_z_accum /= 2;
|
||||
_accum_count = 7;
|
||||
}
|
||||
_last_accum_time = tnow;
|
||||
}
|
||||
|
||||
/*
|
||||
* Take accumulated reads from the magnetometer or try to read once if no
|
||||
* valid data
|
||||
*
|
||||
* bus semaphore must not be locked
|
||||
*/
|
||||
void AP_Compass_HMC5843::read()
|
||||
{
|
||||
if (!_initialised) {
|
||||
// someone has tried to enable a compass for the first time
|
||||
// mid-flight .... we can't do that yet (especially as we won't
|
||||
// have the right orientation!)
|
||||
return;
|
||||
}
|
||||
|
||||
if (_accum_count == 0) {
|
||||
accumulate();
|
||||
if (_retry_time != 0) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3f field(_mag_x_accum * _scaling[0],
|
||||
_mag_y_accum * _scaling[1],
|
||||
_mag_z_accum * _scaling[2]);
|
||||
field /= _accum_count;
|
||||
|
||||
_accum_count = 0;
|
||||
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
|
||||
|
||||
// rotate to the desired orientation
|
||||
// FIXME: wrong way to rotate compass
|
||||
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
|
||||
field.rotate(ROTATION_YAW_90);
|
||||
}
|
||||
|
||||
publish_filtered_field(field, _compass_instance);
|
||||
}
|
||||
|
||||
|
||||
bool AP_Compass_HMC5843::_re_initialize()
|
||||
{
|
||||
if (!_bus->register_write(ConfigRegA, _base_config) ||
|
||||
!_bus->register_write(ConfigRegB, magGain) ||
|
||||
!_bus->register_write(ModeRegister, ContinuousConversion)) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* Read Sensor data - bus semaphore must be taken
|
||||
*/
|
||||
bool AP_Compass_HMC5843::_read_sample()
|
||||
{
|
||||
struct PACKED {
|
||||
be16_t rx;
|
||||
be16_t ry;
|
||||
be16_t rz;
|
||||
} val;
|
||||
int16_t rx, ry, rz;
|
||||
|
||||
if (_retry_time > AP_HAL::millis()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_bus->block_read(REG_DATA_OUTPUT_X_MSB, (uint8_t *) &val, sizeof(val))){
|
||||
_retry_time = AP_HAL::millis() + 1000;
|
||||
return false;
|
||||
}
|
||||
|
||||
rx = be16toh(val.rx);
|
||||
ry = be16toh(val.ry);
|
||||
rz = be16toh(val.rz);
|
||||
|
||||
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
|
||||
std::swap(ry, rz);
|
||||
}
|
||||
|
||||
if (rx == -4096 || ry == -4096 || rz == -4096) {
|
||||
// no valid data available
|
||||
return false;
|
||||
}
|
||||
|
||||
_mag_x = -rx;
|
||||
_mag_y = ry;
|
||||
_mag_z = -rz;
|
||||
|
||||
_retry_time = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_Compass_HMC5843::_detect_version()
|
||||
{
|
||||
_base_config = 0x0;
|
||||
|
||||
if (!_bus->register_write(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
|
||||
!_bus->register_read(ConfigRegA, &_base_config)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
|
||||
/* a 5883L supports the sample averaging config */
|
||||
_product_id = AP_COMPASS_TYPE_HMC5883L;
|
||||
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
|
||||
_product_id = AP_COMPASS_TYPE_HMC5843;
|
||||
} else {
|
||||
/* not behaving like either supported compass type */
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_Compass_HMC5843::_calibrate()
|
||||
{
|
||||
uint8_t calibration_gain = 0x20;
|
||||
uint16_t expected_x = 715;
|
||||
uint16_t expected_yz = 715;
|
||||
int numAttempts = 0, good_count = 0;
|
||||
bool success = false;
|
||||
|
||||
_gain_multiple = (1.0f / 1300) * 1000;
|
||||
|
||||
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
|
||||
calibration_gain = 0x60;
|
||||
/*
|
||||
@ -301,75 +380,26 @@ AP_Compass_HMC5843::init()
|
||||
_gain_multiple = (1.0f / 1090) * 1000;
|
||||
}
|
||||
|
||||
if (!_calibrate(calibration_gain, expected_x, expected_yz)) {
|
||||
hal.console->printf("HMC5843: Could not calibrate sensor\n");
|
||||
goto errout;
|
||||
}
|
||||
|
||||
// leave test mode
|
||||
if (!re_initialise()) {
|
||||
goto errout;
|
||||
}
|
||||
|
||||
if (!_bus->start_measurements()) {
|
||||
hal.console->printf("HMC5843: Could not start measurements on bus\n");
|
||||
goto errout;
|
||||
}
|
||||
_initialised = true;
|
||||
|
||||
_bus_sem->give();
|
||||
hal.scheduler->resume_timer_procs();
|
||||
|
||||
// perform an initial read
|
||||
read();
|
||||
|
||||
#if 0
|
||||
hal.console->printf("CalX: %.2f CalY: %.2f CalZ: %.2f\n",
|
||||
_scaling[0], _scaling[1], _scaling[2]);
|
||||
#endif
|
||||
|
||||
_compass_instance = register_compass();
|
||||
set_dev_id(_compass_instance, _product_id);
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
||||
set_external(_compass_instance, true);
|
||||
#endif
|
||||
|
||||
return true;
|
||||
|
||||
errout:
|
||||
_bus_sem->give();
|
||||
fail_sem:
|
||||
hal.scheduler->resume_timer_procs();
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
|
||||
uint16_t expected_x,
|
||||
uint16_t expected_yz)
|
||||
{
|
||||
int numAttempts = 0, good_count = 0;
|
||||
bool success = false;
|
||||
|
||||
while (success == 0 && numAttempts < 25 && good_count < 5)
|
||||
{
|
||||
while (success == 0 && numAttempts < 25 && good_count < 5) {
|
||||
numAttempts++;
|
||||
|
||||
// force positiveBias (compass should return 715 for all channels)
|
||||
if (!write_register(ConfigRegA, PositiveBiasConfig))
|
||||
if (!_bus->register_write(ConfigRegA, PositiveBiasConfig))
|
||||
continue; // compass not responding on the bus
|
||||
|
||||
hal.scheduler->delay(50);
|
||||
|
||||
// set gains
|
||||
if (!write_register(ConfigRegB, calibration_gain) ||
|
||||
!write_register(ModeRegister, SingleConversion))
|
||||
if (!_bus->register_write(ConfigRegB, calibration_gain) ||
|
||||
!_bus->register_write(ModeRegister, SingleConversion))
|
||||
continue;
|
||||
|
||||
// read values from the compass
|
||||
hal.scheduler->delay(50);
|
||||
if (!read_raw())
|
||||
continue; // we didn't read valid values
|
||||
if (!_read_sample()) {
|
||||
// we didn't read valid values
|
||||
continue;
|
||||
}
|
||||
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
@ -429,143 +459,107 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
|
||||
return success;
|
||||
}
|
||||
|
||||
// Read Sensor data
|
||||
void AP_Compass_HMC5843::read()
|
||||
/* AP_HAL::I2CDevice implementation of the HMC5843 */
|
||||
AP_HMC5843_BusDriver_HALDevice::AP_HMC5843_BusDriver_HALDevice(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
|
||||
: _dev(std::move(dev))
|
||||
{
|
||||
if (!_initialised) {
|
||||
// someone has tried to enable a compass for the first time
|
||||
// mid-flight .... we can't do that yet (especially as we won't
|
||||
// have the right orientation!)
|
||||
}
|
||||
|
||||
bool AP_HMC5843_BusDriver_HALDevice::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
|
||||
{
|
||||
return _dev->read_registers(reg, buf, size);
|
||||
}
|
||||
|
||||
bool AP_HMC5843_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
|
||||
{
|
||||
return _dev->read_registers(reg, val, 1);
|
||||
}
|
||||
|
||||
bool AP_HMC5843_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
return _dev->write_register(reg, val);
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore *AP_HMC5843_BusDriver_HALDevice::get_semaphore()
|
||||
{
|
||||
return _dev->get_semaphore();
|
||||
}
|
||||
|
||||
/* HMC5843 on an auxiliary bus of IMU driver */
|
||||
AP_HMC5843_BusDriver_Auxiliary::AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
|
||||
uint8_t addr)
|
||||
{
|
||||
/*
|
||||
* Only initialize members. Fails are handled by configure or while
|
||||
* getting the semaphore
|
||||
*/
|
||||
_bus = ins.get_auxiliary_bus(backend_id);
|
||||
if (!_bus) {
|
||||
return;
|
||||
}
|
||||
if (_retry_time != 0) {
|
||||
if (AP_HAL::millis() < _retry_time) {
|
||||
return;
|
||||
}
|
||||
if (!re_initialise()) {
|
||||
_retry_time = AP_HAL::millis() + 1000;
|
||||
_bus->set_high_speed(false);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (_accum_count == 0) {
|
||||
accumulate();
|
||||
if (_retry_time != 0) {
|
||||
_bus->set_high_speed(false);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3f field(_mag_x_accum * _scaling[0],
|
||||
_mag_y_accum * _scaling[1],
|
||||
_mag_z_accum * _scaling[2]);
|
||||
field /= _accum_count;
|
||||
|
||||
_accum_count = 0;
|
||||
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
|
||||
|
||||
// rotate to the desired orientation
|
||||
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
|
||||
field.rotate(ROTATION_YAW_90);
|
||||
}
|
||||
|
||||
publish_filtered_field(field, _compass_instance);
|
||||
_retry_time = 0;
|
||||
}
|
||||
|
||||
/* I2C implementation of the HMC5843 */
|
||||
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();
|
||||
}
|
||||
|
||||
uint8_t AP_HMC5843_SerialBus_I2C::read_raw(struct raw_value *rv)
|
||||
{
|
||||
return register_read(0x03, (uint8_t*)rv, sizeof(*rv));
|
||||
}
|
||||
|
||||
|
||||
/* MPU6000 implementation of the HMC5843 */
|
||||
AP_HMC5843_SerialBus_MPU6000::AP_HMC5843_SerialBus_MPU6000(AP_InertialSensor &ins,
|
||||
uint8_t addr)
|
||||
{
|
||||
// Only initialize members. Fails are handled by configure or while
|
||||
// getting the semaphore
|
||||
_bus = ins.get_auxiliary_bus(HAL_INS_MPU60XX_SPI);
|
||||
if (!_bus)
|
||||
return;
|
||||
_slave = _bus->request_next_slave(addr);
|
||||
}
|
||||
|
||||
AP_HMC5843_SerialBus_MPU6000::~AP_HMC5843_SerialBus_MPU6000()
|
||||
AP_HMC5843_BusDriver_Auxiliary::~AP_HMC5843_BusDriver_Auxiliary()
|
||||
{
|
||||
/* After started it's owned by AuxiliaryBus */
|
||||
if (!_started)
|
||||
if (!_started) {
|
||||
delete _slave;
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_HMC5843_SerialBus_MPU6000::configure()
|
||||
bool AP_HMC5843_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
|
||||
{
|
||||
if (!_bus || !_slave)
|
||||
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_DATA_OUTPUT_X_MSB);
|
||||
|
||||
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_HMC5843_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
|
||||
{
|
||||
return _slave->passthrough_read(reg, val, 1) == 1;
|
||||
}
|
||||
|
||||
bool AP_HMC5843_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
return _slave->passthrough_write(reg, val) == 1;
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore *AP_HMC5843_BusDriver_Auxiliary::get_semaphore()
|
||||
{
|
||||
return _bus->get_semaphore();
|
||||
}
|
||||
|
||||
|
||||
bool AP_HMC5843_BusDriver_Auxiliary::configure()
|
||||
{
|
||||
if (!_bus || !_slave) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_HMC5843_SerialBus_MPU6000::set_high_speed(bool val)
|
||||
bool AP_HMC5843_BusDriver_Auxiliary::start_measurements()
|
||||
{
|
||||
}
|
||||
|
||||
uint8_t AP_HMC5843_SerialBus_MPU6000::register_read(uint8_t reg, uint8_t *buf, uint8_t size)
|
||||
{
|
||||
return _slave->passthrough_read(reg, buf, size) == size ? 0 : 1;
|
||||
}
|
||||
|
||||
uint8_t AP_HMC5843_SerialBus_MPU6000::register_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
return _slave->passthrough_write(reg, val) >= 0 ? 0 : 1;
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore* AP_HMC5843_SerialBus_MPU6000::get_semaphore()
|
||||
{
|
||||
return _bus ? _bus->get_semaphore() : nullptr;
|
||||
}
|
||||
|
||||
uint8_t AP_HMC5843_SerialBus_MPU6000::read_raw(struct raw_value *rv)
|
||||
{
|
||||
if (_started)
|
||||
return _slave->read((uint8_t*)rv) >= 0 ? 0 : 1;
|
||||
|
||||
return _slave->passthrough_read(0x03, (uint8_t*)rv, sizeof(*rv)) >= 0 ? 0 : 1;
|
||||
}
|
||||
|
||||
bool AP_HMC5843_SerialBus_MPU6000::start_measurements()
|
||||
{
|
||||
if (_bus->register_periodic_read(_slave, 0x03, sizeof(struct raw_value)) < 0)
|
||||
if (_bus->register_periodic_read(_slave, REG_DATA_OUTPUT_X_MSB, 6) < 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_started = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,8 +1,9 @@
|
||||
/// -*- 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_Math/AP_Math.h>
|
||||
|
||||
#include "Compass.h"
|
||||
@ -11,37 +12,33 @@
|
||||
class AuxiliaryBus;
|
||||
class AuxiliaryBusSlave;
|
||||
class AP_InertialSensor;
|
||||
class AP_HMC5843_SerialBus;
|
||||
class AP_HMC5843_BusDriver;
|
||||
|
||||
class AP_Compass_HMC5843 : public AP_Compass_Backend
|
||||
{
|
||||
public:
|
||||
static AP_Compass_Backend *detect_i2c(Compass &compass,
|
||||
AP_HAL::I2CDriver *i2c);
|
||||
static AP_Compass_Backend *detect_mpu6000(Compass &compass);
|
||||
static AP_Compass_Backend *probe(Compass &compass,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
|
||||
|
||||
AP_Compass_HMC5843(Compass &compass, AP_HMC5843_SerialBus *bus);
|
||||
~AP_Compass_HMC5843();
|
||||
static AP_Compass_Backend *probe_mpu6000(Compass &compass);
|
||||
|
||||
bool init();
|
||||
void read();
|
||||
void accumulate();
|
||||
virtual ~AP_Compass_HMC5843();
|
||||
|
||||
bool init() override;
|
||||
void read() override;
|
||||
void accumulate() override;
|
||||
|
||||
private:
|
||||
static AP_Compass_Backend *_detect(Compass &compass,
|
||||
AP_HMC5843_SerialBus *bus);
|
||||
AP_Compass_HMC5843(Compass &compass, AP_HMC5843_BusDriver *bus);
|
||||
|
||||
bool read_raw();
|
||||
bool re_initialise();
|
||||
bool read_register(uint8_t address, uint8_t *value);
|
||||
bool write_register(uint8_t address, uint8_t value);
|
||||
|
||||
bool _calibrate(uint8_t calibration_gain, uint16_t expected_x,
|
||||
uint16_t expected_yz);
|
||||
bool _detect_version();
|
||||
bool _calibrate();
|
||||
bool _re_initialize();
|
||||
|
||||
AP_HMC5843_SerialBus *_bus;
|
||||
AP_HAL::Semaphore *_bus_sem;
|
||||
/* Read a single sample */
|
||||
bool _read_sample();
|
||||
|
||||
AP_HMC5843_BusDriver *_bus;
|
||||
|
||||
float _scaling[3];
|
||||
float _gain_multiple;
|
||||
@ -65,53 +62,49 @@ private:
|
||||
bool _initialised;
|
||||
};
|
||||
|
||||
class AP_HMC5843_SerialBus
|
||||
class AP_HMC5843_BusDriver
|
||||
{
|
||||
public:
|
||||
struct PACKED raw_value {
|
||||
uint8_t val[6];
|
||||
};
|
||||
virtual ~AP_HMC5843_BusDriver() { }
|
||||
|
||||
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 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 uint8_t register_write(uint8_t reg, uint8_t val) = 0;
|
||||
virtual AP_HAL::Semaphore* get_semaphore() = 0;
|
||||
virtual uint8_t read_raw(struct raw_value *rv) = 0;
|
||||
virtual bool configure() { return true; }
|
||||
virtual bool start_measurements() { return true; }
|
||||
};
|
||||
|
||||
class AP_HMC5843_SerialBus_I2C : public AP_HMC5843_SerialBus
|
||||
class AP_HMC5843_BusDriver_HALDevice : public AP_HMC5843_BusDriver
|
||||
{
|
||||
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;
|
||||
uint8_t read_raw(struct raw_value *rv) override;
|
||||
AP_HMC5843_BusDriver_HALDevice(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
|
||||
|
||||
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;
|
||||
|
||||
private:
|
||||
AP_HAL::I2CDriver *_i2c;
|
||||
uint8_t _addr;
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
|
||||
};
|
||||
|
||||
class AP_HMC5843_SerialBus_MPU6000 : public AP_HMC5843_SerialBus
|
||||
class AP_HMC5843_BusDriver_Auxiliary : public AP_HMC5843_BusDriver
|
||||
{
|
||||
public:
|
||||
AP_HMC5843_SerialBus_MPU6000(AP_InertialSensor &ins, uint8_t addr);
|
||||
~AP_HMC5843_SerialBus_MPU6000();
|
||||
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;
|
||||
uint8_t read_raw(struct raw_value *rv) override;
|
||||
AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
|
||||
uint8_t addr);
|
||||
virtual ~AP_HMC5843_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() override;
|
||||
bool start_measurements() override;
|
||||
|
||||
|
||||
@ -424,45 +424,41 @@ void Compass::_detect_backends(void)
|
||||
return;
|
||||
}
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
||||
_add_backend(AP_Compass_HMC5843::detect_i2c(*this, hal.i2c));
|
||||
#if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
|
||||
_add_backend(AP_Compass_HIL::detect(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
|
||||
_add_backend(AP_Compass_PX4::detect(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT
|
||||
_add_backend(AP_Compass_QURT::detect(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_RASPILOT
|
||||
_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_LSM303D::detect_spi(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH
|
||||
// detect_mpu9250() failed will cause panic if no actual mpu9250 backend,
|
||||
// in BH, only one compass should be detected
|
||||
AP_Compass_Backend *backend = AP_Compass_HMC5843::detect_i2c(*this, hal.i2c);
|
||||
AP_Compass_Backend *backend = AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR));
|
||||
if (backend) {
|
||||
_add_backend(backend);
|
||||
} else {
|
||||
_add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0));
|
||||
}
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && \
|
||||
CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE && \
|
||||
CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_BEBOP && \
|
||||
CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_QFLIGHT && \
|
||||
CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_MINLURE
|
||||
_add_backend(AP_Compass_HMC5843::detect_i2c(*this, hal.i2c));
|
||||
#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));
|
||||
#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_i2c(*this, hal.i2c));
|
||||
_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::detect_mpu6000(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C && HAL_COMPASS_AK8963_I2C_BUS == 1
|
||||
_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));
|
||||
#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));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
|
||||
_add_backend(AP_Compass_PX4::detect(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250
|
||||
#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));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT
|
||||
_add_backend(AP_Compass_QFLIGHT::detect(*this));
|
||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT
|
||||
_add_backend(AP_Compass_QURT::detect(*this));
|
||||
#else
|
||||
#error Unrecognised HAL_COMPASS_TYPE setting
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user