[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.

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

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);
 

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<int*>(pthread_getspecific(lastCountKey));
-  if (!lastCountPtr) {
-    lastCountPtr = static_cast<int*>(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<double>(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<double>(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;
 }
 

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

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<bool> m_isRunning{false};
 
-  wpi::mutex m_waitForDataMutex;
+  mutable wpi::mutex m_waitForDataMutex;
   wpi::condition_variable m_waitForDataCond;
   int m_waitForDataCounter;
 

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<Integer> 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.
+   *
+   * <p>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.
    *
+   * <p>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) {