From 1ff656df2d6b3e404bf8ea8536befa0649585efd Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Fri, 14 Apr 2017 16:37:36 -0400
Subject: [PATCH] Sub: Add arming check for ahrs health

---
 ArduSub/AP_Arming_Sub.cpp | 28 +++++++++++++++++++++++++++-
 ArduSub/AP_Arming_Sub.h   |  1 +
 2 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp
index af307f6596..592b9d774d 100644
--- a/ArduSub/AP_Arming_Sub.cpp
+++ b/ArduSub/AP_Arming_Sub.cpp
@@ -49,5 +49,31 @@ bool AP_Arming_Sub::pre_arm_checks(bool report)
         return true;
     }
 
-    return AP_Arming::pre_arm_checks(report) & rc_check(report);
+    return AP_Arming::pre_arm_checks(report) & rc_check(report) & ins_checks(report);
+}
+
+bool AP_Arming_Sub::ins_checks(bool report)
+{
+    // call parent class checks
+    if (!AP_Arming::ins_checks(report)) {
+        return false;
+    }
+
+    // additional plane specific checks
+    if ((checks_to_perform & ARMING_CHECK_ALL) ||
+        (checks_to_perform & ARMING_CHECK_INS)) {
+        if (!ahrs.healthy()) {
+            if (report) {
+                const char *reason = ahrs.prearm_failure_reason();
+                if (reason) {
+                    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason);
+                } else {
+                    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: AHRS not healthy");
+                }
+            }
+            return false;
+        }
+    }
+
+    return true;
 }
diff --git a/ArduSub/AP_Arming_Sub.h b/ArduSub/AP_Arming_Sub.h
index 47a20078c1..cf96f34ae2 100644
--- a/ArduSub/AP_Arming_Sub.h
+++ b/ArduSub/AP_Arming_Sub.h
@@ -14,5 +14,6 @@ public:
     bool pre_arm_checks(bool report) override;
 
 protected:
+    bool ins_checks(bool report) override;
     enum HomeState home_status() const override;
 };