From 9d7ed30023a12a75202937fc361e08d261f8e3b5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 5 Jul 2012 08:11:37 +1000
Subject: [PATCH] AHRS: only use GPS for yaw when compass is not being used

this avoids having two competing controllers
---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 8634266840..e9ef74b74e 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -461,7 +461,15 @@ AP_AHRS_DCM::drift_correction(float deltat)
 
     // step 6
     error = GA_b % GA_e2;
-    error.z = earth_error_Z;
+
+    // only use the gps/accelerometers for earth frame yaw correction 
+    // if we are not using a compass. Otherwise we have two competing
+    // controllers for yaw correction
+    if (_compass && _compass->use_for_yaw()) {
+	    error.z = 0;
+    } else {
+	    error.z = earth_error_Z;
+    }
 
     // convert the error term to body frame
     error = _dcm_matrix.mul_transpose(error);