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<command_item_t> _commands{3};