Add way to atomically check for new data, and wait otherwise (#2015)

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<double>(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;

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?

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<int*>(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<double>(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;