From a5a33579285b426c3b474092b73f5ff91a93ef98 Mon Sep 17 00:00:00 2001
From: Anthony Kenga <anton.k@airdog.com>
Date: Wed, 25 Nov 2015 21:11:21 +0200
Subject: [PATCH] Attitude estimator Q: fix magnetic declination inducing gyro
 bias growth

---
 src/lib/mathlib/math/Quaternion.hpp           | 10 +++++++
 .../attitude_estimator_q_main.cpp             | 27 +++++++++++++++++--
 2 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index b7cb068dd4..4f5ccc623b 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -211,6 +211,16 @@ public:
 		data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
 	}
 
+	/**
+	 * simplified version of the above method to create quaternion representing rotation only by yaw
+	 */
+	void from_yaw(float yaw) {
+		data[0] = cosf(yaw / 2.0f);
+		data[1] = 0.0f;
+		data[2] = 0.0f;
+		data[3] = sinf(yaw / 2.0f);
+	}
+
 	/**
 	 * create Euler angles vector from the quaternion
 	 */
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 2a61e18046..f456e73120 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -202,6 +202,9 @@ private:
 	bool init();
 
 	bool update(float dt);
+
+	// Update magnetic declination (in rads) immediately changing yaw rotation
+	void update_mag_declination(float new_declination);
 };
 
 
@@ -522,7 +525,7 @@ void AttitudeEstimatorQ::task_main()
 
 			if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
 				/* set magnetic declination automatically */
-				_mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon));
+				update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon)));
 			}
 		}
 
@@ -650,7 +653,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 		param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
 		float mag_decl_deg = 0.0f;
 		param_get(_params_handles.mag_decl, &mag_decl_deg);
-		_mag_decl = math::radians(mag_decl_deg);
+		update_mag_declination(math::radians(mag_decl_deg));
 		int32_t mag_decl_auto_int;
 		param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
 		_mag_decl_auto = mag_decl_auto_int != 0;
@@ -685,6 +688,12 @@ bool AttitudeEstimatorQ::init()
 
 	// Convert to quaternion
 	_q.from_dcm(R);
+
+	// Compensate for magnetic declination
+	Quaternion decl_rotation;
+	decl_rotation.from_yaw(_mag_decl);
+	_q = decl_rotation * _q;
+
 	_q.normalize();
 
 	if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
@@ -788,6 +797,20 @@ bool AttitudeEstimatorQ::update(float dt)
 	return true;
 }
 
+void AttitudeEstimatorQ::update_mag_declination(float new_declination)
+{
+	// Apply initial declination or trivial rotations without changing estimation
+	if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
+		_mag_decl = new_declination;
+
+	} else {
+		// Immediately rotate current estimation to avoid gyro bias growth
+		Quaternion decl_rotation;
+		decl_rotation.from_yaw(new_declination - _mag_decl);
+		_q = decl_rotation * _q;
+		_mag_decl = new_declination;
+	}
+}
 
 int attitude_estimator_q_main(int argc, char *argv[])
 {