From f72dd560cb9f99b80a261691e75ec557adb04e72 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 2 May 2013 14:59:33 +1000
Subject: [PATCH] AP_Compass: fixed the HIL sensors compass

this sets up good compass values for HIL sensors
---
 libraries/AP_Compass/AP_Compass_HIL.cpp | 59 ++++++++++++++++++++-----
 libraries/AP_Compass/AP_Compass_HIL.h   |  9 ++--
 2 files changed, 52 insertions(+), 16 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp
index 667e85b0a7..fa674223c7 100644
--- a/libraries/AP_Compass/AP_Compass_HIL.cpp
+++ b/libraries/AP_Compass/AP_Compass_HIL.cpp
@@ -14,6 +14,26 @@
 
 extern const AP_HAL::HAL& hal;
 
+// constructor
+AP_Compass_HIL::AP_Compass_HIL() : Compass() 
+{
+        product_id = AP_COMPASS_TYPE_HIL;
+	_setup_earth_field();
+}
+
+// setup _Bearth
+void AP_Compass_HIL::_setup_earth_field(void)
+{
+	// assume a earth field strength of 400
+        _Bearth(400, 0, 0);
+
+        // rotate _Bearth for inclination and declination. -66 degrees
+	// is the inclination in Canberra, Australia
+        Matrix3f R;
+	R.from_euler(0, ToRad(66), _declination.get());
+        _Bearth = R * _Bearth;
+}
+
 // Public Methods //////////////////////////////////////////////////////////////
 
 bool AP_Compass_HIL::read()
@@ -40,25 +60,40 @@ bool AP_Compass_HIL::read()
     return true;
 }
 
+#define MAG_OFS_X 5.0
+#define MAG_OFS_Y 13.0
+#define MAG_OFS_Z -18.0
+
 // Update raw magnetometer values from HIL data
 //
-void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
+void AP_Compass_HIL::setHIL(float roll, float pitch, float yaw)
 {
-    _hil_mag.x = _mag_x;
-    _hil_mag.y = _mag_y;
-    _hil_mag.z = _mag_z;
+	Matrix3f R;
 
-    // apply default board orientation for this compass type. This is
-    // a noop on most boards
-    _hil_mag.rotate(MAG_BOARD_ORIENTATION);
+        // create a rotation matrix for the given attitude
+        R.from_euler(roll, pitch, yaw);
 
-    // add user selectable orientation
-    _hil_mag.rotate((enum Rotation)_orientation.get());
+	if (_last_declination != _declination.get()) {
+		_setup_earth_field();
+		_last_declination = _declination.get();
+	}
 
-    // and add in AHRS_ORIENTATION setting
-    _hil_mag.rotate(_board_orientation);
+        // convert the earth frame magnetic vector to body frame, and
+        // apply the offsets
+        _hil_mag = R.mul_transpose(_Bearth);
+	_hil_mag -= Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
 
-    healthy = true;
+	// apply default board orientation for this compass type. This is
+	// a noop on most boards
+	_hil_mag.rotate(MAG_BOARD_ORIENTATION);
+
+	// add user selectable orientation
+	_hil_mag.rotate((enum Rotation)_orientation.get());
+
+	// and add in AHRS_ORIENTATION setting
+	_hil_mag.rotate(_board_orientation);
+
+	healthy = true;
 }
 
 void AP_Compass_HIL::accumulate(void)
diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h
index b12f42ebc1..53e3c0e848 100644
--- a/libraries/AP_Compass/AP_Compass_HIL.h
+++ b/libraries/AP_Compass/AP_Compass_HIL.h
@@ -7,14 +7,15 @@
 class AP_Compass_HIL : public Compass
 {
 public:
-    AP_Compass_HIL() : Compass() {
-        product_id = AP_COMPASS_TYPE_HIL;
-    }
+    AP_Compass_HIL();
     bool        read(void);
     void        accumulate(void);
-    void        setHIL(float Mag_X, float Mag_Y, float Mag_Z);
+    void        setHIL(float roll, float pitch, float yaw);
 private:
     Vector3f    _hil_mag;
+    Vector3f    _Bearth;
+    float		_last_declination;
+    void        _setup_earth_field();
 };
 
 #endif