From 59404c25e327223aa8410aa6eeb335fbf1aa7eb4 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 3 Sep 2014 13:50:41 +0900
Subject: [PATCH] Copter: pre-arm consistency check of accels

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

diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 4129372df7..e6b26aff18 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -299,6 +299,11 @@
  # define PREARM_MAX_VELOCITY_CMS           50.0f   // vehicle must be travelling under 50cm/s before arming
 #endif
 
+// arming check's maximum acceptable accelerometer vector difference (in m/s/s) between primary and backup accelerometers
+#ifndef PREARM_MAX_ACCEL_VECTOR_DIFF
+  #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
+
 //////////////////////////////////////////////////////////////////////////////
 //  EKF Checker
 #ifndef EKFCHECK_THRESHOLD_DEFAULT
diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 591b8ceb13..f0feadafdf 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -346,6 +346,24 @@ static void pre_arm_checks(bool display_failure)
             return;
         }
 
+#if INS_MAX_INSTANCES > 1
+        // check all accelerometers point in roughly same direction
+        if (ins.get_accel_count() > 1) {
+            const Vector3f &prime_accel_vec = ins.get_accel();
+            for(uint8_t i=0; i<ins.get_accel_count(); i++) {
+                // get next accel vector
+                const Vector3f &accel_vec = ins.get_accel(i);
+                Vector3f vec_diff = accel_vec - prime_accel_vec;
+                if (vec_diff.length() > PREARM_MAX_ACCEL_VECTOR_DIFF) {
+                    if (display_failure) {
+                        gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Accels inconsistent"));
+                    }
+                    return;
+                }
+            }
+        }
+#endif
+
         // check gyros are healthy
         if(!ins.get_gyro_health_all()) {
             if (display_failure) {