From 9054dd3f9a8befb09cfb4359c643cb875b528242 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Thu, 31 Jul 2014 18:19:53 -0700
Subject: [PATCH] AP_Compass: use apply_correction_function to eliminate
 duplication

---
 libraries/AP_Compass/AP_Compass_HIL.cpp     | 15 +++------------
 libraries/AP_Compass/AP_Compass_HMC5843.cpp | 10 +---------
 libraries/AP_Compass/AP_Compass_PX4.cpp     | 13 ++-----------
 libraries/AP_Compass/AP_Compass_VRBRAIN.cpp | 11 +----------
 libraries/AP_Compass/Compass.cpp            | 14 ++++++++++++++
 libraries/AP_Compass/Compass.h              |  2 ++
 6 files changed, 23 insertions(+), 42 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp
index 5875519b24..4d8df0e45d 100644
--- a/libraries/AP_Compass/AP_Compass_HIL.cpp
+++ b/libraries/AP_Compass/AP_Compass_HIL.cpp
@@ -50,18 +50,9 @@ void AP_Compass_HIL::_setup_earth_field(void)
 
 bool AP_Compass_HIL::read()
 {
-    // get offsets
-    Vector3f ofs = _offset[0].get();
-
-    // apply motor compensation
-    if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
-        _motor_offset[0] = _motor_compensation[0].get() * _thr_or_curr;
-    }else{
-        _motor_offset[0].zero();
-    }
-
-    // return last values provided by setHIL function
-    _field[0] = _hil_mag + ofs + _motor_offset[0];
+    _field[0] = _hil_mag;
+    
+    apply_corrections(_field[0],0);
 
     // values set by setHIL function
     last_update = hal.scheduler->micros();      // record time of update
diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
index 23b3491756..323849d349 100644
--- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp
+++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
@@ -371,15 +371,7 @@ bool AP_Compass_HMC5843::read()
         _field[0].rotate(_board_orientation);
     }
 
-    _field[0] += _offset[0].get();
-
-    // apply motor compensation
-    if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
-        _motor_offset[0] = _motor_compensation[0].get() * _thr_or_curr;
-        _field[0] += _motor_offset[0];
-    }else{
-        _motor_offset[0].zero();
-    }
+    apply_corrections(_field[0],0);
 
     _healthy[0] = true;
 
diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp
index 75c77905a3..8ab642fad0 100644
--- a/libraries/AP_Compass/AP_Compass_PX4.cpp
+++ b/libraries/AP_Compass/AP_Compass_PX4.cpp
@@ -115,18 +115,9 @@ bool AP_Compass_PX4::read(void)
             // add in board orientation from AHRS
             _sum[i].rotate(_board_orientation);
         }
-
-        _sum[i] += _offset[i].get();
-
-        // apply motor compensation
-        if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
-            _motor_offset[i] = _motor_compensation[i].get() * _thr_or_curr;
-            _sum[i] += _motor_offset[i];
-        } else {
-            _motor_offset[i].zero();
-        }
-    
+        
         _field[i] = _sum[i];
+        apply_corrections(_field[i],i);
     
         _sum[i].zero();
         _count[i] = 0;
diff --git a/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp b/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp
index bf3408bcd4..beef714bf7 100644
--- a/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp
+++ b/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp
@@ -124,18 +124,9 @@ bool AP_Compass_VRBRAIN::read(void)
             // add in board orientation from AHRS
             _sum[i].rotate(_board_orientation);
         }
-
-        _sum[i] += _offset[i].get();
-
-        // apply motor compensation
-        if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
-            _motor_offset[i] = _motor_compensation[i].get() * _thr_or_curr;
-            _sum[i] += _motor_offset[i];
-        } else {
-            _motor_offset[i].zero();
-        }
     
         _field[i] = _sum[i];
+        apply_corrections(_field[i],i);
     
         _sum[i].zero();
         _count[i] = 0;
diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp
index 5d035ac843..72324406c4 100644
--- a/libraries/AP_Compass/Compass.cpp
+++ b/libraries/AP_Compass/Compass.cpp
@@ -462,3 +462,17 @@ bool Compass::configured(void)
     }
     return all_configured;
 }
+
+void Compass::apply_corrections(Vector3f &mag, uint8_t i)
+{
+    const Vector3f &offsets = _offset[i].get();
+    const Vector3f &mot = _motor_compensation[i].get();
+    
+    mag += offsets;
+    if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
+        _motor_offset[i] = mot * _thr_or_curr;
+        mag += _motor_offset[i];
+    }else{
+        _motor_offset[i].zero();
+    }
+}
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index 1a6e76227a..97d6de488c 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/libraries/AP_Compass/Compass.h
@@ -282,5 +282,7 @@ protected:
 
     // board orientation from AHRS
     enum Rotation _board_orientation;
+    
+    void apply_corrections(Vector3f &mag, uint8_t i);
 };
 #endif