From fbf79c07f080239232c1463dc65e80a82a4b67ca Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 30 Aug 2013 14:02:09 +1000
Subject: [PATCH] AP_Compass: added COMPASS_EXTERNAL option

this allows configuring of a compass as being externally attached
---
 libraries/AP_Compass/AP_Compass_HIL.cpp     |  6 ++++--
 libraries/AP_Compass/AP_Compass_HMC5843.cpp |  6 ++++--
 libraries/AP_Compass/AP_Compass_PX4.cpp     | 13 +++++++++++--
 libraries/AP_Compass/AP_Compass_PX4.h       |  1 +
 libraries/AP_Compass/Compass.cpp            |  7 +++++++
 libraries/AP_Compass/Compass.h              |  1 +
 6 files changed, 28 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp
index 2cbb322123..843f54c5f9 100644
--- a/libraries/AP_Compass/AP_Compass_HIL.cpp
+++ b/libraries/AP_Compass/AP_Compass_HIL.cpp
@@ -102,8 +102,10 @@ void AP_Compass_HIL::setHIL(float roll, float pitch, float yaw)
     // add user selectable orientation
     _hil_mag.rotate((enum Rotation)_orientation.get());
 
-    // and add in AHRS_ORIENTATION setting
-    _hil_mag.rotate(_board_orientation);
+    if (!_external) {
+        // and add in AHRS_ORIENTATION setting if not an external compass
+        _hil_mag.rotate(_board_orientation);
+    }
 
     healthy = true;
 }
diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
index b9a2163d92..5119b8c5fc 100644
--- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp
+++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
@@ -340,8 +340,10 @@ bool AP_Compass_HMC5843::read()
     // add user selectable orientation
     rot_mag.rotate((enum Rotation)_orientation.get());
 
-    // add in board orientation from AHRS
-    rot_mag.rotate(_board_orientation);
+    if (!_external) {
+        // and add in AHRS_ORIENTATION setting if not an external compass
+        rot_mag.rotate(_board_orientation);
+    }
 
     rot_mag += _offset.get();
 
diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp
index d50769121c..fd0ad37208 100644
--- a/libraries/AP_Compass/AP_Compass_PX4.cpp
+++ b/libraries/AP_Compass/AP_Compass_PX4.cpp
@@ -57,6 +57,9 @@ bool AP_Compass_PX4::init(void)
     // average over up to 20 samples
     ioctl(_mag_fd, SENSORIOCSQUEUEDEPTH, 20);
 
+    // remember if the compass is external
+	_is_external = (ioctl(_mag_fd, MAGIOCGEXTERNAL, 0) > 0);
+
     healthy = false;
     _count = 0;
     _sum.zero();
@@ -89,8 +92,14 @@ bool AP_Compass_PX4::read(void)
     // add user selectable orientation
     _sum.rotate((enum Rotation)_orientation.get());
 
-    // and add in AHRS_ORIENTATION setting
-    _sum.rotate(_board_orientation);
+    // override any user setting of COMPASS_EXTERNAL 
+    _external.set(_is_external);
+
+    if (!_external) {
+        // add in board orientation from AHRS
+        _sum.rotate(_board_orientation);
+    }
+
     _sum += _offset.get();
 
     // apply motor compensation
diff --git a/libraries/AP_Compass/AP_Compass_PX4.h b/libraries/AP_Compass/AP_Compass_PX4.h
index c438490aca..7469ab6a2e 100644
--- a/libraries/AP_Compass/AP_Compass_PX4.h
+++ b/libraries/AP_Compass/AP_Compass_PX4.h
@@ -20,6 +20,7 @@ private:
     Vector3f _sum;
     uint32_t _count;
     uint64_t _last_timestamp;
+    bool _is_external;
 };
 
 #endif // AP_Compass_PX4_H
diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp
index dec6a81238..b53490a7e1 100644
--- a/libraries/AP_Compass/Compass.cpp
+++ b/libraries/AP_Compass/Compass.cpp
@@ -91,6 +91,13 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
     // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw136,24:Pitch90,25:Pitch270
     AP_GROUPINFO("ORIENT", 8, Compass, _orientation, ROTATION_NONE),
 
+    // @Param: EXTERNAL
+    // @DisplayName: Compass is attached via an external cable
+    // @Description: Configure compass so it is attached externally. This is auto-detected on PX4, but must be set correctly on an APM2. Set to 1 if the compass is externally connected. When externally connected the COMPASS_ORIENT option operates independently of the AHRS_ORIENTATION board orientation option
+    // @Values: 0:Internal,1:External
+    // @User: Advanced
+    AP_GROUPINFO("EXTERNAL", 9, Compass, _external, 0),
+
     AP_GROUPEND
 };
 
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index 86cff6f24e..1ff9b6a6b0 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/libraries/AP_Compass/Compass.h
@@ -197,6 +197,7 @@ protected:
     AP_Float _declination;
     AP_Int8 _use_for_yaw;                       ///<enable use for yaw calculation
     AP_Int8 _auto_declination;                  ///<enable automatic declination code
+    AP_Int8 _external;                          ///<compass is external
 
     bool _null_init_done;                           ///< first-time-around flag used by offset nulling