aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: HMC5843: improve readability

Browse Source 
- Capitalize and rename constants
 - Make clear what gain is applied in calibration and what is the
   "normal" gain
 - Make the separation between HMC5883L and HMC5843 explicit when it
   makes sense to improve readability
 - Remove spurious delay in calibrate function
This commit is contained in:
Lucas De Marchi 2016-03-07 21:10:08 -03:00
parent 0291ad869b
commit 41c1209169
2 changed files with 87 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -39,33 +39,54 @@
extern const AP_HAL::HAL& hal;
#define HMC5843_I2C_ADDR 0x1E
#define ConfigRegA 0x00
#define ConfigRegB 0x01
#define magGain 0x20
#define PositiveBiasConfig 0x11
#define NegativeBiasConfig 0x12
#define NormalOperation 0x10
#define ModeRegister 0x02
#define ContinuousConversion 0x00
#define SingleConversion 0x01
/*
* Defaul address: 0x1E
*/
// ConfigRegA valid sample averaging for 5883L
#define SampleAveraging_1 0x00
#define SampleAveraging_2 0x01
#define SampleAveraging_4 0x02
#define SampleAveraging_8 0x03
#define HMC5843_REG_CONFIG_A 0x00
// Valid sample averaging for 5883L
#define HMC5843_SAMPLE_AVERAGING_1 (0x00 << 5)
#define HMC5843_SAMPLE_AVERAGING_2 (0x01 << 5)
#define HMC5843_SAMPLE_AVERAGING_4 (0x02 << 5)
#define HMC5843_SAMPLE_AVERAGING_8 (0x03 << 5)
// Valid data output rates for 5883L
#define HMC5843_OSR_0_75HZ (0x00 << 2)
#define HMC5843_OSR_1_5HZ (0x01 << 2)
#define HMC5843_OSR_3HZ (0x02 << 2)
#define HMC5843_OSR_7_5HZ (0x03 << 2)
#define HMC5843_OSR_15HZ (0x04 << 2)
#define HMC5843_OSR_30HZ (0x05 << 2)
#define HMC5843_OSR_75HZ (0x06 << 2)
// Sensor operation modes
#define HMC5843_OPMODE_NORMAL 0x00
#define HMC5843_OPMODE_POSITIVE_BIAS 0x01
#define HMC5843_OPMODE_NEGATIVE_BIAS 0x02
#define HMC5843_OPMODE_MASK 0x03
// ConfigRegA valid data output rates for 5883L
#define DataOutputRate_0_75HZ 0x00
#define DataOutputRate_1_5HZ 0x01
#define DataOutputRate_3HZ 0x02
#define DataOutputRate_7_5HZ 0x03
#define DataOutputRate_15HZ 0x04
#define DataOutputRate_30HZ 0x05
#define DataOutputRate_75HZ 0x06
#define HMC5843_REG_CONFIG_B 0x01
#define HMC5883L_GAIN_0_88_GA (0x00 << 5)
#define HMC5883L_GAIN_1_30_GA (0x01 << 5)
#define HMC5883L_GAIN_1_90_GA (0x02 << 5)
#define HMC5883L_GAIN_2_50_GA (0x03 << 5)
#define HMC5883L_GAIN_4_00_GA (0x04 << 5)
#define HMC5883L_GAIN_4_70_GA (0x05 << 5)
#define HMC5883L_GAIN_5_60_GA (0x06 << 5)
#define HMC5883L_GAIN_8_10_GA (0x07 << 5)
#define REG_DATA_OUTPUT_X_MSB 0x03
#define HMC5843_GAIN_0_70_GA (0x00 << 5)
#define HMC5843_GAIN_1_00_GA (0x01 << 5)
#define HMC5843_GAIN_1_50_GA (0x02 << 5)
#define HMC5843_GAIN_2_00_GA (0x03 << 5)
#define HMC5843_GAIN_3_20_GA (0x04 << 5)
#define HMC5843_GAIN_3_80_GA (0x05 << 5)
#define HMC5843_GAIN_4_50_GA (0x06 << 5)
#define HMC5843_GAIN_6_50_GA (0x07 << 5)
#define HMC5843_REG_MODE 0x02
#define HMC5843_MODE_CONTINUOUS 0x00
#define HMC5843_MODE_SINGLE 0x01
#define HMC5843_REG_DATA_OUTPUT_X_MSB 0x03
AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass, AP_HMC5843_BusDriver *bus)
: AP_Compass_Backend(compass)
@ -140,7 +161,7 @@ bool AP_Compass_HMC5843::init()
goto errout;
}
if (!_re_initialize()) {
if (!_setup_sampling_mode()) {
goto errout;
}
@ -216,7 +237,7 @@ void AP_Compass_HMC5843::accumulate()
// 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;
raw_field *= _gain_scale;
// rotate raw_field from sensor frame to body frame
rotate_field(raw_field, _compass_instance);
@ -282,12 +303,11 @@ void AP_Compass_HMC5843::read()
publish_filtered_field(field, _compass_instance);
}
bool AP_Compass_HMC5843::_re_initialize()
bool AP_Compass_HMC5843::_setup_sampling_mode()
{
if (!_bus->register_write(ConfigRegA, _base_config) ||
!_bus->register_write(ConfigRegB, magGain) ||
!_bus->register_write(ModeRegister, ContinuousConversion)) {
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)) {
return false;
}
return true;
@ -309,7 +329,7 @@ bool AP_Compass_HMC5843::_read_sample()
return false;
}
if (!_bus->block_read(REG_DATA_OUTPUT_X_MSB, (uint8_t *) &val, sizeof(val))){
if (!_bus->block_read(HMC5843_REG_DATA_OUTPUT_X_MSB, (uint8_t *) &val, sizeof(val))){
_retry_time = AP_HAL::millis() + 1000;
return false;
}
@ -340,16 +360,21 @@ 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)) {
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;
}
if (_base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
if (_base_config == try_config) {
/* a 5883L supports the sample averaging config */
_product_id = AP_COMPASS_TYPE_HMC5883L;
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
_gain_config = HMC5883L_GAIN_1_30_GA;
_gain_scale = (1.0f / 1090) * 1000;
} else if (_base_config == (HMC5843_OPMODE_NORMAL | HMC5843_OSR_75HZ)) {
_product_id = AP_COMPASS_TYPE_HMC5843;
_gain_config = HMC5843_GAIN_1_00_GA;
_gain_scale = (1.0f / 1300) * 1000;
} else {
/* not behaving like either supported compass type */
return false;
@ -360,39 +385,46 @@ bool AP_Compass_HMC5843::_detect_version()
bool AP_Compass_HMC5843::_calibrate()
{
uint8_t calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
uint8_t calibration_gain;
uint16_t expected_x;
uint16_t expected_yz;
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;
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.
* 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;
_gain_multiple = (1.0f / 1090) * 1000;
} else {
calibration_gain = HMC5843_GAIN_1_00_GA;
expected_x = 715;
expected_yz = 715;
}
uint8_t old_config = _base_config & ~(HMC5843_OPMODE_MASK);
while (success == 0 && numAttempts < 25 && good_count < 5) {
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
if (!_bus->register_write(ConfigRegA, PositiveBiasConfig))
continue; // compass not responding on the bus
if (!_bus->register_write(HMC5843_REG_CONFIG_A,
old_config | HMC5843_OPMODE_POSITIVE_BIAS)) {
// compass not responding on the bus
continue;
}
hal.scheduler->delay(50);
// set gains
if (!_bus->register_write(ConfigRegB, calibration_gain) ||
!_bus->register_write(ModeRegister, SingleConversion))
if (!_bus->register_write(HMC5843_REG_CONFIG_B, calibration_gain) ||
!_bus->register_write(HMC5843_REG_MODE, HMC5843_MODE_SINGLE)) {
continue;
}
// read values from the compass
hal.scheduler->delay(50);
@ -401,8 +433,6 @@ bool AP_Compass_HMC5843::_calibrate()
continue;
}
hal.scheduler->delay(10);
float cal[3];
// hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z);
@ -417,7 +447,6 @@ bool AP_Compass_HMC5843::_calibrate()
// still be changing its state from the application of the
// strap excitation. After that we accept values in a
// reasonable range
if (numAttempts <= 2) {
continue;
}
@ -516,7 +545,7 @@ bool AP_HMC5843_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint3
* 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);
assert(reg == HMC5843_REG_DATA_OUTPUT_X_MSB);
int n = _slave->read(buf);
return n == static_cast<int>(size);
@ -553,7 +582,7 @@ bool AP_HMC5843_BusDriver_Auxiliary::configure()
bool AP_HMC5843_BusDriver_Auxiliary::start_measurements()
{
if (_bus->register_periodic_read(_slave, REG_DATA_OUTPUT_X_MSB, 6) < 0) {
if (_bus->register_periodic_read(_slave, HMC5843_REG_DATA_OUTPUT_X_MSB, 6) < 0) {
return false;
}

5
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -33,7 +33,7 @@ private:
bool _detect_version();
bool _calibrate();
bool _re_initialize();
bool _setup_sampling_mode();
/* Read a single sample */
bool _read_sample();
@ -41,7 +41,7 @@ private:
AP_HMC5843_BusDriver *_bus;
float _scaling[3];
float _gain_multiple;
float _gain_scale;
uint32_t _last_accum_time;
// when unhealthy the millis() value to retry at
@ -57,6 +57,7 @@ private:
uint8_t _base_config;
uint8_t _compass_instance;
uint8_t _gain_config;
uint8_t _product_id;
bool _initialised;