From 5f6d1d8d258ba4e193460de4ee0d4701997d5572 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Fri, 6 Nov 2015 12:46:04 +1100
Subject: [PATCH] AP_NavEKF2: Add fault score calculation

---
 libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 23 +++++++++++++++++++++
 libraries/AP_NavEKF2/AP_NavEKF2_core.h      |  4 ++++
 2 files changed, 27 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
index 805f9a3cce..cc3e400d66 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
@@ -41,6 +41,29 @@ bool NavEKF2_core::healthy(void) const
     return true;
 }
 
+// Return a consolidated fault score where higher numbers are less healthy
+// Intended to be used by the front-end to determine which is the primary EKF
+float NavEKF2_core::faultScore(void) const
+{
+    float score = 0.0f;
+    // If velocity, position or height measurements are failing consistency checks, this adds to the score
+    if (velTestRatio > 1.0f) {
+        score += velTestRatio-1.0f;
+    }
+    if (posTestRatio > 1.0f) {
+        score += posTestRatio-1.0f;
+    }
+    if (hgtTestRatio > 1.0f) {
+        score += hgtTestRatio-1.0f;
+    }
+    // If the tilt error is excessive this adds to the score
+    const float tiltErrThreshold = 0.1f;
+    if (tiltAlignComplete && yawAlignComplete && tiltErrFilt > tiltErrThreshold) {
+        score += tiltErrFilt / tiltErrThreshold;
+    }
+    return score;
+}
+
 // return data for debugging optical flow fusion
 void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const
 {
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index 6524fcdcbd..36ce1e14f7 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -80,6 +80,10 @@ public:
     // Check basic filter health metrics and return a consolidated health status
     bool healthy(void) const;
 
+    // Return a consolidated fault score where higher numbers are less healthy
+    // Intended to be used by the front-end to determine which is the primary EKF
+    float faultScore(void) const;
+
     // Return the last calculated NED position relative to the reference point (m).
     // If a calculated solution is not available, use the best available data and return false
     // If false returned, do not use for flight control