From c62fc336cb56d82ce94f46496eca688cf31770ed Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 3 Sep 2016 17:12:39 +1000
Subject: [PATCH] AP_AHRS: use current EKF2 IMU core in gyro estimate

The EKF2 implementation uses one IMU per EKF2 core. When reporting the
gyro estimate, accel_ef estimate and gyro bias estimate we need to use
the values associated with the current IMU index being used by the
current EKF2 core. Otherwise we will have an inconsistency between the
gyro estimate and attitude estimate

This affects all multi-IMU systems using EKF2
---
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 46 +++++++++++++++++++---------
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   | 12 +++-----
 2 files changed, 36 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index b78d70e174..4604d0d915 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -217,39 +217,37 @@ void AP_AHRS_NavEKF::update_EKF2(void)
             update_trig();
 
             // keep _gyro_bias for get_gyro_drift()
+            _gyro_bias.zero();            
             EKF2.getGyroBias(-1,_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) && healthy_count < 2 && _ins.use_gyro(i)) {
-                    _gyro_estimate += _ins.get_gyro(i);
-                    healthy_count++;
-                }
-            }
-            if (healthy_count > 1) {
-                _gyro_estimate /= healthy_count;
+
+            // the gyro bias applies only to the IMU associated with the primary EKF2
+            // core
+            int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
+            if (primary_imu == -1) {
+                _gyro_estimate = _ins.get_gyro();
+            } else {
+                _gyro_estimate = _ins.get_gyro(primary_imu);
             }
             _gyro_estimate += _gyro_bias;
 
-            float abias;
+            float abias = 0;
             EKF2.getAccelZBias(-1,abias);
 
-            // This EKF uses the primary IMU
-            // Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream
-            // update _accel_ef_ekf
+            // This EKF is currently using primary_imu, and abias applies to only that IMU
             for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
                 Vector3f accel = _ins.get_accel(i);
-                if (i==_ins.get_primary_accel()) {
+                if (i == primary_imu) {
                     accel.z -= abias;
                 }
                 if (_ins.get_accel_health(i)) {
                     _accel_ef_ekf[i] = _dcm_matrix * accel;
                 }
             }
-            _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()];
+            _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
         }
     }
 }
@@ -1397,5 +1395,23 @@ bool AP_AHRS_NavEKF::have_ekf_logging(void) const
     return false;
 }
 
+// get earth-frame accel vector for primary IMU
+const Vector3f &AP_AHRS_NavEKF::get_accel_ef() const
+{
+    int8_t imu = -1;
+    switch (ekf_type()) {
+    case 2:
+        // let EKF2 choose primary IMU
+        imu = EKF2.getPrimaryCoreIMUIndex();
+        break;
+    default:
+        break;
+    }
+    if (imu == -1) {
+        imu = _ins.get_primary_accel();
+    }
+    return get_accel_ef(imu);
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE
 
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 439873850f..5d9b79812b 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -63,11 +63,11 @@ public:
                    NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags = FLAG_NONE);
 
     // return the smoothed gyro vector corrected for drift
-    const Vector3f &get_gyro(void) const;
-    const Matrix3f &get_rotation_body_to_ned(void) const;
+    const Vector3f &get_gyro(void) const override;
+    const Matrix3f &get_rotation_body_to_ned(void) const override;
 
     // return the current drift correction integrator value
-    const Vector3f &get_gyro_drift(void) const;
+    const Vector3f &get_gyro_drift(void) const override;
 
     // reset the current gyro drift estimate
     //  should be called if gyro offsets are recalculated
@@ -123,10 +123,8 @@ public:
     // EKF has a better ground speed vector estimate
     Vector2f groundspeed_vector(void);
 
-    const Vector3f &get_accel_ef(uint8_t i) const;
-    const Vector3f &get_accel_ef() const {
-        return get_accel_ef(_ins.get_primary_accel());
-    };
+    const Vector3f &get_accel_ef(uint8_t i) const override;
+    const Vector3f &get_accel_ef() const override;
 
     // blended accelerometer values in the earth frame in m/s/s
     const Vector3f &get_accel_ef_blended(void) const;