From 79ffddc6db69a52b48fc566c7dffe08ea8585764 Mon Sep 17 00:00:00 2001 From: Staroselskii Georgii Date: Thu, 23 Jul 2015 13:52:02 +0300 Subject: [PATCH] AP_Compass: add a macro for checking calibration values in HMC --- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index e17b373e99..88c4e7cf4e 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -326,17 +326,21 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain, // still be changing its state from the application of the // strap excitation. After that we accept values in a // reasonable range +#define IS_CALIBRATION_VALUE_VALID(val) (val > 0.7f && val < 1.35f) + if (numAttempts > 2 && - cal[0] > 0.7f && cal[0] < 1.35f && - cal[1] > 0.7f && cal[1] < 1.35f && - cal[2] > 0.7f && cal[2] < 1.35f) { - // hal.console->printf_P(PSTR("cal=%.2f %.2f %.2f good\n"), cal[0], cal[1], cal[2]); + 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++; calibration[0] += cal[0]; calibration[1] += cal[1]; calibration[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);