From 28fedba4d8477ad935cc99b05de49e13df6945b6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 13 Jul 2014 21:56:39 +1000
Subject: [PATCH] AP_AHRS: fixed gyro_bias sign, and pre-calculate
 gyro_estimate for EKF

this allows us to return a constant vector for the corrected gyro
estimate. Based on discussions with Jon Challinger
---
 libraries/AP_AHRS/AP_AHRS.h          |  2 +-
 libraries/AP_AHRS/AP_AHRS_DCM.h      |  2 +-
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 46 +++++++++++++++-------------
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  4 ++-
 4 files changed, 29 insertions(+), 25 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 39e7309566..ba62448af3 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -163,7 +163,7 @@ public:
     int32_t yaw_sensor;
 
     // return a smoothed and corrected gyro vector
-    virtual const Vector3f get_gyro(void) const = 0;
+    virtual const Vector3f &get_gyro(void) const = 0;
 
     // return the current estimate of the gyro drift
     virtual const Vector3f &get_gyro_drift(void) const = 0;
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 6693fecd53..890d0abe8d 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -39,7 +39,7 @@ public:
     }
 
     // return the smoothed gyro vector corrected for drift
-    const Vector3f get_gyro(void) const {
+    const Vector3f &get_gyro(void) const {
         return _omega;
     }
 
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index ea3dff0dbb..2098f7b04e 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -26,31 +26,12 @@
 extern const AP_HAL::HAL& hal;
 
 // return the smoothed gyro vector corrected for drift
-const Vector3f AP_AHRS_NavEKF::get_gyro(void) const
+const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const
 {
     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;
+    return _gyro_estimate;
 }
 
 const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const
@@ -63,7 +44,10 @@ const Matrix3f &AP_AHRS_NavEKF::get_dcm_matrix(void) const
 
 const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
 {
-    return AP_AHRS_DCM::get_gyro_drift();
+    if (!using_EKF()) {
+        return AP_AHRS_DCM::get_gyro_drift();
+    }
+    return _gyro_bias;
 }
 
 void AP_AHRS_NavEKF::update(void)
@@ -100,6 +84,24 @@ void AP_AHRS_NavEKF::update(void)
             if (yaw_sensor < 0)
                 yaw_sensor += 36000;
             update_trig();
+
+            // keep _gyro_bias for get_gyro_drift()
+            EKF.getGyroBias(_gyro_bias);
+            _gyro_bias = -_gyro_bias;
+
+            // calculate corrected gryo estimate for get_gyro()
+            _gyro_estimate.zero();
+            uint8_t healthy_count = 0;    
+            for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
+                if (_ins.get_gyro_health(i)) {
+                    _gyro_estimate += _ins.get_gyro(i);
+                    healthy_count++;
+                }
+            }
+            if (healthy_count > 1) {
+                _gyro_estimate /= healthy_count;
+            }
+            _gyro_estimate += _gyro_bias;
         }
     }
 }
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 52426fba61..563535bcae 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -42,7 +42,7 @@ public:
         }
 
     // return the smoothed gyro vector corrected for drift
-    const Vector3f get_gyro(void) const;
+    const Vector3f &get_gyro(void) const;
     const Matrix3f &get_dcm_matrix(void) const;
 
     // return the current drift correction integrator value
@@ -102,6 +102,8 @@ private:
     bool ekf_started;
     Matrix3f _dcm_matrix;
     Vector3f _dcm_attitude;
+    Vector3f _gyro_bias;
+    Vector3f _gyro_estimate;
     const uint16_t startup_delay_ms;
     uint32_t start_time_ms;
 };