Additions for REV Hardware Client

FRC-Team-4143 · Jun 2, 2021 · 9f96975 · 9f96975
1 parent d6c2e17
commit 9f96975
Show file tree

Hide file tree

Showing 8 changed files with 77 additions and 16 deletions.
diff --git a/CANBridge.code-workspace b/CANBridge.code-workspace
@@ -68,7 +68,14 @@
 			"fstream": "cpp",
 			"sstream": "cpp",
 			"iomanip": "cpp",
-			"candle_defs.h": "c"
+			"candle_defs.h": "c",
+			"compare": "cpp",
+			"concepts": "cpp",
+			"functional": "cpp",
+			"list": "cpp",
+			"set": "cpp",
+			"unordered_map": "cpp",
+			"xhash": "cpp"
 		},
 		"java.configuration.updateBuildConfiguration": "disabled"
 	}

diff --git a/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDevice.cpp b/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDevice.cpp
@@ -87,6 +87,10 @@ CandleWinUSBDevice::~CandleWinUSBDevice()
     close_candle_dev(m_handle);
 }
 
+void CandleWinUSBDevice::ClearSendQueue() {
+    m_thread.clearQueue();
+}
+
 std::string CandleWinUSBDevice::GetName() const
 {
     return m_name;
@@ -108,6 +112,14 @@ int CandleWinUSBDevice::GetNumberOfErrors()
     return m_thread.GetNumberOfErrors();
 }
 
+void CandleWinUSBDevice::setThreadPriority(utils::ThreadPriority priority) {
+    m_thread.setPriority(priority);
+}
+
+void CandleWinUSBDevice::stopRepeatedMessage(uint32_t messageId) {
+    m_thread.stopRepeatedMessage(messageId);
+}
+
 CANStatus CandleWinUSBDevice::SendCANMessage(const CANMessage& msg, int periodMs)
 {
     m_thread.EnqueueMessage(msg, periodMs);
@@ -158,15 +170,14 @@ CANStatus CandleWinUSBDevice::ReadStreamSession(uint32_t sessionHandle, struct H
     return m_thread.GetLastThreadError();
 }
 
-CANStatus CandleWinUSBDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr)
+CANStatus CandleWinUSBDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) {
+    return GetCANDetailStatus(percentBusUtilization, busOff, txFull,receiveErr, transmitErr, nullptr);
+}
+
+CANStatus CandleWinUSBDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime)
 {
-    rev::usb::CANStatusDetails details;
-    m_thread.GetCANStatus(&details);
-    *busOff = details.busOffCount;
-    *txFull = details.txFullCount;
-    *receiveErr = details.receiveErrCount;
-    *transmitErr = details.transmitErrCount;
-    *percentBusUtilization = 0.0; // todo how to get this
+    m_thread.GetCANStatusDetails(busOff, txFull, receiveErr, transmitErr, lastErrorTime);
+    *percentBusUtilization = 0.0; // todo how to get this 
 
     return m_thread.GetLastThreadError();
 }

diff --git a/src/main/native/cpp/Drivers/Serial/SerialDevice.cpp b/src/main/native/cpp/Drivers/Serial/SerialDevice.cpp
@@ -139,6 +139,10 @@ CANStatus SerialDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_
     return m_thread.GetLastThreadError();
 }
 
+CANStatus SerialDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) {
+    return GetCANDetailStatus(percentBusUtilization, busOff, txFull, receiveErr, transmitErr);
+}
+
 bool SerialDevice::IsConnected()
 {
     return true;

diff --git a/src/main/native/include/rev/CANDevice.h b/src/main/native/include/rev/CANDevice.h
@@ -33,6 +33,7 @@
 #include "rev/CANMessage.h"
 #include "rev/CANStatus.h"
 #include "rev/CANBridgeUtils.h"
+#include "utils/ThreadUtils.h"
 
 #include <hal/simulation/CanData.h>
 #include <hal/CAN.h>
@@ -58,8 +59,12 @@ class CANDevice {
     virtual CANStatus ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) = 0;
 
     virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) = 0;
+    virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) = 0;
 
     virtual bool IsConnected() = 0;
+    virtual void setThreadPriority(utils::ThreadPriority priority) {};
+    virtual void stopRepeatedMessage(uint32_t messageId) {};
+    virtual void ClearSendQueue() {};
 };
 
 } // namespace usb

diff --git a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDevice.h b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDevice.h
@@ -60,8 +60,11 @@ class CandleWinUSBDevice : public CANDevice {
     virtual CANStatus ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) override;
 
     virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) override;
-
+    virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) override;
     virtual bool IsConnected() override;
+    virtual void setThreadPriority(utils::ThreadPriority priority);
+    virtual void stopRepeatedMessage(uint32_t messageId);
+    virtual void ClearSendQueue();
 private:
     candle_handle m_handle;
     CandleWinUSBDeviceThread m_thread;

diff --git a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h
@@ -78,6 +78,23 @@ class CandleWinUSBDeviceThread :public DriverDeviceThread {
         utils::SetThreadPriority(m_thread.get(), utils::ThreadPriority::High);
     }
 
+    void setPriority(utils::ThreadPriority priority) {
+        if (m_thread.get() != nullptr)
+            utils::SetThreadPriority(m_thread.get(), priority);
+    }
+
+    void clearQueue() {
+        bool empty = m_sendQueue.empty();
+    }
+
+    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);
+        }
+    }
+
     void OpenStream(uint32_t* handle, CANBridge_CANFilter filter, uint32_t maxSize, CANStatus *status) override {
         std::lock_guard<std::mutex> lock(m_streamMutex);
 
@@ -99,11 +116,11 @@ class CandleWinUSBDeviceThread :public DriverDeviceThread {
        candle_frame_t incomingFrame;
 
         reading = candle_frame_read(m_device, &incomingFrame, 0);
-
         // Received a new frame, store it
         if (reading) {
             candle_frametype_t frameType = candle_frame_type(&incomingFrame);
             if(frameType == CANDLE_FRAMETYPE_ERROR) {
+                time(&lastErrorTime);
                 // Parse error data
                 if (incomingFrame.can_id & 0x00000040) {
                     m_statusDetails.busOffCount++;
@@ -149,7 +166,7 @@ class CandleWinUSBDeviceThread :public DriverDeviceThread {
    }
 
    bool WriteMessages(detail::CANThreadSendQueueElement el, std::chrono::steady_clock::time_point now) {
-        if (el.m_intervalMs == 0 || (now - el.m_prevTimestamp >= std::chrono::milliseconds(el.m_intervalMs)) ) {
+        if (el.m_intervalMs <= 1 || (now - el.m_prevTimestamp >= std::chrono::milliseconds(el.m_intervalMs)) ) {
             candle_frame_t frame;
             frame.can_dlc = el.m_msg.GetSize();
             // set extended id flag
@@ -186,21 +203,24 @@ class CandleWinUSBDeviceThread :public DriverDeviceThread {
                 if (m_sendQueue.size() > 0) {
                     detail::CANThreadSendQueueElement el = m_sendQueue.front();
                     if (el.m_intervalMs == -1) {
-                        m_sendQueue.pop();
+                        while(m_sendQueue.size() > 0) {
+                            m_sendQueue.pop();
+                        }
                         continue;
                     }
 
                     auto now = std::chrono::steady_clock::now();
 
-                    // Don't pop queue if send fails
+                    m_sendQueue.pop();
                     if (WriteMessages(el, now)) {
-                        m_sendQueue.pop();
 
                         // Return to end of queue if repeated
                         if (el.m_intervalMs > 0 ) {
                             el.m_prevTimestamp = now;
                             m_sendQueue.push(el);
                         }
+                    } else {
+                        m_sendQueue.push(el);
                     }
                 }
             }

diff --git a/src/main/native/include/rev/Drivers/DriverDeviceThread.h b/src/main/native/include/rev/Drivers/DriverDeviceThread.h
@@ -37,6 +37,7 @@
 #include <map>
 #include <queue>
 #include <memory>
+#include <time.h>
 
 #include <iostream>
 
@@ -64,7 +65,7 @@ class DriverDeviceThread {
         m_threadIntervalMs(threadIntervalMs),
         m_counter(counter) {
     }
-
+    
     virtual void Start() = 0;
 
     void Stop() {
@@ -133,6 +134,14 @@ class DriverDeviceThread {
         details = &m_statusDetails;
     }
 
+    void GetCANStatusDetails(uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* errorTime) {
+        *busOff = m_statusDetails.busOffCount;
+        *txFull = m_statusDetails.txFullCount;
+        *receiveErr = m_statusDetails.receiveErrCount;
+        *transmitErr = m_statusDetails.transmitErrCount;
+        *errorTime = lastErrorTime;
+    }
+
     CANStatus GetLastThreadError() {
         CANStatus last = m_threadStatus;
         m_threadStatus = CANStatus::kOk;
@@ -156,6 +165,7 @@ class DriverDeviceThread {
     uint32_t m_counter;
 
     CANStatusDetails m_statusDetails;
+    time_t lastErrorTime;
     int m_statusErrCount = 0;
     CANStatus m_threadStatus = CANStatus::kOk;
 

diff --git a/src/main/native/include/rev/Drivers/SerialPort/SerialDevice.h b/src/main/native/include/rev/Drivers/SerialPort/SerialDevice.h
@@ -58,6 +58,7 @@ class SerialDevice : public CANDevice {
     virtual CANStatus ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) override;
 
     virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) override;
+    virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) override;
 
     virtual bool IsConnected() override;
 private: