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