From 36ea946cf53d8a1267049f7454b1ef334730ad01 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 17 Sep 2016 19:02:02 +1000 Subject: [PATCH] AP_Compass: use board defined compass ofs max in calibrator --- libraries/AP_Compass/CompassCalibrator.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index c0a427cf96..de1215eff9 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -68,6 +68,14 @@ extern const AP_HAL::HAL& hal; ///////////////////// PUBLIC INTERFACE ///////////////////// //////////////////////////////////////////////////////////// +#ifdef AP_ARMING_COMPASS_OFFSETS_MAX +#define COMPASS_CALIBRATOR_OFS_MAX AP_ARMING_COMPASS_OFFSETS_MAX +#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL +#define COMPASS_CALIBRATOR_OFS_MAX 2000 +#else +#define COMPASS_CALIBRATOR_OFS_MAX 1000 +#endif + CompassCalibrator::CompassCalibrator(): _tolerance(COMPASS_CAL_DEFAULT_TOLERANCE), _sample_buffer(NULL) @@ -341,9 +349,9 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { bool CompassCalibrator::fit_acceptable() { if( !isnan(_fitness) && _params.radius > 150 && _params.radius < 950 && //Earth's magnetic field strength range: 250-850mG - fabsf(_params.offset.x) < 1000 && - fabsf(_params.offset.y) < 1000 && - fabsf(_params.offset.z) < 1000 && + fabsf(_params.offset.x) < COMPASS_CALIBRATOR_OFS_MAX && + fabsf(_params.offset.y) < COMPASS_CALIBRATOR_OFS_MAX && + fabsf(_params.offset.z) < COMPASS_CALIBRATOR_OFS_MAX && _params.diag.x > 0.2f && _params.diag.x < 5.0f && _params.diag.y > 0.2f && _params.diag.y < 5.0f && _params.diag.z > 0.2f && _params.diag.z < 5.0f &&