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 <bapstroman@gmail.com>

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.
 		//

src/modules/mavlink/mavlink_command_sender.h

@@ -109,7 +109,7 @@ class MavlinkCommandSender
 		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<command_item_t> _commands{3};