Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
Andrew Tridgell 2016-11-07 19:04:40 +11:00
parent f62851f780
commit f31a017ace
3 changed files with 69 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -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,

124
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -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()
/*
ask for a new oneshot sample
*/
void AP_Compass_HMC5843::_take_sample()
{
_base_config = 0x0;
_bus->register_write(HMC5843_REG_MODE,
HMC5843_MODE_SINGLE);
}
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;
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 (_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 */
if (id[0] != 'H' ||
id[1] != '4' ||
id[2] != '3') {
// not a HMC5x83 device
return false;
}
@ -377,28 +382,22 @@ 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;
calibration_gain = HMC5883L_GAIN_2_50_GA;
/*
* 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;
}
uint8_t old_config = _base_config & ~(HMC5843_OPMODE_MASK);
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;
}

11
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -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;
};