From 664622523ddcfdba31ca3ca4f8aace85e31d5bcf Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 25 Feb 2012 14:11:07 +1100
Subject: [PATCH] Compass: added COMPASS_LEARN and COMPASS_USE parameters

these allow you to control if the compass should be used for yaw and
if it should learn its offsets. This is useful for locking in compass
offsets once they are confirmed to be good, and for learning offsets
without using them in flights.

The default is to behave the same as previously, which is
COMPASS_LEARN=1 and COMPASS_USE=1
---
 libraries/AP_Compass/AP_Compass_HMC5843.cpp |  3 +++
 libraries/AP_Compass/Compass.cpp            | 20 ++++++++++++++++----
 libraries/AP_Compass/Compass.h              |  4 ++++
 3 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
index 12809d74a8..080eaad0ab 100644
--- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp
+++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
@@ -138,6 +138,9 @@ AP_Compass_HMC5843::init()
   uint16_t expected_yz = 715;
   float gain_multiple = 1.0;
 
+  // call the parent class init for common code
+  Compass::init();
+
   delay(10);
 
   // determine if we are using 5843 or 5883L
diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp
index ea9586b49d..c9d4a4e7be 100644
--- a/libraries/AP_Compass/Compass.cpp
+++ b/libraries/AP_Compass/Compass.cpp
@@ -5,6 +5,8 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
     AP_GROUPINFO("ORIENT", 0, Compass, _orientation_matrix),
     AP_GROUPINFO("OFS",    1, Compass, _offset),
     AP_GROUPINFO("DEC",    2, Compass, _declination),
+    AP_GROUPINFO("LEARN",  3, Compass, _learn), // true if learning calibration
+    AP_GROUPINFO("USE",    4, Compass, _use_for_yaw), // true if used for DCM yaw
     AP_GROUPEND
 };
 
@@ -30,6 +32,14 @@ Compass::Compass(void) :
 bool
 Compass::init()
 {
+    // enable learning by default
+    if (!_learn.load()) {
+        _learn.set(1);
+    }
+    // enable use for yaw calculation by default
+    if (!_use_for_yaw.load()) {
+        _use_for_yaw.set(1);
+    }
     return true;
 }
 
@@ -166,15 +176,18 @@ Compass::calculate(const Matrix3f &dcm_matrix)
 void
 Compass::null_offsets(const Matrix3f &dcm_matrix)
 {
+    if (_null_enable == false || _learn == 0) {
+        // auto-calibration is disabled
+        return;
+    }
+
     // Update our estimate of the offsets in the magnetometer
-    Vector3f    calc(0.0, 0.0, 0.0);        // XXX should be safe to remove explicit init here as the default ctor should do the right thing
+    Vector3f    calc;
     Matrix3f    dcm_new_from_last;
     float       weight;
 
     Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);
     
-    if(_null_enable == false) return;
-
     if(_null_init_done) {
         dcm_new_from_last = dcm_matrix.transposed() * _last_dcm_matrix;      // Note 11/20/2010: transpose() is not working, transposed() is.
 
@@ -192,7 +205,6 @@ Compass::null_offsets(const Matrix3f &dcm_matrix)
     }
     _mag_body_last = mag_body_new - calc;
     _last_dcm_matrix = dcm_matrix;
-
 }
 
 
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index a047ef0717..cc3bc5e704 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/libraries/AP_Compass/Compass.h
@@ -124,6 +124,8 @@ public:
 	///
 	void null_offsets_disable(void);
 
+    /// return true if the compass should be used for yaw calculations
+    bool use_for_yaw(void) { return healthy && _use_for_yaw; }
 
 	/// Sets the local magnetic field declination.
 	///
@@ -138,6 +140,8 @@ protected:
 	AP_Matrix3f         _orientation_matrix;
 	AP_Vector3f         _offset;
 	AP_Float            _declination;
+    AP_Int8             _learn;                 ///<enable calibration learning
+    AP_Int8             _use_for_yaw;           ///<enable use for yaw calculation
 
 	bool                _null_enable;        	///< enabled flag for offset nulling
 	bool                _null_init_done;        ///< first-time-around flag used by offset nulling