diff --git a/isobus/src/isobus_virtual_terminal_client.cpp b/isobus/src/isobus_virtual_terminal_client.cpp
index 054164cc..b8af3ec6 100644
--- a/isobus/src/isobus_virtual_terminal_client.cpp
+++ b/isobus/src/isobus_virtual_terminal_client.cpp
@@ -4503,11 +4503,11 @@ namespace isobus
 			return true;
 		}
 
+		LOCK_GUARD(Mutex, commandQueueMutex);
 		if (replace && replace_command(data))
 		{
 			return true;
 		}
-		LOCK_GUARD(Mutex, commandQueueMutex);
 		commandQueue.emplace_back(data);
 		return true;
 	}
@@ -4531,6 +4531,10 @@ namespace isobus
 					it = commandQueue.erase(it);
 				}
 			}
+			else
+			{
+				it++;
+			}
 		}
 		return alreadyReplaced;
 	}