aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: tidy queued_param_send a little

Browse Source
This commit is contained in:
Peter Barker 2018-12-18 14:13:17 +11:00 committed by Peter Barker
parent 6d4e886bc9
commit 61e048a41b
1 changed files with 5 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/GCS_MAVLink/GCS_Param.cpp
View File

@ -45,9 +45,7 @@ GCS_MAVLINK::queued_param_send()
if (_queued_parameter == nullptr) {
return;
}
uint16_t bytes_allowed;
uint8_t count;
uint32_t tnow = AP_HAL::millis();
uint32_t tstart = AP_HAL::micros();
@ -55,7 +53,7 @@ GCS_MAVLINK::queued_param_send()
// 1/(1000 * 1/8 * 0.001 * 0.3)
const uint32_t link_bw = _port->bw_in_kilobytes_per_second();
bytes_allowed = link_bw * (tnow - _queued_parameter_send_time_ms) * 26;
uint16_t 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;
@ -63,7 +61,7 @@ GCS_MAVLINK::queued_param_send()
if (bytes_allowed > comm_get_txspace(chan)) {
bytes_allowed = comm_get_txspace(chan);
}
count = bytes_allowed / size_for_one_param_value_msg;
uint8_t 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
@ -72,22 +70,13 @@ GCS_MAVLINK::queued_param_send()
}
while (_queued_parameter != nullptr && count--) {
AP_Param *vp;
float value;
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
// 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);
char param_name[AP_MAX_NAME_SIZE];
vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
_queued_parameter->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
mavlink_msg_param_value_send(
chan,
param_name,
value,
_queued_parameter->cast_to_float(_queued_parameter_type),
mav_var_type(_queued_parameter_type),
_queued_parameter_count,
_queued_parameter_index);