From bb3fd295ea9d900cc05136d4ea8203893e77c9ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 22 Jan 2025 16:32:52 +0100 Subject: [PATCH] mavlink: reduce rate multiplier to 0.25 while sending parameters Co-authored-by: alexcekay --- src/modules/mavlink/mavlink_main.cpp | 5 +++++ src/modules/mavlink/mavlink_main.h | 4 ++++ src/modules/mavlink/mavlink_parameters.cpp | 2 ++ src/modules/mavlink/mavlink_parameters.h | 13 +++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 1 + 5 files changed, 25 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2cb3333641..fe3703e5d6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1318,6 +1318,11 @@ Mavlink::update_rate_mult() /* scale up and down as the link permits */ float bandwidth_mult = (float)(_datarate * mavlink_ulog_streaming_rate_inv - const_rate) / rate; + /* Reduce rate while sending parameters in low bandwidth mode */ + if (sending_parameters() && _mode == Mavlink::MAVLINK_MODE_LOW_BANDWIDTH) { + bandwidth_mult = fminf(bandwidth_mult, 0.25f); + } + /* if we do not have flow control, limit to the set data rate */ if (!get_flow_control_enabled()) { bandwidth_mult = fminf(1.0f, bandwidth_mult); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 8f165a4925..0379bd2c11 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -180,6 +180,9 @@ public: void check_events_enable() { _should_check_events.store(true); } void check_events_disable() { _should_check_events.store(false); } + bool sending_parameters() const { return _sending_parameters.load(); } + void set_sending_parameters(bool sending) { _sending_parameters.store(sending); } + int get_uart_fd() const { return _uart_fd; } /** @@ -570,6 +573,7 @@ private: bool _received_messages{false}; /**< Whether we've received valid mavlink messages. */ px4::atomic_bool _should_check_events{false}; /**< Events subscription: only one MAVLink instance should check */ + px4::atomic_bool _sending_parameters{false}; /**< True if parameters are currently sent out */ unsigned _main_loop_delay{1000}; /**< mainloop delay, depends on data rate */ diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index ab341d0186..f1908d5e09 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -552,6 +552,8 @@ MavlinkParametersManager::send_param(param_t param, int component_id) _mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet); } + _last_param_sent = hrt_absolute_time(); + return 0; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 6ad09889c2..478c487881 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -77,6 +77,17 @@ public: void handle_message(const mavlink_message_t *msg); + /** + * Check if parameters are sent out. This includes: + * - while sending all parameters + * - while sending a single requested parameter + * - while sending out changed parameters + */ + bool send_active() const + { + return hrt_absolute_time() < _last_param_sent + 2_s; + } + private: int _send_all_index{-1}; @@ -161,6 +172,8 @@ protected: Mavlink &_mavlink; + hrt_abstime _last_param_sent{0}; + bool _first_send{false}; hrt_abstime _last_param_sent_timestamp{0}; // time at which the last parameter was sent }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 36c73a63c3..91f8681a27 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3291,6 +3291,7 @@ MavlinkReceiver::run() if (_mavlink.get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) { _parameters_manager.send(); + _mavlink.set_sending_parameters(_parameters_manager.send_active()); } if (_mavlink.ftp_enabled()) {