Switches DS to use Occur callback rather than internal netcomm semaphore (#510)

Allows us to control multithreaded access and spurious wakeups easier.
closes #509

Switches DS to use new waitForData functionality

Adds a few new functions

hal/include/HAL/DriverStation.h

@@ -106,7 +106,10 @@ double HAL_GetMatchTime(int32_t* status);
 
 #ifndef HAL_USE_LABVIEW
 
+void HAL_ReleaseDSMutex(void);
+bool HAL_IsNewControlData(void);
 void HAL_WaitForDSData(void);
+HAL_Bool HAL_WaitForDSDataTimeout(double timeout);
 void HAL_InitializeDriverStation(void);
 
 void HAL_ObserveUserProgramStarting(void);

hal/lib/athena/FRCDriverStation.cpp

@@ -5,6 +5,7 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include <atomic>
 #include <chrono>
 #include <cstdio>
 #include <cstdlib>
@@ -27,6 +28,7 @@ struct HAL_JoystickAxesInt {
 static priority_mutex msgMutex;
 static priority_condition_variable newDSDataAvailableCond;
 static priority_mutex newDSDataAvailableMutex;
+static int newDSDataAvailableCounter{0};
 
 extern "C" {
 int32_t HAL_SetErrorData(const char* errors, int32_t errorsLength,
@@ -244,17 +246,96 @@ void HAL_ObserveUserProgramTest(void) {
   FRC_NetworkCommunication_observeUserProgramTest();
 }
 
+bool HAL_IsNewControlData(void) {
+  // 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};
+  int currentCount = 0;
+  {
+    std::unique_lock<priority_mutex> lock(newDSDataAvailableMutex);
+    currentCount = newDSDataAvailableCounter;
+  }
+  if (lastCount == currentCount) return false;
+  lastCount = currentCount;
+  return true;
+}
+
 /**
  * Waits for the newest DS packet to arrive. Note that this is a blocking call.
  */
-void HAL_WaitForDSData(void) {
+void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); }
+
+/**
+ * Waits for the newest DS packet to arrive. If timeout is <= 0, this will wait
+ * forever. Otherwise, it will wait until either a new packet, or the timeout
+ * time has passed. Returns true on new data, false on timeout.
+ */
+HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
+  auto timeoutTime =
+      std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
   std::unique_lock<priority_mutex> lock(newDSDataAvailableMutex);
-  newDSDataAvailableCond.wait(lock);
+  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);
+    }
+  }
+  return true;
 }
 
-void HAL_InitializeDriverStation(void) {
-  //  Set our DS new data condition variable.
-  setNewDataSem(newDSDataAvailableCond.native_handle());
+// Internal NetComm function to set new packet callback
+extern int NetCommRPCProxy_SetOccurFuncPointer(
+    int32_t (*occurFunc)(uint32_t refNum));
+
+// Constant number to be used for our occur handle
+constexpr int32_t refNumber = 42;
+
+static int32_t newDataOccur(uint32_t refNum) {
+  // Since we could get other values, require our specific handle
+  // to signal our threads
+  if (refNum != refNumber) return 0;
+  std::lock_guard<priority_mutex> lock(newDSDataAvailableMutex);
+  // Nofify all threads
+  newDSDataAvailableCounter++;
+  newDSDataAvailableCond.notify_all();
+  return 0;
 }
 
+/*
+ * Call this to initialize the driver station communication. This will properly
+ * handle multiple calls. However note that this CANNOT be called from a library
+ * that interfaces with LabVIEW.
+ */
+void HAL_InitializeDriverStation(void) {
+  static std::atomic_bool initialized{false};
+  static priority_mutex initializeMutex;
+  // Initial check, as if it's true initialization has finished
+  if (initialized) return;
+
+  std::lock_guard<priority_mutex> lock(initializeMutex);
+  // Second check in case another thread was waiting
+  if (initialized) return;
+
+  // Set up the occur function internally with NetComm
+  NetCommRPCProxy_SetOccurFuncPointer(newDataOccur);
+  // Set up our occur reference number
+  setNewDataOccurRef(refNumber);
+
+  initialized = true;
+}
+
+/*
+ * Releases the DS Mutex to allow proper shutdown of any threads that are
+ * waiting on it.
+ */
+void HAL_ReleaseDSMutex(void) { newDataOccur(refNumber); }
+
 }  // extern "C"

hal/lib/athena/HAL.cpp

@@ -372,6 +372,5 @@ int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
 void NumericArrayResize() {}
 void RTSetCleanupProc() {}
 void EDVR_CreateReference() {}
-void Occur() {}
 
 }  // extern "C"

ni-libraries/lib/libFRC_NetworkCommunicationLV.so

@@ -1,2 +0,0 @@
-OUTPUT_FORMAT(elf32-littlearm)
-GROUP ( libFRC_NetworkCommunicationLV.so.17 )

ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17

@@ -1,2 +0,0 @@
-OUTPUT_FORMAT(elf32-littlearm)
-GROUP ( libFRC_NetworkCommunicationLV.so.17.0 )

ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17.0

@@ -1,2 +0,0 @@
-OUTPUT_FORMAT(elf32-littlearm)
-GROUP ( libFRC_NetworkCommunicationLV.so.17.0.0 )

wpilibc/athena/include/DriverStation.h

@@ -117,13 +117,6 @@ class DriverStation : public SensorBase, public RobotStateInterface {
   std::thread m_dsThread;
   std::atomic<bool> m_isRunning{false};
 
-  // WPILib WaitForData control variables
-  bool m_waitForDataPredicate = false;
-  std::condition_variable_any m_waitForDataCond;
-  priority_mutex m_waitForDataMutex;
-
-  mutable std::atomic<bool> m_newControlData{false};
-
   mutable priority_mutex m_joystickDataMutex;
 
   // Robot state status variables

wpilibc/athena/src/DriverStation.cpp

@@ -356,9 +356,7 @@ bool DriverStation::IsDSAttached() const {
  *
  * @return True if the control data has been updated since the last call.
  */
-bool DriverStation::IsNewControlData() const {
-  return m_newControlData.exchange(false);
-}
+bool DriverStation::IsNewControlData() const { return HAL_IsNewControlData(); }
 
 /**
  * Is the driver station attached to a Field Management System?
@@ -475,28 +473,7 @@ void DriverStation::WaitForData() { WaitForData(0); }
  * @return true if new data, otherwise false
  */
 bool DriverStation::WaitForData(double timeout) {
-#if defined(_MSC_VER) && _MSC_VER < 1900
-  auto timeoutTime = std::chrono::steady_clock::now() +
-                     std::chrono::duration<int64_t, std::nano>(
-                         static_cast<int64_t>(timeout * 1e9));
-#else
-  auto timeoutTime =
-      std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
-#endif
-
-  std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
-  while (!m_waitForDataPredicate) {
-    if (timeout > 0) {
-      auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
-      if (timedOut == std::cv_status::timeout) {
-        return false;
-      }
-    } else {
-      m_waitForDataCond.wait(lock);
-    }
-  }
-  m_waitForDataPredicate = false;
-  return true;
+  return static_cast<bool>(HAL_WaitForDSDataTimeout(timeout));
 }
 
 /**
@@ -628,15 +605,6 @@ void DriverStation::Run() {
   while (m_isRunning) {
     HAL_WaitForDSData();
     GetData();
-    // notify IsNewControlData variables
-    m_newControlData = true;
-
-    // notify WaitForData block
-    {
-      std::lock_guard<priority_mutex> lock(m_waitForDataMutex);
-      m_waitForDataPredicate = true;
-    }
-    m_waitForDataCond.notify_all();
 
     if (++period >= 4) {
       MotorSafetyHelper::CheckMotors();

wpilibj/src/athena/cpp/lib/HAL.cpp

@@ -276,9 +276,19 @@ Java_edu_wpi_first_wpilibj_hal_HAL_getJoystickAxisType(JNIEnv*, jclass,
   return HAL_GetJoystickAxisType(joystickNum, axis);
 }
 
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_HAL
+ * Method:    isNewControlData
+ * Signature: ()Z
+ */
+JNIEXPORT jboolean JNICALL 
+Java_edu_wpi_first_wpilibj_hal_HAL_isNewControlData(JNIEnv *, jclass) {
+  return static_cast<jboolean>(HAL_IsNewControlData());
+}
+
 /*
  * Class: edu_wpi_first_wpilibj_hal_HAL
- * Method:    HAL_waitForDSData
+ * Method:    waitForDSData
  * Signature: ()V
  */
 JNIEXPORT void JNICALL
@@ -286,6 +296,27 @@ Java_edu_wpi_first_wpilibj_hal_HAL_waitForDSData(JNIEnv* env, jclass) {
   HAL_WaitForDSData();
 }
 
+/*
+ * Class: edu_wpi_first_wpilibj_hal_HAL
+ * Method:    releaseDSMutex
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_wpilibj_hal_HAL_releaseDSMutex(JNIEnv* env, jclass) {
+  HAL_ReleaseDSMutex();
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_HAL
+ * Method:    waitForDSDataTimeout
+ * Signature: (D)Z
+ */
+JNIEXPORT jboolean JNICALL 
+Java_edu_wpi_first_wpilibj_hal_HAL_waitForDSDataTimeout(JNIEnv *, jclass, 
+                                                        jdouble timeout) {
+  return static_cast<jboolean>(HAL_WaitForDSDataTimeout(timeout));
+}
+
 /*
  * Class: edu_wpi_first_wpilibj_hal_HAL
  * Method:    HAL_GetMatchTime

wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -9,12 +9,6 @@ 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 java.util.concurrent.atomic.AtomicBoolean;
-
 import edu.wpi.first.wpilibj.hal.AllianceStationID;
 import edu.wpi.first.wpilibj.hal.ControlWord;
 import edu.wpi.first.wpilibj.hal.HAL;
@@ -92,17 +86,8 @@ public class DriverStation implements RobotState.Interface {
 
   // Internal Driver Station thread
   private Thread m_thread;
-
-  private final Lock m_dataMutex;
-  private final Condition m_dataCond;
-
   private volatile boolean m_threadKeepAlive = true;
 
-  // WPILib WaitForData control variables
-  private boolean m_waitForDataPredicate;
-
-  private AtomicBoolean m_newControlData;
-
   private final Object m_joystickMutex;
 
   // Robot state status variables
@@ -132,10 +117,7 @@ public class DriverStation implements RobotState.Interface {
    * variable.
    */
   private DriverStation() {
-    m_dataMutex = new ReentrantLock();
-    m_dataCond = m_dataMutex.newCondition();
     m_joystickMutex = new Object();
-    m_newControlData = new AtomicBoolean(false);
     for (int i = 0; i < kJoystickPorts; i++) {
       m_joystickButtons[i] = new HALJoystickButtons();
       m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
@@ -543,7 +525,7 @@ public class DriverStation implements RobotState.Interface {
    * @return True if the control data has been updated since the last call.
    */
   public boolean isNewControlData() {
-    return m_newControlData.getAndSet(false);
+    return HAL.isNewControlData();
   }
 
   @SuppressWarnings({"SummaryJavadoc", "JavadocMethod"})
@@ -648,40 +630,7 @@ public class DriverStation implements RobotState.Interface {
    * @return true if there is new data, otherwise false
    */
   public boolean waitForData(double timeout) {
-    long startTime = Utility.getFPGATime();
-    long timeoutMicros = (long) (timeout * 1000000);
-    m_dataMutex.lock();
-    try {
-      try {
-        while (!m_waitForDataPredicate) {
-          if (timeout > 0) {
-            long now = Utility.getFPGATime();
-            if (now < startTime + timeoutMicros) {
-              // We still have time to wait
-              boolean signaled = m_dataCond.await(startTime + timeoutMicros - now,
-                                                  TimeUnit.MICROSECONDS);
-              if (!signaled) {
-                // Return false if a timeout happened
-                return false;
-              }
-            } else {
-              // Time has elapsed.
-              return false;
-            }
-          } else {
-            m_dataCond.await();
-          }
-        }
-        m_waitForDataPredicate = false;
-        // Return true if we have received a proper signal
-        return true;
-      } catch (InterruptedException ex) {
-        // return false on a thread interrupt
-        return false;
-      }
-    } finally {
-      m_dataMutex.unlock();
-    }
+    return HAL.waitForDSDataTimeout(timeout);
   }
 
   /**
@@ -816,15 +765,6 @@ public class DriverStation implements RobotState.Interface {
     while (m_threadKeepAlive) {
       HAL.waitForDSData();
       getData();
-      m_dataMutex.lock();
-      try {
-        m_waitForDataPredicate = true;
-        m_dataCond.signalAll();
-      } finally {
-        m_dataMutex.unlock();
-      }
-      // notify isNewControlData variable
-      m_newControlData.set(true);
 
       if (++safetyCounter >= 4) {
         MotorSafetyHelper.checkMotors();

wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/HAL.java

@@ -83,6 +83,15 @@ public class HAL extends JNIWrapper {
     }
   }
 
+  @SuppressWarnings("JavadocMethod")
+  public static native boolean isNewControlData();
+
+  @SuppressWarnings("JavadocMethod")
+  public static native void releaseDSMutex();
+
+  @SuppressWarnings("JavadocMethod")
+  public static native boolean waitForDSDataTimeout(double timeout);
+
   public static int kMaxJoystickAxes = 12;
   public static int kMaxJoystickPOVs = 12;