Sub: Add arming check for ahrs health

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;
 }

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;
 };