From 943e3dd83b18aa1dbfdfd51443bfd9e90f6845ad Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 17 Jun 2015 18:27:57 -0700 Subject: [PATCH 1/2] CANJaguar messages are now received in a separate thread --- wpilibc/wpilibC++Devices/include/CANJaguar.h | 14 +++++++-- wpilibc/wpilibC++Devices/src/CANJaguar.cpp | 44 ++++++++++++++++++++++------ 2 files changed, 46 insertions(+), 12 deletions(-) diff --git a/wpilibc/wpilibC++Devices/include/CANJaguar.h b/wpilibc/wpilibC++Devices/include/CANJaguar.h index 186d3b4..72444bb 100644 --- a/wpilibc/wpilibC++Devices/include/CANJaguar.h +++ b/wpilibc/wpilibC++Devices/include/CANJaguar.h @@ -17,6 +17,9 @@ #include "tables/ITable.h" #include "NetworkCommunication/CANSessionMux.h" +#include +#include +#include #include /** @@ -146,6 +149,11 @@ protected: void setupPeriodicStatus(); void updatePeriodicStatus(); + void updateFunc(); + + std::thread* m_updateThread; + std::atomic m_runUpdate{false}; + mutable std::recursive_mutex m_mutex; uint8_t m_deviceNumber; float m_value; @@ -197,9 +205,9 @@ protected: uint8_t m_hardwareVersion; // Which periodic status messages have we received at least once? - bool m_receivedStatusMessage0; - bool m_receivedStatusMessage1; - bool m_receivedStatusMessage2; + std::atomic m_receivedStatusMessage0; + std::atomic m_receivedStatusMessage1; + std::atomic m_receivedStatusMessage2; bool m_controlEnabled; diff --git a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp index 4017fd7..68cbb54 100644 --- a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp +++ b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp @@ -205,6 +205,9 @@ void CANJaguar::InitCANJaguar() break; } + m_runUpdate = true; + m_updateThread = new std::thread(&CANJaguar::updateFunc, this); + HALReport(HALUsageReporting::kResourceType_CANJaguar, m_deviceNumber, m_controlMode); LiveWindow::GetInstance()->AddActuator("CANJaguar", m_deviceNumber, this); } @@ -254,6 +257,10 @@ CANJaguar::CANJaguar(uint8_t deviceNumber) CANJaguar::~CANJaguar() { + m_runUpdate = false; + m_updateThread->join(); + delete m_updateThread; + allocated->Free(m_deviceNumber-1); int32_t status; @@ -587,32 +594,51 @@ void CANJaguar::updatePeriodicStatus() { // Check if a new bus voltage/output voltage/current/temperature message // has arrived and unpack the values into the cached member variables if(getMessage(LM_API_PSTAT_DATA_S0, CAN_MSGID_FULL_M, data, &dataSize)) { + m_mutex.lock(); m_busVoltage = unpackFXP8_8(data); m_outputVoltage = unpackPercentage(data + 2) * m_busVoltage; m_outputCurrent = unpackFXP8_8(data + 4); m_temperature = unpackFXP8_8(data + 6); + m_mutex.unlock(); m_receivedStatusMessage0 = true; } // Check if a new position/speed message has arrived and do the same if(getMessage(LM_API_PSTAT_DATA_S1, CAN_MSGID_FULL_M, data, &dataSize)) { + m_mutex.lock(); m_position = unpackFXP16_16(data); m_speed = unpackFXP16_16(data + 4); + m_mutex.unlock(); m_receivedStatusMessage1 = true; } // Check if a new limits/faults message has arrived and do the same if(getMessage(LM_API_PSTAT_DATA_S2, CAN_MSGID_FULL_M, data, &dataSize)) { + m_mutex.lock(); m_limits = data[0]; m_faults = data[1]; + m_mutex.unlock(); m_receivedStatusMessage2 = true; } } /** + * Check for new periodic status updates in this thread and unpack them into + * local variables. + */ +void CANJaguar::updateFunc() +{ + while (m_runUpdate) + { + updatePeriodicStatus(); + Wait(0.01); + } +} + +/** * Check all unverified params and make sure they're equal to their local * cached versions. If a value isn't available, it gets requested. If a value * doesn't match up, it gets set again. @@ -1671,7 +1697,7 @@ CANJaguar::ControlMode CANJaguar::GetControlMode() */ float CANJaguar::GetBusVoltage() { - updatePeriodicStatus(); + std::lock_guard lock(m_mutex); return m_busVoltage; } @@ -1683,7 +1709,7 @@ float CANJaguar::GetBusVoltage() */ float CANJaguar::GetOutputVoltage() { - updatePeriodicStatus(); + std::lock_guard lock(m_mutex); return m_outputVoltage; } @@ -1695,7 +1721,7 @@ float CANJaguar::GetOutputVoltage() */ float CANJaguar::GetOutputCurrent() { - updatePeriodicStatus(); + std::lock_guard lock(m_mutex); return m_outputCurrent; } @@ -1707,7 +1733,7 @@ float CANJaguar::GetOutputCurrent() */ float CANJaguar::GetTemperature() { - updatePeriodicStatus(); + std::lock_guard lock(m_mutex); return m_temperature; } @@ -1721,7 +1747,7 @@ float CANJaguar::GetTemperature() */ double CANJaguar::GetPosition() { - updatePeriodicStatus(); + std::lock_guard lock(m_mutex); return m_position; } @@ -1733,7 +1759,7 @@ double CANJaguar::GetPosition() */ double CANJaguar::GetSpeed() { - updatePeriodicStatus(); + std::lock_guard lock(m_mutex); return m_speed; } @@ -1745,7 +1771,7 @@ double CANJaguar::GetSpeed() */ bool CANJaguar::GetForwardLimitOK() { - updatePeriodicStatus(); + std::lock_guard lock(m_mutex); return m_limits & kForwardLimit; } @@ -1757,7 +1783,7 @@ bool CANJaguar::GetForwardLimitOK() */ bool CANJaguar::GetReverseLimitOK() { - updatePeriodicStatus(); + std::lock_guard lock(m_mutex); return m_limits & kReverseLimit; } @@ -1773,7 +1799,7 @@ bool CANJaguar::GetReverseLimitOK() */ uint16_t CANJaguar::GetFaults() { - updatePeriodicStatus(); + std::lock_guard lock(m_mutex); return m_faults; } -- 2.4.4