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

@ -204,6 +204,8 @@ bool AP_Compass_HMC5843::init()
// read from sensor at 75Hz // read from sensor at 75Hz
_bus->register_periodic_callback(13333, _bus->register_periodic_callback(13333,
FUNCTOR_BIND_MEMBER(&AP_Compass_HMC5843::_timer, bool)); FUNCTOR_BIND_MEMBER(&AP_Compass_HMC5843::_timer, bool));
hal.console->printf("HMC5843 found on bus 0x%x\n", _bus->get_bus_id());
return true; return true;
@ -395,18 +397,15 @@ bool AP_Compass_HMC5843::_calibrate()
*/ */
float expected[3] = { 1.16*600, 1.08*600, 1.16*600 }; float expected[3] = { 1.16*600, 1.08*600, 1.16*600 };
uint8_t old_config; uint8_t base_config = HMC5843_OSR_15HZ;
if (!_bus->register_read(HMC5843_REG_CONFIG_A, uint8_t num_samples = 0;
&old_config)) {
return false;
}
while (success == 0 && numAttempts < 25 && good_count < 5) { while (success == 0 && numAttempts < 25 && good_count < 5) {
numAttempts++; numAttempts++;
// force positiveBias (compass should return 715 for all channels) // force positiveBias (compass should return 715 for all channels)
if (!_bus->register_write(HMC5843_REG_CONFIG_A, 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 // compass not responding on the bus
continue; continue;
} }
@ -426,6 +425,8 @@ bool AP_Compass_HMC5843::_calibrate()
continue; continue;
} }
num_samples++;
float cal[3]; float cal[3];
// hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z); // 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 #if 0
/* useful for debugging */ /* useful for debugging */
printf("MagX: %d MagY: %d MagZ: %d\n", (int)_mag_x, (int)_mag_y, (int)_mag_z); hal.console->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("CalX: %.2f CalY: %.2f CalZ: %.2f\n", cal[0], cal[1], cal[2]);
#endif #endif
} }
_bus->register_write(HMC5843_REG_CONFIG_A, base_config);
if (good_count >= 5) { if (good_count >= 5) {
_scaling[0] = _scaling[0] / good_count; _scaling[0] = _scaling[0] / good_count;
_scaling[1] = _scaling[1] / good_count; _scaling[1] = _scaling[1] / good_count;
@ -476,6 +479,11 @@ bool AP_Compass_HMC5843::_calibrate()
_scaling[0] = 1.0; _scaling[0] = 1.0;
_scaling[1] = 1.0; _scaling[1] = 1.0;
_scaling[2] = 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 #if 0