From d79302c366fadcfa0ba6a91804edf4a7632deea9 Mon Sep 17 00:00:00 2001 From: Stefano Colli <45536733+StefanoColli@users.noreply.github.com> Date: Tue, 8 Jul 2025 13:48:23 +0200 Subject: [PATCH] mavlink_parameters: throttle MAVLink param transmission on low-bandwidth links (#25126) * mavlink_parameters: throttle MAVLink param transmission if in low-bandwidth mode * mavlink_parameters: simplify param transmission throttling --------- Co-authored-by: Matthias Grob --- src/modules/mavlink/mavlink_parameters.cpp | 9 +++++++++ src/modules/mavlink/mavlink_parameters.h | 1 + 2 files changed, 10 insertions(+) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 2d8c47da82..ab341d0186 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -408,6 +408,14 @@ MavlinkParametersManager::send_untransmitted() bool MavlinkParametersManager::send_one() { + const hrt_abstime now = hrt_absolute_time(); + + // If in low-bandwidth mode, throttle parameter transmission to 8 Hz + if (_mavlink.get_mode() == Mavlink::MAVLINK_MODE_LOW_BANDWIDTH + && now < _last_param_sent_timestamp + 125_ms) { + return false; + } + if (_send_all_index >= 0) { /* send all parameters if requested, but only after the system has booted */ @@ -445,6 +453,7 @@ MavlinkParametersManager::send_one() if (p != PARAM_INVALID) { send_param(p); + _last_param_sent_timestamp = now; } if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index c1042d0e71..6ad09889c2 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -162,4 +162,5 @@ protected: Mavlink &_mavlink; bool _first_send{false}; + hrt_abstime _last_param_sent_timestamp{0}; // time at which the last parameter was sent };