From 807cfc8aac30d57093f54f633aea1b813663d00b Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 26 Jul 2019 10:39:08 +0200 Subject: [PATCH] mavlink: fix race condition in mavlink_command_sender - if we receive an ack for a command through a specific mavlink channel then do not drop the corresponding command in the queue if this specific mavlink channel did not issue the command. If we don't do this we can end up in a situation where we associate an ack coming through a specific mavlink channel to a command in the queue which was not requested by this mavlink channel. Moreover, the actual command for which the ack was meant remains in the queue and eventually triggers a timeout. Signed-off-by: RomanBapst --- src/modules/mavlink/mavlink_command_sender.cpp | 5 +++-- src/modules/mavlink/mavlink_command_sender.h | 3 ++- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp index 11e34248b8..cc9b49b2a8 100644 --- a/src/modules/mavlink/mavlink_command_sender.cpp +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -112,7 +112,7 @@ int MavlinkCommandSender::handle_vehicle_command(const struct vehicle_command_s } void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_t &ack, - uint8_t from_sysid, uint8_t from_compid) + uint8_t from_sysid, uint8_t from_compid, uint8_t channel) { CMD_DEBUG("handling result %d for command %d (from %d:%d)", ack.result, ack.command, from_sysid, from_compid); @@ -124,7 +124,8 @@ void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_ // Check if the incoming ack matches any of the commands that we have sent. if (item->command.command == ack.command && from_sysid == item->command.target_system && - from_compid == item->command.target_component) { + 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(); diff --git a/src/modules/mavlink/mavlink_command_sender.h b/src/modules/mavlink/mavlink_command_sender.h index 5e2d174930..fd7a0d98ee 100644 --- a/src/modules/mavlink/mavlink_command_sender.h +++ b/src/modules/mavlink/mavlink_command_sender.h @@ -81,7 +81,8 @@ public: * Handle mavlink command_ack. * thread-safe */ - void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid); + void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid, + uint8_t channel); private: MavlinkCommandSender() = default; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 818d0c0a45..f5550354d5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -524,7 +524,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(msg, &ack); - MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid); + MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink->get_channel()); vehicle_command_ack_s command_ack = {}; command_ack.timestamp = hrt_absolute_time();