diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp index 23c874f741..9ffab8b257 100644 --- a/hal/src/main/native/athena/FRCDriverStation.cpp +++ b/hal/src/main/native/athena/FRCDriverStation.cpp @@ -338,12 +338,45 @@ void HAL_ObserveUserProgramTest(void) { FRC_NetworkCommunication_observeUserProgramTest(); } -HAL_Bool HAL_IsNewControlData(void) { +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. thread_local int lastCount{-1}; + return lastCount; +} + +void HAL_WaitForCachedControlData(void) { + HAL_WaitForCachedControlDataTimeout(0); +} + +HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) { + int& lastCount = GetThreadLocalLastCount(); + std::unique_lock lock{*newDSDataAvailableMutex}; + int currentCount = newDSDataAvailableCounter; + if (lastCount != currentCount) { + lastCount = currentCount; + return true; + } + auto timeoutTime = + std::chrono::steady_clock::now() + std::chrono::duration(timeout); + + 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); + } + } + return true; +} + +HAL_Bool HAL_IsNewControlData(void) { + int& lastCount = GetThreadLocalLastCount(); std::lock_guard lock{*newDSDataAvailableMutex}; int currentCount = newDSDataAvailableCounter; if (lastCount == currentCount) return false; diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h index db98698b62..471c18b971 100644 --- a/hal/src/main/native/include/hal/DriverStation.h +++ b/hal/src/main/native/include/hal/DriverStation.h @@ -193,6 +193,24 @@ 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? diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index a17994edbe..e8404cf415 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -210,7 +210,11 @@ static void InitLastCountKey(void) { } #endif -HAL_Bool HAL_IsNewControlData(void) { +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)); @@ -223,13 +227,43 @@ HAL_Bool HAL_IsNewControlData(void) { #else thread_local int lastCount{-1}; #endif - // 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. + return lastCount; +} + +HAL_Bool HAL_WaitForCachedControlDataTimeout(double timeout) { + int& lastCount = GetThreadLocalLastCount(); + std::unique_lock lock(newDSDataAvailableMutex); + int currentCount = newDSDataAvailableCounter; + if (lastCount != currentCount) { + lastCount = currentCount; + return true; + } + + if (isFinalized.load()) { + return false; + } + + auto timeoutTime = + std::chrono::steady_clock::now() + std::chrono::duration(timeout); + + 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); + } + } + return true; +} + +HAL_Bool HAL_IsNewControlData(void) { + int& lastCount = GetThreadLocalLastCount(); int currentCount = 0; { - std::unique_lock lock(newDSDataAvailableMutex); + std::scoped_lock lock(newDSDataAvailableMutex); currentCount = newDSDataAvailableCounter; } if (lastCount == currentCount) return false;