Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: improve init and calibration of hmc5843

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-09 14:57:07 +11:00
parent d248b33104
commit 4602b4d679
1 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -205,6 +205,8 @@ bool AP_Compass_HMC5843::init()
_bus->register_periodic_callback(13333,
FUNCTOR_BIND_MEMBER(&AP_Compass_HMC5843::_timer, bool));
hal.console->printf("HMC5843 found on bus 0x%x\n", _bus->get_bus_id());
return true;
errout:
@ -395,18 +397,15 @@ bool AP_Compass_HMC5843::_calibrate()
*/
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 base_config = HMC5843_OSR_15HZ;
uint8_t num_samples = 0;
while (success == 0 && numAttempts < 25 && good_count < 5) {
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
if (!_bus->register_write(HMC5843_REG_CONFIG_A,
old_config | HMC5843_OPMODE_POSITIVE_BIAS)) {
base_config | HMC5843_OPMODE_POSITIVE_BIAS)) {
// compass not responding on the bus
continue;
}
@ -426,6 +425,8 @@ bool AP_Compass_HMC5843::_calibrate()
continue;
}
num_samples++;
float cal[3];
// hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z);
@ -461,11 +462,13 @@ bool AP_Compass_HMC5843::_calibrate()
#if 0
/* useful for debugging */
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]);
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]);
#endif
}
_bus->register_write(HMC5843_REG_CONFIG_A, base_config);
if (good_count >= 5) {
_scaling[0] = _scaling[0] / good_count;
_scaling[1] = _scaling[1] / good_count;
@ -476,6 +479,11 @@ bool AP_Compass_HMC5843::_calibrate()
_scaling[0] = 1.0;
_scaling[1] = 1.0;
_scaling[2] = 1.0;
if (num_samples > 5) {
// a sensor can be broken for calibration but still
// otherwise workable, accept it if we are reading samples
success = true;
}
}
#if 0