From 11c9e46ec7b0c7815bc822e92e3d680644174dfc Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 29 Jul 2015 12:19:18 +0900
Subject: [PATCH] Copter: arming check that accels and gyro are healty

---
 ArduCopter/motors.cpp | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp
index ba09a3d30e..d2db9dee0e 100644
--- a/ArduCopter/motors.cpp
+++ b/ArduCopter/motors.cpp
@@ -682,6 +682,22 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
     start_logging();
 #endif
 
+    // check accels and gyro are healthy
+    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
+        if(!ins.get_accel_health_all()) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Accelerometers not healthy"));
+            }
+            return false;
+        }
+        if(!ins.get_gyro_health_all()) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyros not healthy"));
+            }
+            return false;
+        }
+    }
+
     // always check if inertial nav has started and is ready
     if(!ahrs.healthy()) {
         if (display_failure) {