AP_Compass: added estimation of compass scale factor

this adds new COMPASS_SCALE, COMPASS_SCALE2 and COMPASS_SCALE3
parameters, which give the sensor scaling factor. It is used to
compensate for an incorrect scaling in a compass.

The 3D compass calibration process will set the correct value
automatically, otherwise users can set the value to a known value for
an existing compass

libraries/AP_Compass/AP_Compass.cpp

@@ -509,6 +509,31 @@ const AP_Param::GroupInfo Compass::var_info[] = {
     // @Values: 0:Disabled,1:Enabled
     AP_GROUPINFO("ENABLE", 39, Compass, _enabled, 1),
 
+    // @Param: SCALE
+    // @DisplayName: Compass1 scale factor
+    // @Description: Scaling factor for first compass to compensate for sensor scaling errors. If this is 0 then no scaling is done
+    // @User: Standard
+    // @Range: 0 1.3
+    AP_GROUPINFO("SCALE", 40, Compass, _state[0].scale_factor, 0),
+
+#if HAL_COMPASS_MAX_SENSORS > 1
+    // @Param: SCALE2
+    // @DisplayName: Compass2 scale factor
+    // @Description: Scaling factor for 2nd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done
+    // @User: Standard
+    // @Range: 0 1.3
+    AP_GROUPINFO("SCALE2", 41, Compass, _state[1].scale_factor, 0),
+#endif
+
+#if HAL_COMPASS_MAX_SENSORS > 2
+    // @Param: SCALE3
+    // @DisplayName: Compass3 scale factor
+    // @Description: Scaling factor for 3rd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done
+    // @User: Standard
+    // @Range: 0 1.3
+    AP_GROUPINFO("SCALE3", 42, Compass, _state[2].scale_factor, 0),
+#endif
+    
     AP_GROUPEND
 };
 
@@ -1007,6 +1032,14 @@ Compass::set_and_save_offdiagonals(uint8_t i, const Vector3f &offdiagonals)
     }
 }
 
+void
+Compass::set_and_save_scale_factor(uint8_t i, float scale_factor)
+{
+    if (i < COMPASS_MAX_INSTANCES) {
+        _state[i].scale_factor.set_and_save(scale_factor);
+    }
+}
+
 void
 Compass::save_offsets(uint8_t i)
 {
@@ -1309,6 +1342,19 @@ bool Compass::consistent() const
     return true;
 }
 
+/*
+  return true if we have a valid scale factor
+ */
+bool Compass::have_scale_factor(uint8_t i) const
+{
+    if (i >= get_count() ||
+        _state[i].scale_factor < COMPASS_MIN_SCALE_FACTOR ||
+        _state[i].scale_factor > COMPASS_MAX_SCALE_FACTOR) {
+        return false;
+    }
+    return true;
+}
+
 
 // singleton instance
 Compass *Compass::_singleton;

libraries/AP_Compass/AP_Compass.h

@@ -106,6 +106,7 @@ public:
     void set_and_save_offsets(uint8_t i, const Vector3f &offsets);
     void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals);
     void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals);
+    void set_and_save_scale_factor(uint8_t i, float scale_factor);
 
     /// Saves the current offset x/y/z values for one or all compasses
     ///
@@ -124,6 +125,9 @@ public:
     const Vector3f &get_field(uint8_t i) const { return _state[i].field; }
     const Vector3f &get_field(void) const { return get_field(get_primary()); }
 
+    /// Return true if we have set a scale factor for a compass
+    bool have_scale_factor(uint8_t i) const;
+
     // compass calibrator interface
     void cal_update();
 
@@ -431,6 +435,7 @@ private:
         AP_Vector3f offset;
         AP_Vector3f diagonals;
         AP_Vector3f offdiagonals;
+        AP_Float    scale_factor;
 
         // device id detected at init.
         // saved to eeprom when offsets are saved allowing ram &

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -59,6 +59,12 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
     // add in the basic offsets
     mag += offsets;
 
+    // add in scale factor, use a wide sanity check. The calibrator
+    // uses a narrower check.
+    if (_compass.have_scale_factor(i)) {
+        mag *= state.scale_factor;
+    }
+
     // apply eliptical correction
     Matrix3f mat(
         diagonals.x, offdiagonals.x, offdiagonals.y,

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -145,11 +145,13 @@ bool Compass::_accept_calibration(uint8_t i)
         _cal_saved[i] = true;
 
         Vector3f ofs, diag, offdiag;
-        cal.get_calibration(ofs, diag, offdiag);
+        float scale_factor;
+        cal.get_calibration(ofs, diag, offdiag, scale_factor);
 
         set_and_save_offsets(i, ofs);
         set_and_save_diagonals(i,diag);
         set_and_save_offdiagonals(i,offdiag);
+        set_and_save_scale_factor(i,scale_factor);
 
         if (_state[i].external && _rotate_auto >= 2) {
             _state[i].orientation.set_and_save_ifchanged(cal.get_orientation());
@@ -234,7 +236,8 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
             cal_status == COMPASS_CAL_BAD_ORIENTATION) {
             float fitness = _calibrator[compass_id].get_fitness();
             Vector3f ofs, diag, offdiag;
-            _calibrator[compass_id].get_calibration(ofs, diag, offdiag);
+            float scale_factor;
+            _calibrator[compass_id].get_calibration(ofs, diag, offdiag, scale_factor);
             uint8_t autosaved = _cal_saved[compass_id];
 
             mavlink_msg_mag_cal_report_send(
@@ -247,7 +250,8 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
                 offdiag.x, offdiag.y, offdiag.z,
                 _calibrator[compass_id].get_orientation_confidence(),
                 _calibrator[compass_id].get_original_orientation(),
-                _calibrator[compass_id].get_orientation()
+                _calibrator[compass_id].get_orientation(),
+                scale_factor
             );
         }
     }

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -115,6 +115,9 @@ void AP_Compass_SITL::_timer()
             // rotate the first compass, allowing for testing of external compass rotation
             f.rotate_inverse((enum Rotation)_sitl->mag_orient.get());
             f.rotate(get_board_orientation());
+
+            // scale the first compass to simulate sensor scale factor errors
+            f *= _sitl->mag_scaling;
         }
         
         accumulate_sample(f, _compass_instance[i], 10);

libraries/AP_Compass/CompassCalibrator.cpp

@@ -61,8 +61,12 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_GeodesicGrid.h>
 #include <AP_AHRS/AP_AHRS.h>
+#include <AP_GPS/AP_GPS.h>
 #include <GCS_MAVLink/GCS.h>
 
+#define FIELD_RADIUS_MIN 150
+#define FIELD_RADIUS_MAX 950
+
 extern const AP_HAL::HAL& hal;
 
 ////////////////////////////////////////////////////////////
@@ -104,7 +108,7 @@ void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint
     set_status(COMPASS_CAL_WAITING_TO_START);
 }
 
-void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals)
+void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor)
 {
     if (_status != COMPASS_CAL_SUCCESS) {
         return;
@@ -113,6 +117,7 @@ void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals,
     offsets = _params.offset;
     diagonals = _params.diag;
     offdiagonals = _params.offdiag;
+    scale_factor = _params.scale_factor;
 }
 
 float CompassCalibrator::get_completion_percent() const
@@ -131,6 +136,7 @@ float CompassCalibrator::get_completion_percent() const
             return 100.0f;
         case COMPASS_CAL_FAILED:
         case COMPASS_CAL_BAD_ORIENTATION:
+        case COMPASS_CAL_BAD_RADIUS:
         default:
             return 0.0f;
     };
@@ -215,7 +221,7 @@ void CompassCalibrator::update(bool &failure)
         }
     } else if (_status == COMPASS_CAL_RUNNING_STEP_TWO) {
         if (_fit_step >= 35) {
-            if (fit_acceptable() && calculate_orientation()) {
+            if (fit_acceptable() && fix_radius() && calculate_orientation()) {
                 set_status(COMPASS_CAL_SUCCESS);
             } else {
                 set_status(COMPASS_CAL_FAILED);
@@ -266,6 +272,7 @@ void CompassCalibrator::reset_state()
     _params.offset.zero();
     _params.diag = Vector3f(1.0f,1.0f,1.0f);
     _params.offdiag.zero();
+    _params.scale_factor = 0;
 
     memset(_completion_mask, 0, sizeof(_completion_mask));
     initialize_fit();
@@ -336,13 +343,15 @@ bool CompassCalibrator::set_status(compass_cal_status_t status)
             return true;
 
         case COMPASS_CAL_FAILED:
-            if (_status == COMPASS_CAL_BAD_ORIENTATION) {
+            if (_status == COMPASS_CAL_BAD_ORIENTATION ||
+                _status == COMPASS_CAL_BAD_RADIUS) {
                 // don't overwrite bad orientation status
                 return false;
             }
             FALLTHROUGH;
             
         case COMPASS_CAL_BAD_ORIENTATION:
+        case COMPASS_CAL_BAD_RADIUS:
             if (_status == COMPASS_CAL_NOT_STARTED) {
                 return false;
             }
@@ -368,7 +377,7 @@ 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
+        _params.radius > FIELD_RADIUS_MIN && _params.radius < FIELD_RADIUS_MAX &&
         fabsf(_params.offset.x) < _offset_max &&
         fabsf(_params.offset.y) < _offset_max &&
         fabsf(_params.offset.z) < _offset_max &&
@@ -923,3 +932,40 @@ bool CompassCalibrator::calculate_orientation(void)
 
     return fit_acceptable();
 }
+
+/*
+  fix radius of the fit to compensate for sensor scale factor errors
+  return false if radius is outside acceptable range
+ */
+bool CompassCalibrator::fix_radius(void)
+{
+    if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
+        // we don't have a position, leave scale factor as 0. This
+        // will disable use of WMM in the EKF. Users can manually set
+        // scale factor after calibration if it is known
+        _params.scale_factor = 0;
+        return true;
+    }
+    const struct Location &loc = AP::gps().location();
+    float intensity;
+    float declination;
+    float inclination;
+    AP_Declination::get_mag_field_ef(loc.lat * 1e-7f, loc.lng * 1e-7f, intensity, declination, inclination);
+
+    float expected_radius = intensity * 1000; // mGauss
+    float correction = expected_radius / _params.radius;
+
+    if (correction > COMPASS_MAX_SCALE_FACTOR || correction < COMPASS_MIN_SCALE_FACTOR) {
+        // don't allow more than 30% scale factor correction
+        gcs().send_text(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f",
+                        _compass_idx,
+                        _params.radius,
+                        expected_radius);
+        set_status(COMPASS_CAL_BAD_RADIUS);
+        return false;
+    }
+
+    _params.scale_factor = correction;
+
+    return true;
+}

libraries/AP_Compass/CompassCalibrator.h

@@ -16,8 +16,12 @@ enum compass_cal_status_t {
     COMPASS_CAL_SUCCESS = 4,
     COMPASS_CAL_FAILED = 5,
     COMPASS_CAL_BAD_ORIENTATION = 6,
+    COMPASS_CAL_BAD_RADIUS = 7,
 };
 
+#define COMPASS_MIN_SCALE_FACTOR 0.3
+#define COMPASS_MAX_SCALE_FACTOR 1.6
+
 class CompassCalibrator {
 public:
     CompassCalibrator();
@@ -45,7 +49,7 @@ public:
     enum compass_cal_status_t get_status() const { return _status; }
 
     // get calibration outputs (offsets, diagonals, offdiagonals) and fitness
-    void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
+    void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor);
     float get_fitness() const { return sqrtf(_fitness); }
 
     // get corrected (and original) orientation
@@ -80,6 +84,7 @@ private:
         Vector3f offset;    // offsets
         Vector3f diag;      // diagonal scaling
         Vector3f offdiag;   // off diagonal scaling
+        float scale_factor; // scaling factor to compensate for radius error
     };
 
     // compact class for approximate attitude, to save memory
@@ -155,6 +160,9 @@ private:
     Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
     bool calculate_orientation();
 
+    // fix radius to compensate for sensor scaling errors
+    bool fix_radius();
+
     uint8_t _compass_idx;                   // index of the compass providing data
     enum compass_cal_status_t _status;      // current state of calibrator
     uint32_t _last_sample_ms;               // system time of last sample received for timeout

libraries/AP_Compass/Compass_learn.cpp

@@ -19,6 +19,13 @@ CompassLearn::CompassLearn(Compass &_compass) :
     compass(_compass)
 {
     gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised");
+    for (uint8_t i=0; i<compass.get_count(); i++) {
+        if (compass._state[i].use_for_yaw) {
+            // reset scale factors, we can't learn scale factors in
+            // flight
+            compass.set_and_save_scale_factor(i, 0.0);
+        }
+    }
 }
 
 /*
@@ -133,6 +140,7 @@ void CompassLearn::update(void)
             for (uint8_t i=0; i<compass.get_count(); i++) {
                 if (compass._state[i].use_for_yaw) {
                     compass.save_offsets(i);
+                    compass.set_and_save_scale_factor(i, 0.0);
                     compass.set_use_for_yaw(i, true);
                 }
             }