GCS_MAVLink: check reply channel for space rather than current channel

This commit is contained in:
Peter Barker 2019-09-25 09:42:18 +10:00 committed by Andrew Tridgell
parent e7c2528654
commit 4838ee6f58
1 changed files with 8 additions and 9 deletions

View File

@ -400,8 +400,8 @@ uint8_t GCS_MAVLINK::send_parameter_async_replies()
uint8_t async_replies_sent_count = 0;
while (async_replies_sent_count < 5) {
if (param_replies.empty()) {
// nothing to do
struct pending_param_reply reply;
if (!param_replies.peek(reply)) {
return async_replies_sent_count;
}
@ -411,18 +411,12 @@ uint8_t GCS_MAVLINK::send_parameter_async_replies()
*/
uint32_t saved_reserve_param_space_start_ms = reserve_param_space_start_ms;
reserve_param_space_start_ms = 0; // bypass packet_overhead_chan reservation checking
if (!HAVE_PAYLOAD_SPACE(chan, PARAM_VALUE)) {
if (!HAVE_PAYLOAD_SPACE(reply.chan, PARAM_VALUE)) {
reserve_param_space_start_ms = AP_HAL::millis();
return async_replies_sent_count;
}
reserve_param_space_start_ms = saved_reserve_param_space_start_ms;
struct pending_param_reply reply;
if (!param_replies.pop(reply)) {
// internal error
return async_replies_sent_count;
}
mavlink_msg_param_value_send(
reply.chan,
reply.param_name,
@ -433,6 +427,11 @@ uint8_t GCS_MAVLINK::send_parameter_async_replies()
_queued_parameter_send_time_ms = AP_HAL::millis();
async_replies_sent_count++;
if (!param_replies.pop()) {
// internal error...
return async_replies_sent_count;
}
}
return async_replies_sent_count;
}