From d3aca5544e91d35f525701b5fcc1942d2533b19c Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 2 Mar 2017 20:33:13 +0900
Subject: [PATCH] AP_AHRS: get_gyro_latest returns latest rates from IMU with
 drift correction

Pulling the gyro values from the IMU allows our rate controllers to run before the EKF
---
 libraries/AP_AHRS/AP_AHRS.cpp        | 7 +++++++
 libraries/AP_AHRS/AP_AHRS.h          | 3 +++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 2 +-
 libraries/AP_AHRS/AP_AHRS_View.cpp   | 7 +++++++
 libraries/AP_AHRS/AP_AHRS_View.h     | 3 +++
 5 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 33c3ec348f..9bf6669e63 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -131,6 +131,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
     AP_GROUPEND
 };
 
+// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
+Vector3f AP_AHRS::get_gyro_latest(void) const
+{
+    uint8_t primary_gyro = get_primary_gyro_index();
+    return get_ins().get_gyro(primary_gyro) + get_gyro_drift();
+}
+
 // return airspeed estimate if available
 bool AP_AHRS::airspeed_estimate(float *airspeed_ret) const
 {
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index b6ed49521e..e4fc4df531 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -235,6 +235,9 @@ public:
     // return a smoothed and corrected gyro vector
     virtual const Vector3f &get_gyro(void) const = 0;
 
+    // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
+    Vector3f get_gyro_latest(void) const;
+
     // 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_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 60150d03cb..8f0e589106 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -1471,7 +1471,7 @@ bool AP_AHRS_NavEKF::have_ekf_logging(void) const
     return false;
 }
 
-// get earth-frame accel vector for primary IMU
+// get the index of the current primary IMU
 uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const
 {
     int8_t imu = -1;
diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp
index fd9d8fc4a7..163d4fb7b7 100644
--- a/libraries/AP_AHRS/AP_AHRS_View.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_View.cpp
@@ -67,3 +67,10 @@ void AP_AHRS_View::update(void)
                    trig.cos_roll, trig.cos_pitch, trig.cos_yaw,
                    trig.sin_roll, trig.sin_pitch, trig.sin_yaw);
 }
+
+// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
+Vector3f AP_AHRS_View::get_gyro_latest(void) const {
+    Vector3f gyro_latest = ahrs.get_gyro_latest();
+    gyro_latest.rotate(rotation);
+    return gyro_latest;
+}
diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h
index 185089eec4..42e077e863 100644
--- a/libraries/AP_AHRS/AP_AHRS_View.h
+++ b/libraries/AP_AHRS/AP_AHRS_View.h
@@ -39,6 +39,9 @@ public:
         return gyro;
     }
 
+    // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
+    Vector3f get_gyro_latest(void) const;
+
     // return a DCM rotation matrix representing our current
     // attitude in this view
     const Matrix3f &get_rotation_body_to_ned(void) const {