diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index ca8f742b5e..3733bde10a 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -28,6 +28,10 @@ extern AP_HAL::HAL& hal; #define COMPASS_LEARN_DEFAULT Compass::LEARN_INTERNAL #endif +#ifndef AP_COMPASS_OFFSETS_MAX_DEFAULT +#define AP_COMPASS_OFFSETS_MAX_DEFAULT 600 +#endif + const AP_Param::GroupInfo Compass::var_info[] = { // index 0 was used for the old orientation matrix @@ -404,6 +408,14 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @User: Advanced AP_GROUPINFO("CAL_FIT", 30, Compass, _calibration_threshold, AP_COMPASS_CALIBRATION_FITNESS_DEFAULT), + // @Param: OFFS_MAX + // @DisplayName: Compass maximum offset + // @Description: This sets the maximum allowed compass offset in calibration and arming checks + // @Range: 500 3000 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("OFFS_MAX", 31, Compass, _offset_max, AP_COMPASS_OFFSETS_MAX_DEFAULT), + AP_GROUPEND }; diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 7e796b3a04..bcd5491900 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -281,6 +281,11 @@ public: enum LearnType get_learn_type(void) const { return (enum LearnType)_learn.get(); } + + // return maximum allowed compass offsets + uint16_t get_offsets_max(void) const { + return (uint16_t)_offset_max.get(); + } private: /// Register a new compas driver, allocating an instance number @@ -382,6 +387,8 @@ private: enum Rotation rotation; } _state[COMPASS_MAX_INSTANCES]; + AP_Int16 _offset_max; + CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES]; // if we want HIL only diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index d696faa3e4..5f39bedd2a 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -62,7 +62,7 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay) _calibrator[i].set_tolerance(_calibration_threshold*2); } _cal_saved[i] = false; - _calibrator[i].start(retry, delay); + _calibrator[i].start(retry, delay, get_offsets_max()); // disable compass learning both for calibration and after completion _learn.set_and_save(0); diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index b07f4df2ec..8e711edafc 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -67,14 +67,6 @@ 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(nullptr) @@ -86,10 +78,11 @@ void CompassCalibrator::clear() { set_status(COMPASS_CAL_NOT_STARTED); } -void CompassCalibrator::start(bool retry, float delay) { +void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) { if(running()) { return; } + _offset_max = offset_max; _attempt = 1; _retry = retry; _delay_start_sec = delay; @@ -347,9 +340,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) < COMPASS_CALIBRATOR_OFS_MAX && - fabsf(_params.offset.y) < COMPASS_CALIBRATOR_OFS_MAX && - fabsf(_params.offset.z) < COMPASS_CALIBRATOR_OFS_MAX && + fabsf(_params.offset.x) < _offset_max && + fabsf(_params.offset.y) < _offset_max && + fabsf(_params.offset.z) < _offset_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 && diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index ed814afbea..61d7ad3634 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -22,7 +22,7 @@ public: CompassCalibrator(); - void start(bool retry=false, float delay=0.0f); + void start(bool retry, float delay, uint16_t offset_max); void clear(); void update(bool &failure); @@ -82,6 +82,7 @@ private: bool _retry; float _tolerance; uint8_t _attempt; + uint16_t _offset_max; completion_mask_t _completion_mask;