From 966d66ef40eb10f9cc8e9e77649d9dfe9d3d1335 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Tue, 15 Apr 2014 13:21:12 -0700
Subject: [PATCH] AP_AHRS_NavEKF: use gyro drift states from EKF in get_gyro

---
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 24 +++++++++++++++++++++++-
 1 file changed, 23 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index ffb263e8e3..ea3dff0dbb 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -28,7 +28,29 @@ extern const AP_HAL::HAL& hal;
 // return the smoothed gyro vector corrected for drift
 const Vector3f AP_AHRS_NavEKF::get_gyro(void) const
 {
-    return AP_AHRS_DCM::get_gyro();
+    if (!using_EKF()) {
+        return AP_AHRS_DCM::get_gyro();
+    }
+
+    Vector3f angRate;
+
+    // average the available gyro sensors
+    angRate.zero();
+    uint8_t gyro_count = 0;
+    for (uint8_t i = 0; i<_ins.get_gyro_count(); i++) {
+        if (_ins.get_gyro_health(i)) {
+            angRate += _ins.get_gyro(i);
+            gyro_count++;
+        }
+    }
+    if (gyro_count != 0) {
+        angRate /= gyro_count;
+    }
+
+    Vector3f gyrobias;
+    EKF.getGyroBias(gyrobias);
+
+    return angRate+gyrobias;
 }
 
 const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const