Merge pull request #25 from REVrobotics/fix/dont-clear-all-frames

Only remove elements matching the id we're throwing out
REVrobotics · Mar 25, 2024 · 185ba56 · 185ba56
2 parents 8316a1f + cb17689
commit 185ba56
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 3 deletions.
diff --git 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
@@ -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);