[hal, wpilib] New DS thread model and implementation (#3787)

The current DS thread model has some pretty major issues. It makes it difficult to know if all data is from the same remote packet, and if the data changes while the robot loop is running. Additionally, the DS thread is used for a few other things (MotorSafety and State Tracking for EducationalRobot). This also makes sim difficult, as user code has to wait for the thread to know it has new data.

This change completely rethinks how threading works in the driver station model.

First, the DS HAL system receives a new data callback, either from Netcomm or DriverStationSim. Inside the context of this callback, all the low latency data is read and put into a cache. Doing some investigation on the robot side, this is perfectly safe to do, and also ensures a ds packet will not be parsed before we finish reading the current packet data.

After all data is read, the cache is swapped with a 2nd buffer. This buffer just stores the data, none of the HAL DS calls read from this buffer. An event is then fired, stating there is new data ready to go.

Robot code calls HAL_UpdateDSData(). This swaps the 2nd buffer with a 3rd buffer, which always contains the current data. This data will not be updated until HAL_UpdateDSData is called again. Which solves the state problem.

The high level driver station classes have. an updateData() call, which calls HAL_UpdateDSData, and then update button state variables, then data log and update the NT FMS data table (Java also caches across the JNI boundary here, but that could trivially be removed). An extra event provider is provided, allowing other threads to know when this call has been completed.

IterativeRobotBase calls DS.updateData() at the beginning of each loop, and only once per loop. This means all commands will always have the same state.

All of this means there is no longer a DS thread. Everything happens synchronously. This means Sim and testing is easier, as you can just call DriverStationSim.NotifyNewData(), and then DriverStation.UpdateData(), and you can guarantee that all the DriverStation.*** data is up to date.

As for Motor Safety and Educational Robot State Handling, those can all be handled by their own threads. The Educational Thread only needs to run under EducationalRobot, and MotorSafety will only be started if there is a motor safety object enabled.

wpilibc/src/main/native/include/frc/DriverStation.h

@@ -7,6 +7,7 @@
 #include <string>
 
 #include <units/time.h>
+#include <wpi/Synchronization.h>
 
 namespace wpi::log {
 class DataLog;
@@ -215,17 +216,6 @@ class DriverStation final {
    */
   static bool IsDSAttached();
 
-  /**
-   * Has a new control packet from the driver station arrived since the last
-   * time this function was called?
-   *
-   * Warning: If you call this function from more than one place at the same
-   * time, you will not get the intended behavior.
-   *
-   * @return True if the control data has been updated since the last call.
-   */
-  static bool IsNewControlData();
-
   /**
    * Is the driver station attached to a Field Management System?
    *
@@ -289,41 +279,6 @@ class DriverStation final {
    */
   static int GetLocation();
 
-  /**
-   * Wait until a new packet comes from the driver station.
-   *
-   * This blocks on a semaphore, so the waiting is efficient.
-   *
-   * 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.
-   */
-  static void WaitForData();
-
-  /**
-   * 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
-   *
-   * This blocks on a semaphore, so the waiting is efficient.
-   *
-   * This is a good way to delay processing until there is new driver station
-   * data to act on.
-   *
-   * @param timeout Timeout
-   *
-   * @return true if new data, otherwise false
-   */
-  static bool WaitForData(units::second_t timeout);
-
   /**
    * Return the approximate match time.
    *
@@ -348,45 +303,10 @@ class DriverStation final {
    */
   static double GetBatteryVoltage();
 
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be
-   * executing for diagnostic purposes only.
-   *
-   * @param entering If true, starting disabled code; if false, leaving disabled
-   *                 code.
-   */
-  static void InDisabled(bool entering);
+  static void RefreshData();
 
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be
-   * executing for diagnostic purposes only.
-   *
-   * @param entering If true, starting autonomous code; if false, leaving
-   *                 autonomous code.
-   */
-  static void InAutonomous(bool entering);
-
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be
-   * executing for diagnostic purposes only.
-   *
-   * @param entering If true, starting teleop code; if false, leaving teleop
-   *                 code.
-   */
-  static void InTeleop(bool entering);
-
-  /**
-   * Only to be used to tell the Driver Station what code you claim to be
-   * executing for diagnostic purposes only.
-   *
-   * @param entering If true, starting test code; if false, leaving test code.
-   */
-  static void InTest(bool entering);
-
-  /**
-   * Forces WaitForData() to return immediately.
-   */
-  static void WakeupWaitForData();
+  static void ProvideRefreshedDataEventHandle(WPI_EventHandle handle);
+  static void RemoveRefreshedDataEventHandle(WPI_EventHandle handle);
 
   /**
    * Allows the user to specify whether they want joystick connection warnings

wpilibc/src/main/native/include/frc/RobotBase.h

@@ -174,14 +174,6 @@ class RobotBase {
    */
   bool IsTest() const;
 
-  /**
-   * Indicates if new data is available from the driver station.
-   *
-   * @return Has new data arrived over the network since the last time this
-   *         function was called?
-   */
-  bool IsNewDataAvailable() const;
-
   /**
    * Gets the ID of the main robot thread.
    */

wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <atomic>
+#include <thread>
+
+namespace frc::internal {
+class DriverStationModeThread {
+ public:
+  DriverStationModeThread();
+  ~DriverStationModeThread();
+
+  DriverStationModeThread(const DriverStationModeThread& other) = delete;
+  DriverStationModeThread(DriverStationModeThread&& other) = delete;
+  DriverStationModeThread& operator=(const DriverStationModeThread& other) =
+      delete;
+  DriverStationModeThread& operator=(DriverStationModeThread&& other) = delete;
+
+  void InAutonomous(bool entering);
+  void InDisabled(bool entering);
+  void InTeleop(bool entering);
+  void InTest(bool entering);
+
+ private:
+  std::atomic_bool m_keepAlive{false};
+  std::thread m_thread;
+  void Run();
+  bool m_userInDisabled{false};
+  bool m_userInAutonomous{false};
+  bool m_userInTeleop{false};
+  bool m_userInTest{false};
+};
+}  // namespace frc::internal