From 6a328c5d3b711ffafb40beec6d38c5e1e25a3b1e Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Sun, 26 Jun 2022 09:17:46 +1000
Subject: [PATCH] AP_AHRS: Add accessor function for airspeed health monitoring

---
 libraries/AP_AHRS/AP_AHRS.cpp | 30 ++++++++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS.h   |  4 ++++
 2 files changed, 34 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index f96d4a82cb..cee0b56a10 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -971,6 +971,36 @@ bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const
     return false;
 }
 
+// return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed
+// returns false if the data is unavailable
+bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance, uint32_t &age_ms) const
+{
+    switch (active_EKF_type()) {
+    case EKFType::NONE:
+        break;
+#if HAL_NAVEKF2_AVAILABLE
+    case EKFType::TWO:
+        break;
+#endif
+
+#if HAL_NAVEKF3_AVAILABLE
+    case EKFType::THREE:
+        return EKF3.getAirSpdHealthData(innovation, innovationVariance, age_ms);
+#endif
+
+#if AP_AHRS_SIM_ENABLED
+    case EKFType::SIM:
+        break;
+#endif
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+        break;
+#endif
+    }
+    return false;
+}
+
 // return a synthetic airspeed estimate (one derived from sensors
 // other than an actual airspeed sensor), if available. return
 // true if we have a synthetic airspeed.  ret will not be modified
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 93f49fe019..d85749d26c 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -155,6 +155,10 @@ public:
     // returns false if estimate is unavailable
     bool airspeed_vector_true(Vector3f &vec) const;
 
+    // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed
+    // returns false if the data is unavailable
+    bool airspeed_health_data(float &innovation, float &innovationVariance, uint32_t &age_ms) const;
+
     // return true if airspeed comes from an airspeed sensor, as
     // opposed to an IMU estimate
     bool airspeed_sensor_enabled(void) const;