From 43d006aff26e32bbd442649de3cb627be710facc Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 2 Aug 2019 22:05:18 +0200 Subject: [PATCH] mavlink command sender: give channels more time to request command - if a channel receives an ack for a command, do not immediately remove the command item from the send queue but wait until the next ack timeout occurs. This gives other mavlink channels time to try to put identical commands into the send queue. Signed-off-by: RomanBapst --- .../mavlink/mavlink_command_sender.cpp | 21 ++++++++++++++++--- src/modules/mavlink/mavlink_command_sender.h | 2 +- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp index 3ce060f9b3..7ca7600bad 100644 --- a/src/modules/mavlink/mavlink_command_sender.cpp +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -126,9 +126,7 @@ void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_ from_sysid == item->command.target_system && from_compid == item->command.target_component && item->num_sent_per_channel[channel] != -1) { - // Drop it anyway because the command seems to have arrived at the destination, even if we - // receive IN_PROGRESS because we trust that it will be handled after that. - _commands.drop_current(); + item->num_sent_per_channel[channel] = -2; // mark this as acknowledged break; } } @@ -148,6 +146,23 @@ void MavlinkCommandSender::check_timeout(mavlink_channel_t channel) continue; } + // Loop through num_sent_per_channel and check if any channel has receives an ack for this command + // (indicated by the value -2). We avoid removing the command at the time of receiving the ack + // as some channels might be lagging behind and will end up putting the same command into the buffer. + bool dropped_command = false; + + for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) { + if (item->num_sent_per_channel[i] == -2) { + _commands.drop_current(); + dropped_command = true; + break; + } + } + + if (dropped_command) { + continue; + } + // The goal of this is to retry from all channels. Therefore, we keep // track of the retry count for each channel. // diff --git a/src/modules/mavlink/mavlink_command_sender.h b/src/modules/mavlink/mavlink_command_sender.h index fd7a0d98ee..ba36af1155 100644 --- a/src/modules/mavlink/mavlink_command_sender.h +++ b/src/modules/mavlink/mavlink_command_sender.h @@ -109,7 +109,7 @@ private: mavlink_command_long_t command = {}; hrt_abstime timestamp_us = 0; hrt_abstime last_time_sent_us = 0; - int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1}; + int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1}; // -1: channel did not request this command to be sent, -2: channel got an ack for this command } command_item_t; TimestampedList _commands{3};