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)
{
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
if (!write_register(ConfigRegA, PositiveBiasConfig))
continue; // compass not responding on the bus
hal.scheduler->delay(50);
// set gains
if (!write_register(ConfigRegB, calibration_gain) ||
!write_register(ModeRegister, SingleConversion))
continue;
// read values from the compass
hal.scheduler->delay(50);
if (!read_raw())
continue; // we didn't read valid values
hal.scheduler->delay(10);
float cal[3];
// hal.console->printf_P(PSTR("mag %d %d %d\n"), _mag_x, _mag_y, _mag_z);
cal[0] = fabsf(expected_x / (float)_mag_x);
cal[1] = fabsf(expected_yz / (float)_mag_y);
cal[2] = fabsf(expected_yz / (float)_mag_z);
// hal.console->printf_P(PSTR("cal=%.2f %.2f %.2f\n"), cal[0], cal[1], cal[2]);
// we throw away the first two samples as the compass may
// 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;
}
#define IS_CALIBRATION_VALUE_VALID(val) (val > 0.7f && val < 1.35f)
if (IS_CALIBRATION_VALUE_VALID(cal[0]) &&
IS_CALIBRATION_VALUE_VALID(cal[1]) &&
IS_CALIBRATION_VALUE_VALID(cal[2])) {
// hal.console->printf_P(PSTR("car=%.2f %.2f %.2f good\n"), cal[0], cal[1], cal[2]);
good_count++;
_scaling[0] += cal[0];
_scaling[1] += cal[1];
_scaling[2] += cal[2];
}
#undef IS_CALIBRATION_VALUE_VALID
#if 0
/* useful for debugging */
hal.console->printf_P(PSTR("MagX: %d MagY: %d MagZ: %d\n"), (int)_mag_x, (int)_mag_y, (int)_mag_z);
hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"), cal[0], cal[1], cal[2]);
#endif
}
if (good_count >= 5) {
/*
The use of gain_multiple below is incorrect, as the gain
difference between 2.5Ga mode and 1Ga mode is already taken
into account by the expected_x and expected_yz values. We
are not going to fix it however as it would mean all
APM1/APM2 users redoing their compass calibration. The
impact is that the values we report on APM1/APM2 are lower
than they should be (by a multiple of about 0.6). This
doesn't have any impact other than the learned compass
offsets
*/
_scaling[0] = _scaling[0] * gain_multiple / good_count;
_scaling[1] = _scaling[1] * gain_multiple / good_count;
_scaling[2] = _scaling[2] * gain_multiple / good_count;
success = true;
} else {
/* best guess */
_scaling[0] = 1.0;
_scaling[1] = 1.0;
_scaling[2] = 1.0;
}
return success;
}
bool AP_Compass_HMC5843::_calibrate()
{
uint8_t calibration_gain;
uint16_t expected_x;
uint16_t expected_yz;
int numAttempts = 0, good_count = 0;
bool success = false;
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
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.
*/
expected_x = 766;
expected_yz = 713;
} 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(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(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);
if (!_read_sample()) {
// we didn't read valid values
continue;
}
float cal[3];
// hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z);
cal[0] = fabsf(expected_x / (float)_mag_x);
cal[1] = fabsf(expected_yz / (float)_mag_y);
cal[2] = fabsf(expected_yz / (float)_mag_z);
// hal.console->printf("cal=%.2f %.2f %.2f\n", cal[0], cal[1], cal[2]);
// we throw away the first two samples as the compass may
// 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;
}
#define IS_CALIBRATION_VALUE_VALID(val) (val > 0.7f && val < 1.35f)
if (IS_CALIBRATION_VALUE_VALID(cal[0]) &&
IS_CALIBRATION_VALUE_VALID(cal[1]) &&
IS_CALIBRATION_VALUE_VALID(cal[2])) {
// hal.console->printf("car=%.2f %.2f %.2f good\n", cal[0], cal[1], cal[2]);
good_count++;
_scaling[0] += cal[0];
_scaling[1] += cal[1];
_scaling[2] += cal[2];
}
#undef IS_CALIBRATION_VALUE_VALID
#if 0
/* useful for debugging */
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
}
if (good_count >= 5) {
_scaling[0] = _scaling[0] / good_count;
_scaling[1] = _scaling[1] / good_count;
_scaling[2] = _scaling[2] / good_count;
success = true;
} else {
/* best guess */
_scaling[0] = 1.0;
_scaling[1] = 1.0;
_scaling[2] = 1.0;
}
return success;
}
bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
uint16_t expected_x,
uint16_t expected_yz)
{
int numAttempts = 0, good_count = 0;
bool success = false;
while (success == 0 && numAttempts < 25 && good_count < 5)
{
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
if (!write_register(ConfigRegA, PositiveBiasConfig))
continue; // compass not responding on the bus
hal.scheduler->delay(50);
// set gains
if (!write_register(ConfigRegB, calibration_gain) ||
!write_register(ModeRegister, SingleConversion))
continue;
// read values from the compass
hal.scheduler->delay(50);
if (!read_raw())
continue; // we didn't read valid values
hal.scheduler->delay(10);
float cal[3];
// hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z);
cal[0] = fabsf(expected_x / (float)_mag_x);
cal[1] = fabsf(expected_yz / (float)_mag_y);
cal[2] = fabsf(expected_yz / (float)_mag_z);
// hal.console->printf("cal=%.2f %.2f %.2f\n", cal[0], cal[1], cal[2]);
// we throw away the first two samples as the compass may
// 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;
}
#define IS_CALIBRATION_VALUE_VALID(val) (val > 0.7f && val < 1.35f)
if (IS_CALIBRATION_VALUE_VALID(cal[0]) &&
IS_CALIBRATION_VALUE_VALID(cal[1]) &&
IS_CALIBRATION_VALUE_VALID(cal[2])) {
// hal.console->printf("car=%.2f %.2f %.2f good\n", cal[0], cal[1], cal[2]);
good_count++;
_scaling[0] += cal[0];
_scaling[1] += cal[1];
_scaling[2] += cal[2];
}
#undef IS_CALIBRATION_VALUE_VALID
#if 0
/* useful for debugging */
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
}
if (good_count >= 5) {
_scaling[0] = _scaling[0] / good_count;
_scaling[1] = _scaling[1] / good_count;
_scaling[2] = _scaling[2] / good_count;
success = true;
} else {
/* best guess */
_scaling[0] = 1.0;
_scaling[1] = 1.0;
_scaling[2] = 1.0;
}
return success;
}
