diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index d700887593..f1d11ad9bd 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -153,6 +153,7 @@ private: uint16_t _queued_parameter_count; ///< saved count of // parameters for // queued send + uint32_t _queued_parameter_send_time_ms; /// Count the number of reportable parameters. /// diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 374aa039e8..9d9f4fd168 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1989,31 +1989,47 @@ GCS_MAVLINK::_count_parameters() void GCS_MAVLINK::queued_param_send() { - // Check to see if we are sending parameters - if (NULL == _queued_parameter) return; + if (_queued_parameter == NULL) { + return; + } + + uint16_t bytes_allowed; + uint8_t count; + uint32_t tnow = millis(); - AP_Param *vp; - float value; + // use at most 30% of bandwidth on parameters. The constant 26 is + // 1/(1000 * 1/8 * 0.001 * 0.3) + bytes_allowed = g.serial3_baud * (tnow - _queued_parameter_send_time_ms) * 26; + if (bytes_allowed > comm_get_txspace(chan)) { + bytes_allowed = comm_get_txspace(chan); + } + count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); - // copy the current parameter and prepare to move to the next - vp = _queued_parameter; + while (_queued_parameter != NULL && count--) { + AP_Param *vp; + float value; - // if the parameter can be cast to float, report it here and break out of the loop - value = vp->cast_to_float(_queued_parameter_type); + // copy the current parameter and prepare to move to the next + vp = _queued_parameter; - char param_name[ONBOARD_PARAM_NAME_LENGTH]; - vp->copy_name(param_name, sizeof(param_name), true); + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(_queued_parameter_type); - mavlink_msg_param_value_send( - chan, - param_name, - value, - mav_var_type(_queued_parameter_type), - _queued_parameter_count, - _queued_parameter_index); - - _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type); - _queued_parameter_index++; + char param_name[ONBOARD_PARAM_NAME_LENGTH]; + vp->copy_name(param_name, sizeof(param_name), true); + + mavlink_msg_param_value_send( + chan, + param_name, + value, + mav_var_type(_queued_parameter_type), + _queued_parameter_count, + _queued_parameter_index); + + _queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type); + _queued_parameter_index++; + } + _queued_parameter_send_time_ms = tnow; } /**