diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 49cfe07324..c16f7315cd 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -47,9 +47,9 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bo AP_Notify::events.initiated_compass_cal = 1; } if (i == get_primary()) { - _calibrator[i].set_tolerance(8); + _calibrator[i].set_tolerance(_calibration_threshold); } else { - _calibrator[i].set_tolerance(16); + _calibrator[i].set_tolerance(_calibration_threshold*2); } _calibrator[i].start(retry, autosave, delay); _compass_cal_autoreboot = autoreboot; diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 2a0016dfc7..74de9b205c 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -365,6 +365,14 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { AP_GROUPINFO("ODI3", 29, Compass, _state[2].offdiagonals, 0), #endif + // @Param: CAL_FIT + // @DisplayName: Compass calibration fitness + // @Description: This controls the fitness level required for a successful compass calibration. A lower value makes for a stricter fit (less likely to pass). This is the value used for the primary magnetometer. Other magnetometers get double the value. + // @Range: 4 20 + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("CAL_FIT", 30, Compass, _calibration_threshold, 8.0f), + AP_GROUPEND }; diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 42b32f19f9..051dea08fa 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -406,6 +406,8 @@ private: // if we want HIL only bool _hil_mode:1; + + AP_Float _calibration_threshold; }; #include "AP_Compass_Backend.h"