AP_Compass: added COMPASS_OFFS_MAX

this allows setup of airframes with hatch magnets
Yukong · Apr 3, 2017 · ee2afd3 · ee2afd3
1 parent 98b7dac
commit ee2afd3
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 14 deletions.
diff --git 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
@@ -281,6 +281,11 @@ friend class AP_Compass_Backend;
     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 @@ friend class AP_Compass_Backend;
         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
@@ -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
@@ -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
@@ -22,7 +22,7 @@ class CompassCalibrator {
 
     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 @@ class CompassCalibrator {
     bool _retry;
     float _tolerance;
     uint8_t _attempt;
+    uint16_t _offset_max;
 
     completion_mask_t _completion_mask;