diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index f683700525..10f619df01 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -34,9 +34,9 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay) AP_Notify::events.initiated_compass_cal = 1; } if (i == get_primary()) { - _calibrator[i].set_tolerance(5); + _calibrator[i].set_tolerance(8); } else { - _calibrator[i].set_tolerance(10); + _calibrator[i].set_tolerance(16); } _calibrator[i].start(retry, autosave, delay); return true; diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 14ad5a571d..ebd82b81db 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -293,11 +293,11 @@ bool CompassCalibrator::accept_sample(const Vector3f& sample) return false; } - float max_distance = fabsf(5.38709f * _params.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 3.0f; + float min_distance = fabsf(5.38709f * _params.radius / sqrtf((float)COMPASS_CAL_NUM_SAMPLES)) / 3.0f; for (uint16_t i = 0; i<_samples_collected; i++){ float distance = (sample - _sample_buffer[i].get()).length(); - if(distance < max_distance) { + if(distance < min_distance) { return false; } }