mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: changes to HMC5843 driver
this makes the HMC5843 use single-shot mode, which makes it more robust to bus resets. It also adds WHOAMI checking and enables automatic temperature compensation. Unfortunately this makes existing calibration values incompatible, so a re-cal is required. The device ID has been changed to reflect the driver change.
This commit is contained in:
parent
f62851f780
commit
f31a017ace
|
@ -50,7 +50,7 @@ public:
|
|||
calibration values invalid then this number must be changed.
|
||||
*/
|
||||
enum DevTypes {
|
||||
DEVTYPE_HMC5883 = 0x01,
|
||||
DEVTYPE_HMC5883 = 0x07,
|
||||
DEVTYPE_LSM303D = 0x02,
|
||||
DEVTYPE_AK8963 = 0x04,
|
||||
DEVTYPE_BMM150 = 0x05,
|
||||
|
|
|
@ -49,6 +49,9 @@ extern const AP_HAL::HAL& hal;
|
|||
#define HMC5843_SAMPLE_AVERAGING_2 (0x01 << 5)
|
||||
#define HMC5843_SAMPLE_AVERAGING_4 (0x02 << 5)
|
||||
#define HMC5843_SAMPLE_AVERAGING_8 (0x03 << 5)
|
||||
|
||||
#define HMC5843_CONF_TEMP_ENABLE (0x80)
|
||||
|
||||
// Valid data output rates for 5883L
|
||||
#define HMC5843_OSR_0_75HZ (0x00 << 2)
|
||||
#define HMC5843_OSR_1_5HZ (0x01 << 2)
|
||||
|
@ -88,6 +91,9 @@ extern const AP_HAL::HAL& hal;
|
|||
|
||||
#define HMC5843_REG_DATA_OUTPUT_X_MSB 0x03
|
||||
|
||||
#define HMC5843_REG_ID_A 0x0A
|
||||
|
||||
|
||||
AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass, AP_HMC5843_BusDriver *bus,
|
||||
bool force_external, enum Rotation rotation)
|
||||
: AP_Compass_Backend(compass)
|
||||
|
@ -155,8 +161,8 @@ bool AP_Compass_HMC5843::init()
|
|||
goto errout;
|
||||
}
|
||||
|
||||
if (!_detect_version()) {
|
||||
hal.console->printf("HMC5843: Could not detect version\n");
|
||||
if (!_check_whoami()) {
|
||||
hal.console->printf("HMC5843: not a HMC device\n");
|
||||
goto errout;
|
||||
}
|
||||
|
||||
|
@ -211,6 +217,10 @@ errout:
|
|||
bool AP_Compass_HMC5843::_timer()
|
||||
{
|
||||
bool result = _read_sample();
|
||||
|
||||
// always ask for a new sample
|
||||
_take_sample();
|
||||
|
||||
if (!result) {
|
||||
return true;
|
||||
}
|
||||
|
@ -227,7 +237,7 @@ bool AP_Compass_HMC5843::_timer()
|
|||
raw_field *= _gain_scale;
|
||||
|
||||
// rotate to the desired orientation
|
||||
if (is_external(_compass_instance) && _is_hmc5883L) {
|
||||
if (is_external(_compass_instance)) {
|
||||
raw_field.rotate(ROTATION_YAW_90);
|
||||
}
|
||||
|
||||
|
@ -296,9 +306,15 @@ void AP_Compass_HMC5843::read()
|
|||
|
||||
bool AP_Compass_HMC5843::_setup_sampling_mode()
|
||||
{
|
||||
if (!_bus->register_write(HMC5843_REG_CONFIG_A, _base_config) ||
|
||||
!_bus->register_write(HMC5843_REG_CONFIG_B, _gain_config) ||
|
||||
!_bus->register_write(HMC5843_REG_MODE, HMC5843_MODE_CONTINUOUS)) {
|
||||
_gain_scale = (1.0f / 1090) * 1000;
|
||||
if (!_bus->register_write(HMC5843_REG_CONFIG_A,
|
||||
HMC5843_CONF_TEMP_ENABLE |
|
||||
HMC5843_OSR_75HZ |
|
||||
HMC5843_SAMPLE_AVERAGING_1) ||
|
||||
!_bus->register_write(HMC5843_REG_CONFIG_B,
|
||||
HMC5883L_GAIN_1_30_GA) ||
|
||||
!_bus->register_write(HMC5843_REG_MODE,
|
||||
HMC5843_MODE_SINGLE)) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
|
@ -316,22 +332,13 @@ bool AP_Compass_HMC5843::_read_sample()
|
|||
} val;
|
||||
int16_t rx, ry, rz;
|
||||
|
||||
if (_retry_time > AP_HAL::millis()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_bus->block_read(HMC5843_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 (_is_hmc5883L) {
|
||||
std::swap(ry, rz);
|
||||
}
|
||||
ry = be16toh(val.rz);
|
||||
rz = be16toh(val.ry);
|
||||
|
||||
if (rx == -4096 || ry == -4096 || rz == -4096) {
|
||||
// no valid data available
|
||||
|
@ -342,32 +349,30 @@ bool AP_Compass_HMC5843::_read_sample()
|
|||
_mag_y = ry;
|
||||
_mag_z = -rz;
|
||||
|
||||
_retry_time = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_Compass_HMC5843::_detect_version()
|
||||
{
|
||||
_base_config = 0x0;
|
||||
|
||||
uint8_t try_config = HMC5843_SAMPLE_AVERAGING_8 | HMC5843_OSR_75HZ | HMC5843_OPMODE_NORMAL;
|
||||
if (!_bus->register_write(HMC5843_REG_CONFIG_A, try_config) ||
|
||||
!_bus->register_read(HMC5843_REG_CONFIG_A, &_base_config)) {
|
||||
return false;
|
||||
/*
|
||||
ask for a new oneshot sample
|
||||
*/
|
||||
void AP_Compass_HMC5843::_take_sample()
|
||||
{
|
||||
_bus->register_write(HMC5843_REG_MODE,
|
||||
HMC5843_MODE_SINGLE);
|
||||
}
|
||||
|
||||
if (_base_config == try_config) {
|
||||
/* a 5883L supports the sample averaging config */
|
||||
_is_hmc5883L = true;
|
||||
_gain_config = HMC5883L_GAIN_1_30_GA;
|
||||
_gain_scale = (1.0f / 1090) * 1000;
|
||||
} else if (_base_config == (HMC5843_OPMODE_NORMAL | HMC5843_OSR_75HZ)) {
|
||||
_is_hmc5883L = false;
|
||||
_gain_config = HMC5843_GAIN_1_00_GA;
|
||||
_gain_scale = (1.0f / 1300) * 1000;
|
||||
} else {
|
||||
/* not behaving like either supported compass type */
|
||||
bool AP_Compass_HMC5843::_check_whoami()
|
||||
{
|
||||
uint8_t id[3];
|
||||
if (!_bus->block_read(HMC5843_REG_ID_A, id, 3)) {
|
||||
// can't talk on bus
|
||||
return false;
|
||||
}
|
||||
if (id[0] != 'H' ||
|
||||
id[1] != '4' ||
|
||||
id[2] != '3') {
|
||||
// not a HMC5x83 device
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -377,27 +382,21 @@ bool AP_Compass_HMC5843::_detect_version()
|
|||
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;
|
||||
|
||||
if (_is_hmc5883L) {
|
||||
calibration_gain = HMC5883L_GAIN_2_50_GA;
|
||||
/*
|
||||
* note that the HMC5883 datasheet gives the x and y expected
|
||||
* values as 766 and the z as 713. Experiments have shown the x
|
||||
* axis is around 766, and the y and z closer to 713.
|
||||
*/
|
||||
expected_x = 766;
|
||||
expected_yz = 713;
|
||||
} else {
|
||||
calibration_gain = HMC5843_GAIN_1_00_GA;
|
||||
expected_x = 715;
|
||||
expected_yz = 715;
|
||||
}
|
||||
|
||||
uint8_t old_config = _base_config & ~(HMC5843_OPMODE_MASK);
|
||||
/*
|
||||
* the expected values are based on observation of real sensors
|
||||
*/
|
||||
float expected[3] = { 1.16*600, 1.08*600, 1.16*600 };
|
||||
|
||||
uint8_t old_config;
|
||||
if (!_bus->register_read(HMC5843_REG_CONFIG_A,
|
||||
&old_config)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
while (success == 0 && numAttempts < 25 && good_count < 5) {
|
||||
numAttempts++;
|
||||
|
@ -428,9 +427,9 @@ bool AP_Compass_HMC5843::_calibrate()
|
|||
|
||||
// hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z);
|
||||
|
||||
cal[0] = fabsf(expected_x / (float)_mag_x);
|
||||
cal[1] = fabsf(expected_yz / (float)_mag_y);
|
||||
cal[2] = fabsf(expected_yz / (float)_mag_z);
|
||||
cal[0] = fabsf(expected[0] / _mag_x);
|
||||
cal[1] = fabsf(expected[1] / _mag_y);
|
||||
cal[2] = fabsf(expected[2] / _mag_z);
|
||||
|
||||
// hal.console->printf("cal=%.2f %.2f %.2f\n", cal[0], cal[1], cal[2]);
|
||||
|
||||
|
@ -459,8 +458,8 @@ bool AP_Compass_HMC5843::_calibrate()
|
|||
|
||||
#if 0
|
||||
/* useful for debugging */
|
||||
hal.console->printf("MagX: %d MagY: %d MagZ: %d\n", (int)_mag_x, (int)_mag_y, (int)_mag_z);
|
||||
hal.console->printf("CalX: %.2f CalY: %.2f CalZ: %.2f\n", cal[0], cal[1], cal[2]);
|
||||
printf("MagX: %d MagY: %d MagZ: %d\n", (int)_mag_x, (int)_mag_y, (int)_mag_z);
|
||||
printf("CalX: %.2f CalY: %.2f CalZ: %.2f\n", cal[0], cal[1], cal[2]);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -476,6 +475,11 @@ bool AP_Compass_HMC5843::_calibrate()
|
|||
_scaling[2] = 1.0;
|
||||
}
|
||||
|
||||
#if 0
|
||||
printf("scaling: %.2f %.2f %.2f\n",
|
||||
_scaling[0], _scaling[1], _scaling[2]);
|
||||
#endif
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
|
|
|
@ -35,7 +35,7 @@ private:
|
|||
AP_Compass_HMC5843(Compass &compass, AP_HMC5843_BusDriver *bus,
|
||||
bool force_external, enum Rotation rotation);
|
||||
|
||||
bool _detect_version();
|
||||
bool _check_whoami();
|
||||
bool _calibrate();
|
||||
bool _setup_sampling_mode();
|
||||
|
||||
|
@ -44,14 +44,14 @@ private:
|
|||
/* Read a single sample */
|
||||
bool _read_sample();
|
||||
|
||||
// ask for a new sample
|
||||
void _take_sample();
|
||||
|
||||
AP_HMC5843_BusDriver *_bus;
|
||||
|
||||
float _scaling[3];
|
||||
float _gain_scale;
|
||||
|
||||
// when unhealthy the millis() value to retry at
|
||||
uint32_t _retry_time;
|
||||
|
||||
int16_t _mag_x;
|
||||
int16_t _mag_y;
|
||||
int16_t _mag_z;
|
||||
|
@ -60,13 +60,10 @@ private:
|
|||
int16_t _mag_z_accum;
|
||||
uint8_t _accum_count;
|
||||
|
||||
uint8_t _base_config;
|
||||
uint8_t _compass_instance;
|
||||
uint8_t _gain_config;
|
||||
|
||||
enum Rotation _rotation;
|
||||
|
||||
bool _is_hmc5883L:1;
|
||||
bool _initialised:1;
|
||||
bool _force_external:1;
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue