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 <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2019-07-26 10:39:08 +02:00
committed by Beat Küng
parent 830d576f45
commit 807cfc8aac
3 changed files with 6 additions and 4 deletions

View File

@@ -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();