diff --git a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h
index 750d3e4..82eedc1 100644
--- a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h
+++ b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h
@@ -203,9 +203,7 @@ class CandleWinUSBDeviceThread :public DriverDeviceThread {
                 if (m_sendQueue.size() > 0) {
                     detail::CANThreadSendQueueElement el = m_sendQueue.front();
                     if (el.m_intervalMs == -1) {
-                        while(m_sendQueue.size() > 0) {
-                            m_sendQueue.pop_front();
-                        }
+                        removeElementsWithId(el.m_msg.GetMessageId());
                         continue;
                     }
 
diff --git a/src/main/native/include/rev/Drivers/DriverDeviceThread.h b/src/main/native/include/rev/Drivers/DriverDeviceThread.h
index 6af2c9c..b01d7ff 100644
--- a/src/main/native/include/rev/Drivers/DriverDeviceThread.h
+++ b/src/main/native/include/rev/Drivers/DriverDeviceThread.h
@@ -84,6 +84,10 @@ class DriverDeviceThread {
         return nullptr; // If no matching element found
     }
 
+    void removeElementsWithId(int targetId) {
+        m_sendQueue.erase(std::remove_if(m_sendQueue.begin(), m_sendQueue.end(), [targetId](detail::CANThreadSendQueueElement element) { return element.m_msg.GetMessageId() == targetId; }), m_sendQueue.end());
+    }
+
     bool EnqueueMessage(const CANMessage& msg, int32_t timeIntervalMs) {
         std::lock_guard<std::mutex> lock(m_writeMutex);