AP_Soaring: Rate limit the NVF publishers to 4Hz

Signed-off-by: Ryan Friedman <[email protected]>
ArduPilot · Jan 28, 2025 · 3a5a2f6 · 3a5a2f6
1 parent 864491d
commit 3a5a2f6
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 4 deletions.
diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp
@@ -8,6 +8,10 @@
 #include <GCS_MAVLink/GCS.h>
 #include <stdint.h>
 
+#if HAL_SOARING_NVF_EKF_ENABLED
+static constexpr uint32_t NVF_PUBLISHER_DELAY_MS = 250;
+#endif // HAL_SOARING_NVF_EKF_ENABLED
+
 // ArduSoar parameters
 const AP_Param::GroupInfo SoaringController::var_info[] = {
     // @Param: ENABLE
@@ -370,10 +374,15 @@ 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]);
+    auto const now_ms = AP_HAL::millis();
+    if (now_ms - _prev_nvf_pub_time_ms > 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_ms = now_ms;
+    }
+
 #endif // HAL_SOARING_NVF_EKF_ENABLED
 #endif
 }

diff --git 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
+    uint32_t _prev_nvf_pub_time_ms;
+#endif // #if HAL_SOARING_NVF_EKF_ENABLED
+
     bool _throttle_suppressed;
 
     float McCready(float alt);