From d53fcb0bab7df76df8f05f6208b7fcd6cb08bbef Mon Sep 17 00:00:00 2001 From: brad112358 Date: Mon, 22 Feb 2021 17:57:29 -0600 Subject: [PATCH] mavlink: use RADIO_STATUS to regulate parameter sending --- src/modules/mavlink/mavlink_main.cpp | 3 +++ src/modules/mavlink/mavlink_main.h | 2 ++ src/modules/mavlink/mavlink_parameters.cpp | 6 ++++-- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c01054ca77..049bb7a249 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1476,6 +1476,9 @@ Mavlink::update_radio_status(const radio_status_s &radio_status) /* this indicates spare bandwidth, increase by 2.5% */ _radio_status_mult *= 1.025f; } + + /* Constrain radio status multiplier between 1% and 100% to allow recovery */ + _radio_status_mult = math::constrain(_radio_status_mult, 0.01f, 1.0f); } } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index bce65b9d9e..4992cd8759 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -518,6 +518,8 @@ public: static hrt_abstime &get_first_start_time() { return _first_start_time; } + bool radio_status_critical() const { return _radio_status_critical; } + private: int _instance_id{0}; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 6f356f71bc..4c1366eeff 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -329,7 +329,8 @@ MavlinkParametersManager::send() int i = 0; // Send while burst is not exceeded, we still have buffer space and still something to send - while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && send_params()) {} + while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() + && send_params()) {} } bool @@ -390,7 +391,8 @@ MavlinkParametersManager::send_untransmitted() break; } } - } while ((_mavlink->get_free_tx_buf() >= get_size()) && (_param_update_index < (int) param_count())); + } while ((_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() + && (_param_update_index < (int) param_count())); // Flag work as done once all params have been sent if (_param_update_index >= (int) param_count()) {