Allow multiple messages to be scheduled with the same ID if the inter…

…val is set to 0.

src/main/native/include/rev/Drivers/DriverDeviceThread.h

@@ -84,20 +84,35 @@ class DriverDeviceThread {
         return nullptr; // If no matching element found
     }
 
+    detail::CANThreadSendQueueElement* findFirstMatchingIdWithNonZeroInterval(int targetId) {
+        for (auto& element : m_sendQueue) {
+            if (element.m_msg.GetMessageId() == targetId && element.m_intervalMs > 0) {
+                return &element;
+            }
+        }
+        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);
 
-        detail::CANThreadSendQueueElement* existing = findFirstMatchingId(msg.GetMessageId());
-
-        if(existing) {
-            existing->m_intervalMs = timeIntervalMs;
-            existing->m_msg = msg;
-        } else {
+        if(timeIntervalMs == 0) {
+            // If the time interval is 0, we want to allow duplicates.
             m_sendQueue.push_back(detail::CANThreadSendQueueElement(msg, timeIntervalMs));
+        } else {
+            // We don't want to replace elements with zero as the interval. Those should be guaranteed to be sent
+            detail::CANThreadSendQueueElement* existing = findFirstMatchingIdWithNonZeroInterval(msg.GetMessageId());
+
+            if(existing) {
+                existing->m_intervalMs = timeIntervalMs;
+                existing->m_msg = msg;
+            } else {
+                m_sendQueue.push_back(detail::CANThreadSendQueueElement(msg, timeIntervalMs));
+            }
         }
 
         // TODO: Limit the max queue size