Merge pull request #24 from REVrobotics/fix/repeat-frames

Fix repeat frames
REVrobotics · Mar 25, 2024 · 8316a1f · 8316a1f
2 parents d857994 + 6a79c2e
commit 8316a1f
Show file tree

Hide file tree

Showing 4 changed files with 65 additions and 13 deletions.
diff --git a/README.md b/README.md
@@ -4,6 +4,41 @@
 
 This repository is for the CANBridge software that is run on non-roboRIO platforms.
 
+## Behavior
+
+When sending a frame with a given interval, the behavior when
+setting a new interval is as follows:
+
+The first time a frame is scheduled with an interval, it
+will be sent at the next available time. The next instance
+of the same frame id will be sent after that interval, even
+if the interval has changed.
+
+See the following pseudo-example:
+
+```
+sendMessage(frame, 5000)
+delay(1000)
+
+updateFrameData(frame)
+sendMessage(frame, 1000)
+delay(500)
+
+updateFrameData(frame)
+sendMessage(frame, 2000)
+```
+
+In this case, the first frame will be scheduled immediately,
+the second will be scheduled 5 seconds later, and after that,
+subsequent frames will be scheduled every 2 seconds. Note
+that any change to the data in the second call will not be
+sent, meaning the second call is essentially a no-op if a
+new call with different data is sent before the previous
+interval is up. Sending a frame with an interval of -1
+will cancel the repeat, and not send the frame. Sending with
+an interval of 0 will schedule the new frame once, then stop
+repeating.
+
 ## Build Requirements
 
 1. Git

diff --git a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h
@@ -90,8 +90,8 @@ class CandleWinUSBDeviceThread :public DriverDeviceThread {
     void stopRepeatedMessage(uint32_t messageId) {
         for (int i = 0; i < m_sendQueue.size(); i++) {
             detail::CANThreadSendQueueElement el = m_sendQueue.front();
-            m_sendQueue.pop();
-            if (el.m_msg.GetMessageId() != messageId) m_sendQueue.push(el);
+            m_sendQueue.pop_front();
+            if (el.m_msg.GetMessageId() != messageId) m_sendQueue.push_back(el);
         }
     }
 
@@ -204,23 +204,23 @@ class CandleWinUSBDeviceThread :public DriverDeviceThread {
                     detail::CANThreadSendQueueElement el = m_sendQueue.front();
                     if (el.m_intervalMs == -1) {
                         while(m_sendQueue.size() > 0) {
-                            m_sendQueue.pop();
+                            m_sendQueue.pop_front();
                         }
                         continue;
                     }
 
                     auto now = std::chrono::steady_clock::now();
 
-                    m_sendQueue.pop();
+                    m_sendQueue.pop_front();
                     if (WriteMessages(el, now)) {
 
                         // Return to end of queue if repeated
                         if (el.m_intervalMs > 0 ) {
                             el.m_prevTimestamp = now;
-                            m_sendQueue.push(el);
+                            m_sendQueue.push_back(el);
                         }
                     } else {
-                        m_sendQueue.push(el);
+                        m_sendQueue.push_back(el);
                     }
                 }
             }

diff --git a/src/main/native/include/rev/Drivers/DriverDeviceThread.h b/src/main/native/include/rev/Drivers/DriverDeviceThread.h
@@ -75,9 +75,26 @@ class DriverDeviceThread {
         }
     }
 
+    detail::CANThreadSendQueueElement* findFirstMatchingId(int targetId) {
+        for (auto& element : m_sendQueue) {
+            if (element.m_msg.GetMessageId() == targetId) {
+                return &element;
+            }
+        }
+        return nullptr; // If no matching element found
+    }
+
     bool EnqueueMessage(const CANMessage& msg, int32_t timeIntervalMs) {
         std::lock_guard<std::mutex> lock(m_writeMutex);
-        m_sendQueue.push(detail::CANThreadSendQueueElement(msg, timeIntervalMs));
+
+        detail::CANThreadSendQueueElement* existing = findFirstMatchingId(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
         return true;
@@ -169,7 +186,7 @@ class DriverDeviceThread {
     int m_statusErrCount = 0;
     CANStatus m_threadStatus = CANStatus::kOk;
 
-    std::queue<detail::CANThreadSendQueueElement> m_sendQueue;
+    std::deque<detail::CANThreadSendQueueElement> m_sendQueue;
     std::map<uint32_t, std::shared_ptr<CANMessage>> m_readStore;
     std::map<uint32_t, std::unique_ptr<CANStreamHandle>> m_readStream; // (id, mask), max size, message buffer
 
@@ -200,20 +217,20 @@ class DriverDeviceThread {
                 for (size_t i=0;i<queueSize;i++) {
                     detail::CANThreadSendQueueElement el = m_sendQueue.front();
                     if (el.m_intervalMs == -1) {
-                        m_sendQueue.pop();
+                        m_sendQueue.pop_front();
                         continue;
                     }
 
                     auto now = std::chrono::steady_clock::now();
 
                     // Don't pop queue if send fails
                     if (WriteMessages(el, now)) {
-                        m_sendQueue.pop();
+                        m_sendQueue.pop_front();
 
                         // Return to end of queue if repeated
                         if (el.m_intervalMs > 0 ) {
                             el.m_prevTimestamp = now;
-                            m_sendQueue.push(el);
+                            m_sendQueue.push_back(el);
                         }
                     } else {
                         // Wait a little bit before trying again

diff --git a/src/main/native/include/rev/Drivers/SerialPort/SerialDeviceThread.h b/src/main/native/include/rev/Drivers/SerialPort/SerialDeviceThread.h
@@ -272,7 +272,7 @@ class SerialDeviceThread : public DriverDeviceThread {
                 std::lock_guard<std::mutex> lock(m_writeMutex);
                 if (m_sendQueue.size() > 0) {
                     detail::CANThreadSendQueueElement el = m_sendQueue.front();
-                    m_sendQueue.pop();
+                    m_sendQueue.pop_front();
                     if (el.m_intervalMs == -1) {
                         continue;
                     }
@@ -285,7 +285,7 @@ class SerialDeviceThread : public DriverDeviceThread {
                         if (el.m_intervalMs > 0 ) {
                             el.m_prevTimestamp = now;
 
-                            m_sendQueue.push(el);
+                            m_sendQueue.push_back(el);
                         }
                         doRead = true;
                     }