[hal, wpilib] Load joystick values upon code initialization (#4950)

During HAL_Initialize, wait up to 100ms for a DS packet to be received. Then in RobotBase, right after calling HAL_Initialize, call each language's RefreshData function to force a high level DS update. If the DS is connected, will get joystick data. If there is no data, nothing different will happen, but in that case there's no joysticks anyway.

hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java

@@ -125,7 +125,7 @@ public class DriverStationJNI extends JNIWrapper {
 
   public static native int sendConsoleLine(String line);
 
-  public static native void refreshDSData();
+  public static native boolean refreshDSData();
 
   public static native void provideNewDataEventHandle(int handle);
 

hal/src/main/native/athena/FRCDriverStation.cpp

@@ -114,7 +114,7 @@ static HAL_ControlWord newestControlWord;
 static JoystickDataCache caches[3];
 static JoystickDataCache* currentRead = &caches[0];
 static JoystickDataCache* currentReadLocal = &caches[0];
-static std::atomic<JoystickDataCache*> currentCache{&caches[1]};
+static std::atomic<JoystickDataCache*> currentCache{nullptr};
 static JoystickDataCache* lastGiven = &caches[1];
 static JoystickDataCache* cacheToUpdate = &caches[2];
 
@@ -508,7 +508,7 @@ static void newDataOccur(uint32_t refNum) {
   }
 }
 
-void HAL_RefreshDSData(void) {
+HAL_Bool HAL_RefreshDSData(void) {
   HAL_ControlWord controlWord;
   std::memset(&controlWord, 0, sizeof(controlWord));
   FRC_NetworkCommunication_getControlWord(
@@ -526,6 +526,7 @@ void HAL_RefreshDSData(void) {
     std::scoped_lock tcpLock(tcpCacheMutex);
     tcpCache.CloneTo(&tcpCurrent);
   }
+  return prev != nullptr;
 }
 
 void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
@@ -551,4 +552,13 @@ void InitializeDriverStation() {
   setNewDataOccurRef(refNumber);
   FRC_NetworkCommunication_setNewTcpDataOccurRef(tcpRefNumber);
 }
+
+void WaitForInitialPacket() {
+  wpi::Event waitForInitEvent;
+  driverStation->newDataEvents.Add(waitForInitEvent.GetHandle());
+  bool timed_out = false;
+  wpi::WaitForObject(waitForInitEvent.GetHandle(), 0.1, &timed_out);
+  // Don't care what the result is, just want to give it a chance.
+  driverStation->newDataEvents.Remove(waitForInitEvent.GetHandle());
+}
 }  // namespace hal

hal/src/main/native/athena/HAL.cpp

@@ -50,6 +50,7 @@ using namespace hal;
 
 namespace hal {
 void InitializeDriverStation();
+void WaitForInitialPacket();
 namespace init {
 void InitializeHAL() {
   InitializeCTREPCM();
@@ -546,6 +547,8 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
     return rv;
   });
 
+  hal::WaitForInitialPacket();
+
   initialized = true;
   return true;
 }

hal/src/main/native/cpp/jni/DriverStationJNI.cpp

@@ -403,13 +403,13 @@ Java_edu_wpi_first_hal_DriverStationJNI_sendConsoleLine
 /*
  * Class:     edu_wpi_first_hal_DriverStationJNI
  * Method:    refreshDSData
- * Signature: ()V
+ * Signature: ()Z
  */
-JNIEXPORT void JNICALL
+JNIEXPORT jboolean JNICALL
 Java_edu_wpi_first_hal_DriverStationJNI_refreshDSData
   (JNIEnv*, jclass)
 {
-  HAL_RefreshDSData();
+  return HAL_RefreshDSData();
 }
 
 /*

hal/src/main/native/include/hal/DriverStation.h

@@ -209,7 +209,7 @@ HAL_Bool HAL_GetOutputsEnabled(void);
  */
 int32_t HAL_GetMatchInfo(HAL_MatchInfo* info);
 
-void HAL_RefreshDSData(void);
+HAL_Bool HAL_RefreshDSData(void);
 
 void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle);
 void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle);

hal/src/main/native/sim/DriverStation.cpp

@@ -75,7 +75,7 @@ static HAL_ControlWord newestControlWord;
 static JoystickDataCache caches[3];
 static JoystickDataCache* currentRead = &caches[0];
 static JoystickDataCache* currentReadLocal = &caches[0];
-static std::atomic<JoystickDataCache*> currentCache{&caches[1]};
+static std::atomic<JoystickDataCache*> currentCache{nullptr};
 static JoystickDataCache* lastGiven = &caches[1];
 static JoystickDataCache* cacheToUpdate = &caches[2];
 
@@ -318,9 +318,9 @@ void HAL_ObserveUserProgramTest(void) {
   // TODO
 }
 
-void HAL_RefreshDSData(void) {
+HAL_Bool HAL_RefreshDSData(void) {
   if (gShutdown) {
-    return;
+    return false;
   }
   HAL_ControlWord controlWord;
   std::memset(&controlWord, 0, sizeof(controlWord));
@@ -336,6 +336,7 @@ void HAL_RefreshDSData(void) {
     currentRead = prev;
   }
   newestControlWord = controlWord;
+  return prev != nullptr;
 }
 
 void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {

wpilibc/src/main/native/cppcs/RobotBase.cpp

@@ -41,6 +41,7 @@ int frc::RunHALInitialization() {
     std::puts("FATAL ERROR: HAL could not be initialized");
     return -1;
   }
+  DriverStation::RefreshData();
   HAL_Report(HALUsageReporting::kResourceType_Language,
              HALUsageReporting::kLanguage_CPlusPlus, 0, GetWPILibVersion());
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -395,6 +395,9 @@ public abstract class RobotBase implements AutoCloseable {
       throw new IllegalStateException("Failed to initialize. Terminating");
     }
 
+    // Force refresh DS data
+    DriverStation.refreshData();
+
     // Call a CameraServer JNI function to force OpenCV native library loading
     // Needed because all the OpenCV JNI functions don't have built in loading
     CameraServerJNI.enumerateSinks();