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>

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

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;

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