From 16ef372b53671a3a4b2833a1bb58e5823e730907 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Mon, 13 Jul 2020 21:57:54 -0700 Subject: [PATCH] [hal] Merge WaitForCachedData into WaitForDSData (#2587) Remove WaitForCachedData as it's no longer required. Also properly handle caching / transition detection logic that occurs at the WPILib level. This also changes DriverStation::IsNewControlData() to check for WPILib-level caching instead of wrapping the HAL function. --- .../main/native/athena/FRCDriverStation.cpp | 50 ++++--------- .../main/native/include/hal/DriverStation.h | 21 +----- hal/src/main/native/sim/DriverStation.cpp | 75 ++++--------------- wpilibc/src/main/native/cpp/DriverStation.cpp | 26 ++++++- .../main/native/include/frc/DriverStation.h | 10 ++- .../edu/wpi/first/wpilibj/DriverStation.java | 24 +++++- 6 files changed, 86 insertions(+), 120 deletions(-) diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp index a822cbaa87..51fe327fa4 100644 --- a/hal/src/main/native/athena/FRCDriverStation.cpp +++ b/hal/src/main/native/athena/FRCDriverStation.cpp @@ -124,7 +124,7 @@ static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) { static wpi::mutex* newDSDataAvailableMutex; static wpi::condition_variable* newDSDataAvailableCond; -static std::atomic_int newDSDataAvailableCounter{0}; +static int newDSDataAvailableCounter{0}; namespace hal { namespace init { @@ -355,41 +355,14 @@ static int& GetThreadLocalLastCount() { // will return false when instead it should return true. However, this at a // 20ms rate occurs once every 2.7 years of DS connected runtime, so not // worth the cycles to check. - thread_local int lastCount{-1}; + thread_local int lastCount{0}; return lastCount; } -void HAL_WaitForCachedControlData(void) { - HAL_WaitForCachedControlDataTimeout(0); -} - -HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) { - int& lastCount = GetThreadLocalLastCount(); - int currentCount = newDSDataAvailableCounter.load(); - if (lastCount != currentCount) { - lastCount = currentCount; - return true; - } - auto timeoutTime = - std::chrono::steady_clock::now() + std::chrono::duration(timeout); - - std::unique_lock lock{*newDSDataAvailableMutex}; - while (newDSDataAvailableCounter.load() == currentCount) { - if (timeout > 0) { - auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime); - if (timedOut == std::cv_status::timeout) { - return false; - } - } else { - newDSDataAvailableCond->wait(lock); - } - } - return true; -} - HAL_Bool HAL_IsNewControlData(void) { + std::scoped_lock lock{*newDSDataAvailableMutex}; int& lastCount = GetThreadLocalLastCount(); - int currentCount = newDSDataAvailableCounter.load(); + int currentCount = newDSDataAvailableCounter; if (lastCount == currentCount) return false; lastCount = currentCount; return true; @@ -406,12 +379,17 @@ void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); } * time has passed. Returns true on new data, false on timeout. */ HAL_Bool HAL_WaitForDSDataTimeout(double timeout) { + std::unique_lock lock{*newDSDataAvailableMutex}; + int& lastCount = GetThreadLocalLastCount(); + int currentCount = newDSDataAvailableCounter; + if (lastCount != currentCount) { + lastCount = currentCount; + return true; + } auto timeoutTime = std::chrono::steady_clock::now() + std::chrono::duration(timeout); - int currentCount = newDSDataAvailableCounter.load(); - std::unique_lock lock{*newDSDataAvailableMutex}; - while (newDSDataAvailableCounter.load() == currentCount) { + while (newDSDataAvailableCounter == currentCount) { if (timeout > 0) { auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime); if (timedOut == std::cv_status::timeout) { @@ -421,6 +399,7 @@ HAL_Bool HAL_WaitForDSDataTimeout(double timeout) { newDSDataAvailableCond->wait(lock); } } + lastCount = newDSDataAvailableCounter; return true; } @@ -431,8 +410,9 @@ static void newDataOccur(uint32_t refNum) { // Since we could get other values, require our specific handle // to signal our threads if (refNum != refNumber) return; + std::scoped_lock lock{*newDSDataAvailableMutex}; // Notify all threads - newDSDataAvailableCounter.fetch_add(1); + ++newDSDataAvailableCounter; newDSDataAvailableCond->notify_all(); } diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h index 091e662449..63a904086f 100644 --- a/hal/src/main/native/include/hal/DriverStation.h +++ b/hal/src/main/native/include/hal/DriverStation.h @@ -199,24 +199,6 @@ int32_t HAL_GetMatchInfo(HAL_MatchInfo* info); */ void HAL_ReleaseDSMutex(void); -/** - * Checks if new control data has arrived since the last - * HAL_WaitForCachedControlData or HAL_IsNewControlData call. If new data has - * not arrived, waits for new data to arrive. Otherwise, returns immediately. - */ -void HAL_WaitForCachedControlData(void); - -/** - * Checks if new control data has arrived since the last - * HAL_WaitForCachedControlData or HAL_IsNewControlData call. If new data has - * not arrived, waits for new data to arrive, or a timeout. Otherwise, returns - * immediately. - * - * @param timeout timeout in seconds - * @return true for new data, false for timeout - */ -HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout); - /** * Has a new control packet from the driver station arrived since the last * time this function was called? @@ -227,6 +209,9 @@ HAL_Bool HAL_IsNewControlData(void); /** * Waits for the newest DS packet to arrive. Note that this is a blocking call. + * Checks if new control data has arrived since the last HAL_WaitForDSData or + * HAL_IsNewControlData call. If new data has not arrived, waits for new data + * to arrive. Otherwise, returns immediately. */ void HAL_WaitForDSData(void); diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index 68c31fd1d1..f5c868cc14 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -218,42 +218,29 @@ void HAL_ObserveUserProgramTest(void) { // TODO } -#ifdef __APPLE__ -static pthread_key_t lastCountKey; -static pthread_once_t lastCountKeyOnce = PTHREAD_ONCE_INIT; - -static void InitLastCountKey(void) { - pthread_key_create(&lastCountKey, std::free); -} -#endif - static int& GetThreadLocalLastCount() { // There is a rollover error condition here. At Packet# = n * (uintmax), this // will return false when instead it should return true. However, this at a // 20ms rate occurs once every 2.7 years of DS connected runtime, so not // worth the cycles to check. -#ifdef __APPLE__ - pthread_once(&lastCountKeyOnce, InitLastCountKey); - int* lastCountPtr = static_cast(pthread_getspecific(lastCountKey)); - if (!lastCountPtr) { - lastCountPtr = static_cast(std::malloc(sizeof(int))); - *lastCountPtr = -1; - pthread_setspecific(lastCountKey, lastCountPtr); - } - int& lastCount = *lastCountPtr; -#else - thread_local int lastCount{-1}; -#endif + thread_local int lastCount{0}; return lastCount; } -void HAL_WaitForCachedControlData(void) { - HAL_WaitForCachedControlDataTimeout(0); +HAL_Bool HAL_IsNewControlData(void) { + std::scoped_lock lock(newDSDataAvailableMutex); + int& lastCount = GetThreadLocalLastCount(); + int currentCount = newDSDataAvailableCounter; + if (lastCount == currentCount) return false; + lastCount = currentCount; + return true; } -HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) { - int& lastCount = GetThreadLocalLastCount(); +void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); } + +HAL_Bool HAL_WaitForDSDataTimeout(double timeout) { std::unique_lock lock(newDSDataAvailableMutex); + int& lastCount = GetThreadLocalLastCount(); int currentCount = newDSDataAvailableCounter; if (lastCount != currentCount) { lastCount = currentCount; @@ -263,7 +250,6 @@ HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) { if (isFinalized.load()) { return false; } - auto timeoutTime = std::chrono::steady_clock::now() + std::chrono::duration(timeout); @@ -277,42 +263,7 @@ HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) { newDSDataAvailableCond->wait(lock); } } - return true; -} - -HAL_Bool HAL_IsNewControlData(void) { - int& lastCount = GetThreadLocalLastCount(); - int currentCount = 0; - { - std::scoped_lock lock(newDSDataAvailableMutex); - currentCount = newDSDataAvailableCounter; - } - if (lastCount == currentCount) return false; - lastCount = currentCount; - return true; -} - -void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); } - -HAL_Bool HAL_WaitForDSDataTimeout(double timeout) { - if (isFinalized.load()) { - return false; - } - auto timeoutTime = - std::chrono::steady_clock::now() + std::chrono::duration(timeout); - - std::unique_lock lock(newDSDataAvailableMutex); - int currentCount = newDSDataAvailableCounter; - while (newDSDataAvailableCounter == currentCount) { - if (timeout > 0) { - auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime); - if (timedOut == std::cv_status::timeout) { - return false; - } - } else { - newDSDataAvailableCond->wait(lock); - } - } + lastCount = newDSDataAvailableCounter; return true; } diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index 7c6488340a..aa125582d2 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -68,6 +68,15 @@ using namespace frc; static constexpr double kJoystickUnpluggedMessageInterval = 1.0; +static int& GetDSLastCount() { + // There is a rollover error condition here. At Packet# = n * (uintmax), this + // will return false when instead it should return true. However, this at a + // 20ms rate occurs once every 2.7 years of DS connected runtime, so not + // worth the cycles to check. + thread_local int lastCount{0}; + return lastCount; +} + DriverStation::~DriverStation() { m_isRunning = false; // Trigger a DS mutex release in case there is no driver station running. @@ -367,7 +376,14 @@ bool DriverStation::IsDSAttached() const { return controlWord.dsAttached; } -bool DriverStation::IsNewControlData() const { return HAL_IsNewControlData(); } +bool DriverStation::IsNewControlData() const { + std::unique_lock lock(m_waitForDataMutex); + int& lastCount = GetDSLastCount(); + int currentCount = m_waitForDataCounter; + if (lastCount == currentCount) return false; + lastCount = currentCount; + return true; +} bool DriverStation::IsFMSAttached() const { HAL_ControlWord controlWord; @@ -448,7 +464,12 @@ bool DriverStation::WaitForData(double timeout) { std::chrono::steady_clock::now() + std::chrono::duration(timeout); std::unique_lock lock(m_waitForDataMutex); + int& lastCount = GetDSLastCount(); int currentCount = m_waitForDataCounter; + if (lastCount != currentCount) { + lastCount = currentCount; + return true; + } while (m_waitForDataCounter == currentCount) { if (timeout > 0) { auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime); @@ -459,6 +480,7 @@ bool DriverStation::WaitForData(double timeout) { m_waitForDataCond.wait(lock); } } + lastCount = m_waitForDataCounter; return true; } diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h index 450870119d..ee5ac68f1a 100644 --- a/wpilibc/src/main/native/include/frc/DriverStation.h +++ b/wpilibc/src/main/native/include/frc/DriverStation.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -313,6 +313,9 @@ class DriverStation : public ErrorBase { * * This is a good way to delay processing until there is new driver station * data to act on. + * + * Checks if new control data has arrived since the last waitForData call + * on the current thread. If new data has not arrived, returns immediately. */ void WaitForData(); @@ -320,6 +323,9 @@ class DriverStation : public ErrorBase { * Wait until a new packet comes from the driver station, or wait for a * timeout. * + * Checks if new control data has arrived since the last waitForData call + * on the current thread. If new data has not arrived, returns immediately. + * * If the timeout is less then or equal to 0, wait indefinitely. * * Timeout is in milliseconds @@ -446,7 +452,7 @@ class DriverStation : public ErrorBase { std::thread m_dsThread; std::atomic m_isRunning{false}; - wpi::mutex m_waitForDataMutex; + mutable wpi::mutex m_waitForDataMutex; wpi::condition_variable m_waitForDataCond; int m_waitForDataCounter; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index 7dfa2a959e..0f388de5f4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -164,6 +164,7 @@ public class DriverStation { private final Lock m_waitForDataMutex; private final Condition m_waitForDataCond; private int m_waitForDataCount; + private final ThreadLocal m_lastCount = ThreadLocal.withInitial(() -> 0); // Robot state status variables private boolean m_userInDisabled; @@ -696,7 +697,17 @@ public class DriverStation { * @return True if the control data has been updated since the last call. */ public boolean isNewControlData() { - return HAL.isNewControlData(); + m_waitForDataMutex.lock(); + try { + int currentCount = m_waitForDataCount; + if (m_lastCount.get() != currentCount) { + m_lastCount.set(currentCount); + return true; + } + } finally { + m_waitForDataMutex.unlock(); + } + return false; } /** @@ -849,6 +860,9 @@ public class DriverStation { /** * Wait for new data from the driver station. + * + *

Checks if new control data has arrived since the last waitForData call + * on the current thread. If new data has not arrived, returns immediately. */ public void waitForData() { waitForData(0); @@ -858,6 +872,9 @@ public class DriverStation { * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data * only. * + *

Checks if new control data has arrived since the last waitForData call + * on the current thread. If new data has not arrived, returns immediately. + * * @param timeout The maximum time in seconds to wait. * @return true if there is new data, otherwise false */ @@ -867,6 +884,10 @@ public class DriverStation { m_waitForDataMutex.lock(); try { int currentCount = m_waitForDataCount; + if (m_lastCount.get() != currentCount) { + m_lastCount.set(currentCount); + return true; + } while (m_waitForDataCount == currentCount) { if (timeout > 0) { long now = RobotController.getFPGATime(); @@ -886,6 +907,7 @@ public class DriverStation { m_waitForDataCond.await(); } } + m_lastCount.set(m_waitForDataCount); // Return true if we have received a proper signal return true; } catch (InterruptedException ex) {