From f20ef6977761d94a5be69bffeca991a01dd00f77 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Wed, 18 Mar 2015 16:18:47 -0700
Subject: [PATCH] Compass: add soft-iron corrections

---
 libraries/AP_Compass/AP_Compass_Backend.cpp |  14 +++
 libraries/AP_Compass/Compass.cpp            | 118 ++++++++++++++++++++
 libraries/AP_Compass/Compass.h              |   4 +
 3 files changed, 136 insertions(+)

diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp
index d6b8ef8f8a..5f9947bc8f 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.cpp
+++ b/libraries/AP_Compass/AP_Compass_Backend.cpp
@@ -53,7 +53,13 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
 {
     Compass::mag_state &state = _compass._state[i];
 
+    if (state.diagonals.get().is_zero()) {
+        state.diagonals.set(Vector3f(1.0f,1.0f,1.0f));
+    }
+
     const Vector3f &offsets = state.offset.get();
+    const Vector3f &diagonals = state.diagonals.get();
+    const Vector3f &offdiagonals = state.offdiagonals.get();
     const Vector3f &mot = state.motor_compensation.get();
 
     /*
@@ -67,6 +73,14 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
     } else {
         state.motor_offset.zero();
     }
+
+    Matrix3f mat(
+        diagonals.x, offdiagonals.x, offdiagonals.y,
+        offdiagonals.x,    diagonals.y, offdiagonals.z,
+        offdiagonals.y, offdiagonals.z,    diagonals.z
+    );
+
+    mag = mat * mag;
 }
 
 void AP_Compass_Backend::publish_unfiltered_field(const Vector3f &mag, uint32_t time_us, uint8_t instance)
diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp
index 4223b9169f..ddb4d9b1d6 100644
--- a/libraries/AP_Compass/Compass.cpp
+++ b/libraries/AP_Compass/Compass.cpp
@@ -265,6 +265,106 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
     AP_GROUPINFO("EXTERN3",23, Compass, _state[2].external, 0),
 #endif
 
+    // @Param: DIA_X
+    // @DisplayName: Compass soft-iron diagonal X component
+    // @Description: DIA_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: DIA_Y
+    // @DisplayName: Compass soft-iron diagonal Y component
+    // @Description: DIA_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: DIA_Z
+    // @DisplayName: Compass soft-iron diagonal Z component
+    // @Description: DIA_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+    AP_GROUPINFO("DIA",    24, Compass, _state[0].diagonals, 0),
+
+    // @Param: ODI_X
+    // @DisplayName: Compass soft-iron off-diagonal X component
+    // @Description: ODI_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: ODI_Y
+    // @DisplayName: Compass soft-iron off-diagonal Y component
+    // @Description: ODI_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: ODI_Z
+    // @DisplayName: Compass soft-iron off-diagonal Z component
+    // @Description: ODI_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+    AP_GROUPINFO("ODI",    25, Compass, _state[0].offdiagonals, 0),
+
+#if COMPASS_MAX_INSTANCES > 1
+    // @Param: DIA2_X
+    // @DisplayName: Compass2 soft-iron diagonal X component
+    // @Description: DIA_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: DIA2_Y
+    // @DisplayName: Compass2 soft-iron diagonal Y component
+    // @Description: DIA_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: DIA2_Z
+    // @DisplayName: Compass2 soft-iron diagonal Z component
+    // @Description: DIA_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+    AP_GROUPINFO("DIA2",    26, Compass, _state[1].diagonals, 0),
+
+    // @Param: ODI2_X
+    // @DisplayName: Compass2 soft-iron off-diagonal X component
+    // @Description: ODI_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: ODI2_Y
+    // @DisplayName: Compass2 soft-iron off-diagonal Y component
+    // @Description: ODI_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: ODI2_Z
+    // @DisplayName: Compass2 soft-iron off-diagonal Z component
+    // @Description: ODI_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+    AP_GROUPINFO("ODI2",    27, Compass, _state[1].offdiagonals, 0),
+#endif
+
+#if COMPASS_MAX_INSTANCES > 2
+    // @Param: DIA3_X
+    // @DisplayName: Compass3 soft-iron diagonal X component
+    // @Description: DIA_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: DIA3_Y
+    // @DisplayName: Compass3 soft-iron diagonal Y component
+    // @Description: DIA_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: DIA3_Z
+    // @DisplayName: Compass3 soft-iron diagonal Z component
+    // @Description: DIA_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+    AP_GROUPINFO("DIA3",    28, Compass, _state[2].diagonals, 0),
+
+    // @Param: ODI3_X
+    // @DisplayName: Compass3 soft-iron off-diagonal X component
+    // @Description: ODI_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: ODI3_Y
+    // @DisplayName: Compass3 soft-iron off-diagonal Y component
+    // @Description: ODI_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+
+    // @Param: ODI3_Z
+    // @DisplayName: Compass3 soft-iron off-diagonal Z component
+    // @Description: ODI_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
+    // @User: Advanced
+    AP_GROUPINFO("ODI3",    29, Compass, _state[2].offdiagonals, 0),
+#endif
+
     AP_GROUPEND
 };
 
@@ -405,6 +505,24 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
     }
 }
 
+void
+Compass::set_and_save_diagonals(uint8_t i, const Vector3f &diagonals)
+{
+    // sanity check compass instance provided
+    if (i < COMPASS_MAX_INSTANCES) {
+        _state[i].diagonals.set_and_save(diagonals);
+    }
+}
+
+void
+Compass::set_and_save_offdiagonals(uint8_t i, const Vector3f &offdiagonals)
+{
+    // sanity check compass instance provided
+    if (i < COMPASS_MAX_INSTANCES) {
+        _state[i].offdiagonals.set_and_save(offdiagonals);
+    }
+}
+
 void
 Compass::save_offsets(uint8_t i)
 {
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index d7367d609c..555511b134 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/libraries/AP_Compass/Compass.h
@@ -88,6 +88,8 @@ public:
     /// @param  offsets             Offsets to the raw mag_ values.
     ///
     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);
 
     /// Saves the current offset x/y/z values for one or all compasses
     ///
@@ -315,6 +317,8 @@ private:
         bool        healthy;
         AP_Int8     orientation;
         AP_Vector3f offset;
+        AP_Vector3f diagonals;
+        AP_Vector3f offdiagonals;
 
 #if COMPASS_MAX_INSTANCES > 1
         // device id detected at init.