From 2438f292358a4c2ac209a74f79eb910c7c7ee60c Mon Sep 17 00:00:00 2001
From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
Date: Thu, 9 Jan 2025 02:18:59 -0700
Subject: [PATCH] AP_Soaring: Add named value float publisher

* This sends the EKF estimate of the thermal to mavproxy_soar
* Useful until we have proper mavlink packets for soaring state

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
---
 libraries/AP_Soaring/AP_Soaring.cpp      | 6 ++++++
 libraries/AP_Soaring/AP_Soaring_config.h | 6 ++++++
 2 files changed, 12 insertions(+)

diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp
index 9fd24b35094c0..a510ff1ec0fa3 100644
--- a/libraries/AP_Soaring/AP_Soaring.cpp
+++ b/libraries/AP_Soaring/AP_Soaring.cpp
@@ -369,6 +369,12 @@ void SoaringController::update_thermalling()
                                            (double)wind_drift.x,
                                            (double)wind_drift.y,
                                            (double)_thermalability);
+#if HAL_SOARING_NVF_EKF_ENABLED
+    gcs().send_named_float("SOAREKFX0", (float)_ekf.X[0]);
+    gcs().send_named_float("SOAREKFX1", (float)_ekf.X[1]);
+    gcs().send_named_float("SOAREKFX2", (float)_ekf.X[2]);
+    gcs().send_named_float("SOAREKFX3", (float)_ekf.X[3]);
+#endif // HAL_SOARING_NVF_EKF_ENABLED
 #endif
 }
 
diff --git a/libraries/AP_Soaring/AP_Soaring_config.h b/libraries/AP_Soaring/AP_Soaring_config.h
index 01c2d81c8e67c..808e61f9e7a9c 100644
--- a/libraries/AP_Soaring/AP_Soaring_config.h
+++ b/libraries/AP_Soaring/AP_Soaring_config.h
@@ -3,3 +3,9 @@
 #ifndef HAL_SOARING_ENABLED
 #define HAL_SOARING_ENABLED 1
 #endif
+
+// Whether to publish named-value-float of the kalman filter thermal estimator.
+// This is used with the mavproxy_soar plugin.
+#ifndef HAL_SOARING_NVF_EKF_ENABLED
+#define HAL_SOARING_NVF_EKF_ENABLED 0
+#endif