diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index a510ff1ec0fa3..a011ab01edf7b 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -8,6 +8,10 @@ #include #include +#if HAL_SOARING_NVF_EKF_ENABLED +static constexpr float NVF_PUBLISHER_DELAY_MS = 250.0; +#endif // HAL_SOARING_NVF_EKF_ENABLED + // ArduSoar parameters const AP_Param::GroupInfo SoaringController::var_info[] = { // @Param: ENABLE @@ -370,10 +374,14 @@ void SoaringController::update_thermalling() (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]); + if (_prev_update_time - _prev_nvf_pub_time > NVF_PUBLISHER_DELAY_MS) { + 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]); + _prev_nvf_pub_time = _prev_update_time; + } + #endif // HAL_SOARING_NVF_EKF_ENABLED #endif } diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index c43e72559069d..e4989c21acdfb 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -48,6 +48,11 @@ class SoaringController { // store time of last update uint64_t _prev_update_time; + // store time of last NVT publish +#if HAL_SOARING_NVF_EKF_ENABLED +uint64_t _prev_nvf_pub_time; +#endif // #if HAL_SOARING_NVF_EKF_ENABLED + bool _throttle_suppressed; float McCready(float alt);