AP_NavEKF2: added getPrimaryCoreIMUIndex()

needed for correct AHRS gyro estimate

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -682,6 +682,16 @@ int8_t NavEKF2::getPrimaryCoreIndex(void) const
     return primary;
 }
 
+// returns the index of the IMU of the primary core
+// return -1 if no primary core selected
+int8_t NavEKF2::getPrimaryCoreIMUIndex(void) const
+{
+    if (!core) {
+        return -1;
+    }
+    return core[primary].getIMUIndex();
+}
+
 // Write the last calculated NE position relative to the reference point (m).
 // If a calculated solution is not available, use the best available data and return false
 // If false returned, do not use for flight control

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -62,6 +62,10 @@ public:
     // return -1 if no primary core selected
     int8_t getPrimaryCoreIndex(void) const;
 
+    // returns the index of the IMU of the primary core
+    // return -1 if no primary core selected
+    int8_t getPrimaryCoreIMUIndex(void) const;
+    
     // Write the last calculated NE position relative to the reference point (m) for the specified instance.
     // An out of range instance (eg -1) returns data for the the primary instance
     // If a calculated solution is not available, use the best available data and return false

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -276,6 +276,9 @@ public:
     // publish output observer angular, velocity and position tracking error
     void getOutputTrackingError(Vector3f &error) const;
 
+    // get the IMU index
+    uint8_t getIMUIndex(void) const { return imu_index; }
+    
 private:
     // Reference to the global EKF frontend for parameters
     NavEKF2 *frontend;