AP_Compass: allow for runtime changes to SIM_MAG_DIA_?

this makes for easier testing

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -25,23 +25,36 @@ AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
         
         // 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
+
-        Vector3f diag = _sitl->mag_diag.get();
+/*
-        if (diag.is_zero()) {
+  create correction matrix for diagnonals and off-diagonals
-            diag(1,1,1);
+*/
-        }
+void AP_Compass_SITL::_setup_eliptical_correcion(void)
-        const Vector3f &diagonals = diag;
+{
-        const Vector3f &offdiagonals = _sitl->mag_offdiag;
+    Vector3f diag = _sitl->mag_diag.get();
-        _eliptical_corr = Matrix3f(diagonals.x,    offdiagonals.x, offdiagonals.y,
+    if (diag.is_zero()) {
-                                   offdiagonals.x, diagonals.y,    offdiagonals.z,
+        diag(1,1,1);
-                                   offdiagonals.y, offdiagonals.z, diagonals.z);
+    }
-        if (!_eliptical_corr.invert()) {
+    const Vector3f &diagonals = diag;
-            _eliptical_corr.identity();
+    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()
@@ -91,6 +104,8 @@ 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();
 

libraries/AP_Compass/AP_Compass_SITL.h

@@ -37,8 +37,11 @@ private:
 
     Vector3f _mag_accum[SITL_NUM_COMPASSES];
     uint32_t _accum_count;
-    Matrix3f _eliptical_corr;
-
 
+    void _setup_eliptical_correcion();
+    
+    Matrix3f _eliptical_corr;
+    Vector3f _last_dia;
+    Vector3f _last_odi;
 };
 #endif // CONFIG_HAL_BOARD