From 334db0a1a532b1c9e0bab693bb640cc15c77bbae Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 3 Sep 2016 17:54:36 +1000
Subject: [PATCH] AP_AHRS: added get_primary_accel_index() and
 get_primary_gyro_index()

---
 libraries/AP_AHRS/AP_AHRS.h          | 10 ++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 29 ++++++++++++++++++++++++++--
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  9 +++++++++
 3 files changed, 46 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 2cdb26dbe7..d7a3fed232 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -162,6 +162,16 @@ public:
         return _baro;
     }
 
+    // get the index of the current primary accelerometer sensor
+    virtual uint8_t get_primary_accel_index(void) const {
+        return _ins.get_primary_accel();
+    }
+
+    // get the index of the current primary gyro sensor
+    virtual uint8_t get_primary_gyro_index(void) const {
+        return _ins.get_primary_gyro();
+    }
+    
     // accelerometer values in the earth frame in m/s/s
     virtual const Vector3f &get_accel_ef(uint8_t i) const {
         return _accel_ef[i];
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 4604d0d915..5c100c0b66 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -1396,7 +1396,7 @@ bool AP_AHRS_NavEKF::have_ekf_logging(void) const
 }
 
 // get earth-frame accel vector for primary IMU
-const Vector3f &AP_AHRS_NavEKF::get_accel_ef() const
+uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const
 {
     int8_t imu = -1;
     switch (ekf_type()) {
@@ -1410,7 +1410,32 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef() const
     if (imu == -1) {
         imu = _ins.get_primary_accel();
     }
-    return get_accel_ef(imu);
+    return imu;
+}
+
+// get earth-frame accel vector for primary IMU
+const Vector3f &AP_AHRS_NavEKF::get_accel_ef() const
+{
+    return get_accel_ef(get_primary_accel_index());
+}
+
+
+// get the index of the current primary accelerometer sensor
+uint8_t AP_AHRS_NavEKF::get_primary_accel_index(void) const
+{
+    if (ekf_type() == 2) {
+        return get_primary_IMU_index();
+    }
+    return _ins.get_primary_accel();
+}
+
+// get the index of the current primary gyro sensor
+uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const
+{
+    if (ekf_type() == 2) {
+        return get_primary_IMU_index();
+    }
+    return _ins.get_primary_gyro();
 }
 
 #endif // AP_AHRS_NAVEKF_AVAILABLE
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 5d9b79812b..1e5c05b22e 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -232,6 +232,12 @@ public:
 
     // is the EKF backend doing its own sensor logging?
     bool have_ekf_logging(void) const override;
+
+    // get the index of the current primary accelerometer sensor
+    uint8_t get_primary_accel_index(void) const override;
+
+    // get the index of the current primary gyro sensor
+    uint8_t get_primary_gyro_index(void) const override;
     
 private:
     enum EKF_TYPE {EKF_TYPE_NONE=0,
@@ -269,6 +275,9 @@ private:
     void update_EKF1(void);
     void update_EKF2(void);
 
+    // get the index of the current primary IMU
+    uint8_t get_primary_IMU_index(void) const;
+    
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     SITL::SITL *_sitl;
     void update_SITL(void);