Compass: implement automatic compass orientation

Tools/ardupilotwaf/chibios.py

@@ -297,7 +297,7 @@ def build(bld):
     bld(
         # build hwdef.h and apj.prototype from hwdef.dat. This is needed after a waf clean
         source=bld.path.ant_glob(bld.env.HWDEF),
-        rule="python '${AP_HAL_ROOT}/hwdef/scripts/chibios_hwdef.py' -D '${BUILDROOT}' %s %s" % (bld.env.HWDEF, bld.env.BOOTLOADER_OPTION),
+        rule="python '${AP_HAL_ROOT}/hwdef/scripts/chibios_hwdef.py' -D '${BUILDROOT}' '%s' %s" % (bld.env.HWDEF, bld.env.BOOTLOADER_OPTION),
         group='dynamic_sources',
         target=[bld.bldnode.find_or_declare('hwdef.h'),
                 bld.bldnode.find_or_declare('apj.prototype'),

libraries/AP_AHRS/AP_AHRS.h

@@ -263,6 +263,9 @@ class AP_AHRS
     const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
     const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
 
+    // get rotation matrix specifically from DCM backend (used for compass calibrator)
+    virtual const Matrix3f &get_DCM_rotation_body_to_ned(void) const = 0;
+
     // get our current position estimate. Return true if a position is available,
     // otherwise false. This call fills in lat, lng and alt
     virtual bool get_position(struct Location &loc) const = 0;

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -436,6 +436,11 @@ AP_AHRS_DCM::drift_correction_yaw(void)
 
     const AP_GPS &_gps = AP::gps();
 
+    if (_compass && _compass->is_calibrating()) {
+        // don't do any yaw correction while calibrating
+        return;
+    }
+
     if (AP_AHRS_DCM::use_compass()) {
         /*
           we are using compass for yaw

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -53,6 +53,9 @@ class AP_AHRS_DCM : public AP_AHRS {
         return _body_dcm_matrix;
     }
 
+    // get rotation matrix specifically from DCM backend (used for compass calibrator)
+    const Matrix3f &get_DCM_rotation_body_to_ned(void) const override { return _body_dcm_matrix; }
+
     // return the current drift correction integrator value
     const Vector3f &get_gyro_drift() const override {
         return _omega_I;

libraries/AP_Compass/AP_Compass.cpp

@@ -443,6 +443,12 @@ const AP_Param::GroupInfo Compass::var_info[] = {
     // @Range: 0 100
     // @Increment: 1
     AP_GROUPINFO("FLTR_RNG", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT),
+
+    // @Param: AUTO_ROT
+    // @DisplayName: Automatically check orientation
+    // @Description: When enabled this will automatically check the orientation of compasses on successful completion of compass calibration. If set to 2 then external compasses will have their orientation automatically corrected.
+    // @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix
+    AP_GROUPINFO("AUTO_ROT", 35, Compass, _rotate_auto, 2),
 
     AP_GROUPEND
 };

libraries/AP_Compass/AP_Compass.h

@@ -417,6 +417,9 @@ friend class AP_Compass_Backend;
     // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
     AP_Int8     _motor_comp_type;
 
+    // automatic compass orientation on calibration
+    AP_Int8     _rotate_auto;
+
     // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation
     float       _thr;
 

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -61,8 +61,14 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay)
         // lot noisier
         _calibrator[i].set_tolerance(_calibration_threshold*2);
     }
+    if (_rotate_auto) {
+        enum Rotation r = _state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE;
+        if (r != ROTATION_CUSTOM) {
+            _calibrator[i].set_orientation(r, _state[i].external, _rotate_auto>=2);
+        }
+    }
     _cal_saved[i] = false;
-    _calibrator[i].start(retry, delay, get_offsets_max());
+    _calibrator[i].start(retry, delay, get_offsets_max(), i);
 
     // disable compass learning both for calibration and after completion
     _learn.set_and_save(0);
@@ -147,6 +153,10 @@ Compass::_accept_calibration(uint8_t i)
         set_and_save_diagonals(i,diag);
         set_and_save_offdiagonals(i,offdiag);
 
+        if (_state[i].external && _rotate_auto >= 2) {
+            _state[i].orientation.set_and_save_ifchanged(cal.get_orientation());
+        }
+
         if (!is_calibrating()) {
             AP_Notify::events.compass_cal_saved = 1;
         }
@@ -215,8 +225,9 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
         }
 
         uint8_t cal_status = _calibrator[compass_id].get_status();
-        if ((cal_status == COMPASS_CAL_SUCCESS ||
-            cal_status == COMPASS_CAL_FAILED)) {
+        if (cal_status == COMPASS_CAL_SUCCESS ||
+            cal_status == COMPASS_CAL_FAILED ||
+            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);
@@ -229,7 +240,10 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
                 fitness,
                 ofs.x, ofs.y, ofs.z,
                 diag.x, diag.y, diag.z,
-                offdiag.x, offdiag.y, offdiag.z
+                offdiag.x, offdiag.y, offdiag.z,
+                _calibrator[compass_id].get_orientation_confidence(),
+                _calibrator[compass_id].get_original_orientation(),
+                _calibrator[compass_id].get_orientation()
             );
         }
     }
@@ -243,6 +257,7 @@ Compass::is_calibrating() const
             case COMPASS_CAL_NOT_STARTED:
             case COMPASS_CAL_SUCCESS:
             case COMPASS_CAL_FAILED:
+            case COMPASS_CAL_BAD_ORIENTATION:
                 break;
             default:
                 return true;

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -22,10 +22,41 @@ AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
             // save so the compass always comes up configured in SITL
             save_dev_id(_compass_instance[i]);
         }
+
+        // make first compass external
+        set_external(_compass_instance[0], true);
+
         hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
     }
 }
 
+
+/*
+  create correction matrix for diagnonals and off-diagonals
+*/
+void AP_Compass_SITL::_setup_eliptical_correcion(void)
+{
+    Vector3f diag = _sitl->mag_diag.get();
+    if (diag.is_zero()) {
+        diag(1,1,1);
+    }
+    const Vector3f &diagonals = diag;
+    const Vector3f &offdiagonals = _sitl->mag_offdiag;
+
+    if (diagonals == _last_dia && offdiagonals == _last_odi) {
+        return;
+    }
+
+    _eliptical_corr = Matrix3f(diagonals.x,    offdiagonals.x, offdiagonals.y,
+                               offdiagonals.x, diagonals.y,    offdiagonals.z,
+                               offdiagonals.y, offdiagonals.z, diagonals.z);
+    if (!_eliptical_corr.invert()) {
+        _eliptical_corr.identity();
+    }
+    _last_dia = diag;
+    _last_odi = offdiagonals;
+}
+
 void AP_Compass_SITL::_timer()
 {
     // TODO: Refactor delay buffer with AP_Baro_SITL.
@@ -73,22 +104,33 @@ void AP_Compass_SITL::_timer()
         new_mag_data = buffer[best_index].data;
     }
 
+    _setup_eliptical_correcion();        
+
+    new_mag_data = _eliptical_corr * new_mag_data;
     new_mag_data -= _sitl->mag_ofs.get();
 
     for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
-        rotate_field(new_mag_data, _compass_instance[i]);
-        publish_raw_field(new_mag_data, _compass_instance[i]);
-        correct_field(new_mag_data, _compass_instance[i]);
+        Vector3f f = new_mag_data;
+        if (i == 0) {
+            // rotate the first compass, allowing for testing of external compass rotation
+            f.rotate_inverse((enum Rotation)_sitl->mag_orient.get());
+        }
+        rotate_field(f, _compass_instance[i]);
+        publish_raw_field(f, _compass_instance[i]);
+        correct_field(f, _compass_instance[i]);
+
+        _mag_accum[i] += f;
     }
 
     if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         return;
     }
 
-    _mag_accum += new_mag_data;
     _accum_count++;
     if (_accum_count == 10) {
-        _mag_accum /= 2;
+        for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
+            _mag_accum[i] /= 2;
+        }
         _accum_count = 5;
         _has_sample = true;
     }
@@ -103,14 +145,13 @@ void AP_Compass_SITL::read()
             return;
         }
 
-        Vector3f field(_mag_accum);
-        field /= _accum_count;
-        _mag_accum.zero();
-        _accum_count = 0;
-
         for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
+            Vector3f field(_mag_accum[i]);
+            field /= _accum_count;
+            _mag_accum[i].zero();
             publish_filtered_field(field, _compass_instance[i]);
         }
+        _accum_count = 0;
 
         _has_sample = false;
         _sem->give();

libraries/AP_Compass/AP_Compass_SITL.h

@@ -35,9 +35,13 @@ class AP_Compass_SITL : public AP_Compass_Backend {
     bool _has_sample;
     uint32_t _last_sample_time;
 
-    Vector3f _mag_accum;
+    Vector3f _mag_accum[SITL_NUM_COMPASSES];
     uint32_t _accum_count;
 
-
+    void _setup_eliptical_correcion();
+
+    Matrix3f _eliptical_corr;
+    Vector3f _last_dia;
+    Vector3f _last_odi;
 };
 #endif // CONFIG_HAL_BOARD