From 77f91e62500a822f4db9bf68d649a7720c0cc5ab Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 2 Mar 2014 13:38:35 +1100
Subject: [PATCH] AP_NavEKF: don't assume the number of gyros == number of
 accels

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 6e7c0e7d30..6b3f1e93fd 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -2559,12 +2559,24 @@ void NavEKF::readIMUData()
     if (_ahrs->get_ins().get_accel_health(0) && _ahrs->get_ins().get_accel_health(1)) {
         accel1 = _ahrs->get_ins().get_accel(0);
         accel2 = _ahrs->get_ins().get_accel(1);
-        angRate = (_ahrs->get_ins().get_gyro(0) + _ahrs->get_ins().get_gyro(1)) * 0.5f;
     } else {
         accel1 = _ahrs->get_ins().get_accel();
         accel2 = accel1;
-        angRate     = _ahrs->get_ins().get_gyro();
     }
+
+    // average the available gyro sensors
+    angRate.zero();
+    uint8_t gyro_count = 0;
+    for (uint8_t i = 0; i<_ahrs->get_ins().get_gyro_count(); i++) {
+        if (_ahrs->get_ins().get_gyro_health(i)) {
+            angRate += _ahrs->get_ins().get_gyro(i);
+            gyro_count++;
+        }
+    }
+    if (gyro_count != 0) {
+        angRate /= gyro_count;
+    }
+
     // trapezoidal integration
     dAngIMU     = (angRate + lastAngRate) * dtIMU * 0.5f;
     lastAngRate = angRate;