From 9f21649dc06a905f863a2a0e04a8b84c94a6ff76 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 9 Mar 2017 09:47:57 +0900
Subject: [PATCH] AP_Arming: add gps consistency and blend health check

---
 libraries/AP_Arming/AP_Arming.cpp | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
index 7095177d8c..8d86071e16 100644
--- a/libraries/AP_Arming/AP_Arming.cpp
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -369,6 +369,26 @@ bool AP_Arming::gps_checks(bool report)
             return false;
         }
     }
+
+    // check GPSs are within 50m of each other and that blending is healthy
+    if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_GPS_CONFIG)) {
+        float distance_m;
+        if (!gps.all_consistent(distance_m)) {
+            if (report) {
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,
+                                                 "PreArm: GPSs inconsistent by %4.1fm",
+                                                  (double)distance_m);
+            }
+            return false;
+        }
+        if (!gps.blend_healthy()) {
+            if (report) {
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: GPS blending unhealthy");
+            }
+            return false;
+        }
+    }
+
     return true;
 }