From 641c8317a58d07e687ad74c2c698c03ad4024c01 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 3 Sep 2014 22:00:09 +0900
Subject: [PATCH] Copter: pre-arm consistency check of gyros

---
 ArduCopter/config.h   |  5 +++++
 ArduCopter/motors.pde | 16 ++++++++++++++++
 2 files changed, 21 insertions(+)

diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 1f60c1635d..f4a8397f56 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -304,6 +304,11 @@
   #define PREARM_MAX_ACCEL_VECTOR_DIFF  1.0f    // pre arm accel check will fail if primary and backup accelerometer vectors differ by 1m/s/s
 #endif
 
+// arming check's maximum acceptable rotation rate difference (in rad/sec) between primary and backup gyros
+#ifndef PREARM_MAX_GYRO_VECTOR_DIFF
+  #define PREARM_MAX_GYRO_VECTOR_DIFF   0.35f   // pre arm gyro check will fail if primary and backup gyro vectors differ by 0.35 rad/sec (=20deg/sec)
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 //  EKF Checker
 #ifndef EKFCHECK_THRESHOLD_DEFAULT
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index f6c63442f7..a9ed222289 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -374,6 +374,22 @@ static void pre_arm_checks(bool display_failure)
             }
             return;
         }
+
+#if INS_MAX_INSTANCES > 1
+        // check all gyros are consistent
+        if (ins.get_gyro_count() > 1) {
+            for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
+                // get rotation rate difference between gyro #i and primary gyro
+                Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro();
+                if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) {
+                    if (display_failure) {
+                        gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Gyros inconsistent"));
+                    }
+                    return;
+                }
+            }
+        }
+#endif
     }
 #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
 #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1