From 5a64cc9e8e6fdc32ef8e02a827af9b8d5e3fec88 Mon Sep 17 00:00:00 2001
From: Henry Wurzburg <hwurzburg@yahoo.com>
Date: Mon, 2 Oct 2023 20:43:49 -0500
Subject: [PATCH] AP_AHRS: add airspeed estimate status logging

---
 libraries/AP_AHRS/AP_AHRS.cpp | 24 ++++++++++++++++++++----
 libraries/AP_AHRS/AP_AHRS.h   | 15 ++++++++++++++-
 2 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 52a0c42e5a..0175b9d69d 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -330,7 +330,7 @@ void AP_AHRS::update_state(void)
     state.primary_core = _get_primary_core_index();
     state.wind_estimate_ok = _wind_estimate(state.wind_estimate);
     state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS();
-    state.airspeed_ok = _airspeed_estimate(state.airspeed);
+    state.airspeed_ok = _airspeed_estimate(state.airspeed, state.airspeed_estimate_type);
     state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true);
     state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec);
     state.quat_ok = _get_quaternion(state.quat);
@@ -829,7 +829,7 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const
 
 // return an airspeed estimate if available. return true
 // if we have an estimate
-bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
+bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const
 {
 #if AP_AIRSPEED_ENABLED && AP_GPS_ENABLED
     if (airspeed_sensor_enabled()) {
@@ -846,12 +846,13 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
                                             gnd_speed + _wind_max);
             airspeed_ret = true_airspeed / get_EAS2TAS();
         }
-
+        airspeed_estimate_type = AirspeedEstimateType::AIRSPEED_SENSOR;
         return true;
     }
 #endif
 
     if (!get_wind_estimation_enabled()) {
+        airspeed_estimate_type = AirspeedEstimateType::NO_NEW_ESTIMATE;
         return false;
     }
 
@@ -864,17 +865,20 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
     switch (active_EKF_type()) {
 #if AP_AHRS_DCM_ENABLED
     case EKFType::DCM:
+        airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
         return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
 #endif
 
 #if AP_AHRS_SIM_ENABLED
     case EKFType::SIM:
+        airspeed_estimate_type = AirspeedEstimateType::SIM;
         return sim.airspeed_estimate(airspeed_ret);
 #endif
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:
 #if AP_AHRS_DCM_ENABLED
+        airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
         return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
 #else
         return false;
@@ -889,7 +893,8 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
 
 #if HAL_EXTERNAL_AHRS_ENABLED
     case EKFType::EXTERNAL:
-        return false;
+        airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
+        return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
 #endif
     }
 
@@ -907,11 +912,13 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
             true_airspeed = MAX(0.0f, true_airspeed);
         }
         airspeed_ret = true_airspeed / get_EAS2TAS();
+        airspeed_estimate_type = AirspeedEstimateType::EKF3_SYNTHETIC;
         return true;
     }
 
 #if AP_AHRS_DCM_ENABLED
     // fallback to DCM
+    airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
     return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
 #endif
 
@@ -3389,6 +3396,15 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
     return state.airspeed_ok;
 }
 
+// return an airspeed estimate if available. return true
+// if we have an estimate
+bool AP_AHRS::airspeed_estimate(float &airspeed_ret, AP_AHRS::AirspeedEstimateType &type) const
+{
+    airspeed_ret = state.airspeed;
+    type = state.airspeed_estimate_type;
+    return state.airspeed_ok;
+}
+
 // return a true airspeed estimate (navigation airspeed) if
 // available. return true if we have an estimate
 bool AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 47089d125e..8ab6bca045 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -139,6 +139,18 @@ public:
     // if we have an estimate
     bool airspeed_estimate(float &airspeed_ret) const;
 
+    enum AirspeedEstimateType : uint8_t {
+        NO_NEW_ESTIMATE = 0,
+        AIRSPEED_SENSOR = 1,
+        DCM_SYNTHETIC = 2,
+        EKF3_SYNTHETIC = 3,
+        SIM = 4,
+    };
+
+    // return an airspeed estimate if available. return true
+    // if we have an estimate
+    bool airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &type) const;
+
     // return a true airspeed estimate (navigation airspeed) if
     // available. return true if we have an estimate
     bool airspeed_estimate_true(float &airspeed_ret) const;
@@ -833,7 +845,7 @@ private:
 
     // return an airspeed estimate if available. return true
     // if we have an estimate
-    bool _airspeed_estimate(float &airspeed_ret) const;
+    bool _airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &status) const;
 
     // return secondary attitude solution if available, as eulers in radians
     bool _get_secondary_attitude(Vector3f &eulers) const;
@@ -926,6 +938,7 @@ private:
         float EAS2TAS;
         bool airspeed_ok;
         float airspeed;
+        AirspeedEstimateType airspeed_estimate_type;
         bool airspeed_true_ok;
         float airspeed_true;
         Vector3f airspeed_vec;