diff --git a/libraries/GCS_MAVLink/GCS_Param.cpp b/libraries/GCS_MAVLink/GCS_Param.cpp index 884b3c0311..c746431e26 100644 --- a/libraries/GCS_MAVLink/GCS_Param.cpp +++ b/libraries/GCS_MAVLink/GCS_Param.cpp @@ -53,11 +53,17 @@ GCS_MAVLINK::queued_param_send() // use at most 30% of bandwidth on parameters. The constant 26 is // 1/(1000 * 1/8 * 0.001 * 0.3) - bytes_allowed = 57 * (tnow - _queued_parameter_send_time_ms) * 26; + const uint32_t link_bw = _port->bw_in_kilobytes_per_second(); + + bytes_allowed = link_bw * (tnow - _queued_parameter_send_time_ms) * 26; + const uint16_t size_for_one_param_value_msg = MAVLINK_MSG_ID_PARAM_VALUE_LEN + packet_overhead(); + if (bytes_allowed < size_for_one_param_value_msg) { + bytes_allowed = size_for_one_param_value_msg; + } if (bytes_allowed > comm_get_txspace(chan)) { bytes_allowed = comm_get_txspace(chan); } - count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + packet_overhead()); + count = bytes_allowed / size_for_one_param_value_msg; // when we don't have flow control we really need to keep the // param download very slow, or it tends to stall