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

AP_Compass: encapsulated version detection in HMC driver

Browse Source
This commit is contained in:
Staroselskii Georgii 2015-07-22 18:09:25 +03:00 committed by Andrew Tridgell
parent 868d0bf3c3
commit 0fbc295d97
2 changed files with 34 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -197,6 +197,27 @@ bool AP_Compass_HMC5843::re_initialise()
}
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()
@ -216,17 +237,11 @@ AP_Compass_HMC5843::init()
hal.scheduler->panic(PSTR("Failed to get HMC5843 semaphore"));
}
// determine if we are using 5843 or 5883L
_base_config = 0;
if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
!read_register(ConfigRegA, &_base_config)) {
_i2c_sem->give();
hal.scheduler->resume_timer_procs();
return false;
if (!_detect_version()) {
goto errout;
}
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
_product_id = AP_COMPASS_TYPE_HMC5883L;
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
calibration_gain = 0x60;
/*
note that the HMC5883 datasheet gives the x and y expected
@ -236,13 +251,6 @@ AP_Compass_HMC5843::init()
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0f / 1090; // adjustment for runtime vs calibration gain
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
_product_id = AP_COMPASS_TYPE_HMC5843;
} else {
// not behaving like either supported compass type
_i2c_sem->give();
hal.scheduler->resume_timer_procs();
return false;
}
calibration[0] = 0;
@ -351,6 +359,12 @@ AP_Compass_HMC5843::init()
}
return success;
errout:
_i2c_sem->give();
hal.scheduler->resume_timer_procs();
return false;
}
// Read Sensor data

3
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -19,6 +19,9 @@ private:
bool re_initialise(void);
bool read_register(uint8_t address, uint8_t *value);
bool write_register(uint8_t address, uint8_t value);
bool _detect_version();
uint32_t _retry_time; // when unhealthy the millis() value to retry at
AP_HAL::Semaphore* _i2c_sem;