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>
ArduPilot · Jan 9, 2025 · 2438f29 · 2438f29
1 parent 44fdd0b
commit 2438f29
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 0 deletions.
diff --git 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
@@ -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