From 0efbe8c80c45425f98bd9eb4b82dff2c6f97e732 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Fri, 22 Jan 2016 15:30:59 -0200
Subject: [PATCH] AP_Compass: allow calling calculate_heading() for different
 instances

---
 libraries/AP_Compass/Compass.cpp | 4 ++--
 libraries/AP_Compass/Compass.h   | 5 ++++-
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp
index 7ccff1c0d1..26b5f05d4d 100644
--- a/libraries/AP_Compass/Compass.cpp
+++ b/libraries/AP_Compass/Compass.cpp
@@ -623,12 +623,12 @@ Compass::get_declination() const
   calculate a compass heading given the attitude from DCM and the mag vector
  */
 float
-Compass::calculate_heading(const Matrix3f &dcm_matrix) const
+Compass::calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const
 {
     float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
 
     // Tilt compensated magnetic field Y component:
-    const Vector3f &field = get_field();
+    const Vector3f &field = get_field(i);
 
     float headY = field.y * dcm_matrix.c.z - field.z * dcm_matrix.c.y;
 
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index 3c8518e915..d2b8f4e443 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/libraries/AP_Compass/Compass.h
@@ -82,7 +82,10 @@ public:
     ///
     /// @returns heading in radians
     ///
-    float calculate_heading(const Matrix3f &dcm_matrix) const;
+    float calculate_heading(const Matrix3f &dcm_matrix) const {
+        return calculate_heading(dcm_matrix, get_primary());
+    }
+    float calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const;
 
     /// Sets offset x/y/z values.
     ///