diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp
index d9e2a02285..190634ce99 100644
--- a/wpilibc/src/main/native/cpp/DriverStation.cpp
+++ b/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -589,7 +589,22 @@ void DriverStation::WaitForData() { WaitForData(0); }
  * @return true if new data, otherwise false
  */
 bool DriverStation::WaitForData(double timeout) {
-  return static_cast<bool>(HAL_WaitForDSDataTimeout(timeout));
+  auto timeoutTime =
+      std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+  std::unique_lock<wpi::mutex> lock(m_waitForDataMutex);
+  int currentCount = m_waitForDataCounter;
+  while (m_waitForDataCounter == currentCount) {
+    if (timeout > 0) {
+      auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
+      if (timedOut == std::cv_status::timeout) {
+        return false;
+      }
+    } else {
+      m_waitForDataCond.wait(lock);
+    }
+  }
+  return true;
 }
 
 /**
@@ -674,6 +689,11 @@ void DriverStation::GetData() {
   m_joystickButtons.swap(m_joystickButtonsCache);
   m_joystickDescriptor.swap(m_joystickDescriptorCache);
   m_matchInfo.swap(m_matchInfoCache);
+
+  std::lock_guard<wpi::mutex> waitLock(m_waitForDataMutex);
+  // Nofify all threads
+  m_waitForDataCounter++;
+  m_waitForDataCond.notify_all();
 }
 
 /**
@@ -682,6 +702,7 @@ void DriverStation::GetData() {
  * This is only called once the first time GetInstance() is called
  */
 DriverStation::DriverStation() {
+  m_waitForDataCounter = 0;
   m_joystickAxes = std::make_unique<HAL_JoystickAxes[]>(kJoystickPorts);
   m_joystickPOVs = std::make_unique<HAL_JoystickPOVs[]>(kJoystickPorts);
   m_joystickButtons = std::make_unique<HAL_JoystickButtons[]>(kJoystickPorts);
diff --git a/wpilibc/src/main/native/include/DriverStation.h b/wpilibc/src/main/native/include/DriverStation.h
index 1c5a56f94b..345bb7fc4e 100644
--- a/wpilibc/src/main/native/include/DriverStation.h
+++ b/wpilibc/src/main/native/include/DriverStation.h
@@ -15,6 +15,7 @@
 
 #include <HAL/DriverStation.h>
 #include <llvm/Twine.h>
+#include <support/condition_variable.h>
 #include <support/deprecated.h>
 #include <support/mutex.h>
 
@@ -155,6 +156,10 @@ class DriverStation : public ErrorBase, public RobotStateInterface {
   std::thread m_dsThread;
   std::atomic<bool> m_isRunning{false};
 
+  wpi::mutex m_waitForDataMutex;
+  wpi::condition_variable m_waitForDataCond;
+  int m_waitForDataCounter;
+
   mutable wpi::mutex m_cacheDataMutex;
 
   // Robot state status variables
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 b6a3c21bca..f4981079d9 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
@@ -8,6 +8,10 @@
 package edu.wpi.first.wpilibj;
 
 import java.nio.ByteBuffer;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
 
 import edu.wpi.first.wpilibj.hal.AllianceStationID;
 import edu.wpi.first.wpilibj.hal.ControlWord;
@@ -100,6 +104,10 @@ public class DriverStation implements RobotState.Interface {
 
   private final Object m_cacheDataMutex;
 
+  private final Lock m_waitForDataMutex;
+  private final Condition m_waitForDataCond;
+  private int m_waitForDataCount;
+
   // Robot state status variables
   private boolean m_userInDisabled = false;
   private boolean m_userInAutonomous = false;
@@ -127,6 +135,10 @@ public class DriverStation implements RobotState.Interface {
    * variable.
    */
   private DriverStation() {
+    m_waitForDataCount = 0;
+    m_waitForDataMutex = new ReentrantLock();
+    m_waitForDataCond = m_waitForDataMutex.newCondition();
+
     m_cacheDataMutex = new Object();
     for (int i = 0; i < kJoystickPorts; i++) {
       m_joystickButtons[i] = new HALJoystickButtons();
@@ -794,7 +806,38 @@ public class DriverStation implements RobotState.Interface {
    * @return true if there is new data, otherwise false
    */
   public boolean waitForData(double timeout) {
-    return HAL.waitForDSDataTimeout(timeout);
+    long startTime = RobotController.getFPGATime();
+    long timeoutMicros = (long) (timeout * 1000000);
+    m_waitForDataMutex.lock();
+    try {
+      int currentCount = m_waitForDataCount;
+      while (m_waitForDataCount == currentCount) {
+        if (timeout > 0) {
+          long now = RobotController.getFPGATime();
+          if (now < startTime + timeoutMicros) {
+            // We still have time to wait
+            boolean signaled = m_waitForDataCond.await(startTime + timeoutMicros - now,
+                                                TimeUnit.MICROSECONDS);
+            if (!signaled) {
+              // Return false if a timeout happened
+              return false;
+            }
+          } else {
+            // Time has elapsed.
+            return false;
+          }
+        } else {
+          m_waitForDataCond.await();
+        }
+      }
+      // Return true if we have received a proper signal
+      return true;
+    } catch (InterruptedException ex) {
+      // return false on a thread interrupt
+      return false;
+    } finally {
+      m_waitForDataMutex.unlock();
+    }
   }
 
   /**
@@ -914,6 +957,11 @@ public class DriverStation implements RobotState.Interface {
       m_matchInfo = m_matchInfoCache;
       m_matchInfoCache = currentInfo;
     }
+
+    m_waitForDataMutex.lock();
+    m_waitForDataCount++;
+    m_waitForDataCond.signalAll();
+    m_waitForDataMutex.unlock();
   }
 
   /**