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

AP_Compass: encapsulated calibration in HMC

Browse Source
This commit is contained in:
Staroselskii Georgii 2015-07-22 18:31:50 +03:00 committed by Andrew Tridgell
parent 0fbc295d97
commit a9e8c1c464
2 changed files with 41 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -222,8 +222,6 @@ bool AP_Compass_HMC5843::_detect_version()
bool
AP_Compass_HMC5843::init()
{
int numAttempts = 0, good_count = 0;
bool success = false;
uint8_t calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
@ -253,9 +251,45 @@ AP_Compass_HMC5843::init()
gain_multiple = 660.0f / 1090; // adjustment for runtime vs calibration gain
}
calibration[0] = 0;
calibration[1] = 0;
calibration[2] = 0;
if (!_calibrate(calibration_gain, expected_x, expected_yz, gain_multiple)) {
goto errout;
}
// leave test mode
if (!re_initialise()) {
goto errout;
}
read();
#if 0
hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"),
calibration[0], calibration[1], calibration[2]);
#endif
_compass_instance = register_compass();
set_dev_id(_compass_instance, _product_id);
_initialised = true;
_i2c_sem->give();
hal.scheduler->resume_timer_procs();
return true;
errout:
_i2c_sem->give();
hal.scheduler->resume_timer_procs();
return false;
}
bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
uint16_t expected_x,
uint16_t expected_yz,
float gain_multiple)
{
int numAttempts = 0, good_count = 0;
bool success = false;
while ( success == 0 && numAttempts < 25 && good_count < 5)
{
@ -333,38 +367,7 @@ AP_Compass_HMC5843::init()
calibration[2] = 1.0;
}
// leave test mode
if (!re_initialise()) {
_i2c_sem->give();
hal.scheduler->resume_timer_procs();
return false;
}
_i2c_sem->give();
hal.scheduler->resume_timer_procs();
_initialised = true;
// perform an initial read
read();
#if 0
hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"),
calibration[0], calibration[1], calibration[2]);
#endif
if (success) {
// register the compass instance in the frontend
_compass_instance = register_compass();
set_dev_id(_compass_instance, _product_id);
}
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

@ -12,7 +12,7 @@
class AP_Compass_HMC5843 : public AP_Compass_Backend
{
private:
float calibration[3];
float calibration[3] = {0};
bool _initialised;
bool read_raw(void);
uint8_t _base_config;
@ -20,6 +20,7 @@ private:
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, float gain_multiple);
bool _detect_version();
uint32_t _retry_time; // when unhealthy the millis() value to retry at