From a7f03619f293f283ef5bd2034ad374a8f61db97f Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 17 Jan 2014 17:21:42 +0900
Subject: [PATCH] Copter: add inav vs baro arming check

In-flight Barometer sanity checking will be a more complete solution but
until then this should catch some bad pre-flight barometer behaviour
---
 ArduCopter/motors.pde | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde
index 4632b8c40..e55b16b1c 100644
--- a/ArduCopter/motors.pde
+++ b/ArduCopter/motors.pde
@@ -241,6 +241,13 @@ static void pre_arm_checks(bool display_failure)
             }
             return;
         }
+        // check Baro & inav alt are within 1m
+        if(fabs(inertial_nav.get_altitude() - baro_alt) > 100) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Alt disparity"));
+            }
+            return;
+        }
     }
 
     // check Compass
@@ -436,6 +443,16 @@ static bool arm_checks(bool display_failure)
         return true;
     }
 
+    // check Baro & inav alt are within 1m
+    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
+        if(fabs(inertial_nav.get_altitude() - baro_alt) > 100) {
+            if (display_failure) {
+                gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Alt disparity"));
+            }
+            return false;
+        }
+    }
+
     // check gps is ok if required - note this same check is also done in pre-arm checks
     if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_GPS)) {
         if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
-- 
GitLab