AP_AHRS: added get_primary_accel_index() and get_primary_gyro_index()

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];

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

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -233,6 +233,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,
 #if AP_AHRS_WITH_EKF1
@@ -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);