diff --git a/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java b/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java index f40a38cb72..3feaee4b19 100644 --- a/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java +++ b/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); diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp index e8db9a59cc..e07b335f5f 100644 --- a/hal/src/main/native/athena/FRCDriverStation.cpp +++ b/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 currentCache{&caches[1]}; +static std::atomic 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 diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 3a8cefdcbf..c2ed275de7 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/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; } diff --git a/hal/src/main/native/cpp/jni/DriverStationJNI.cpp b/hal/src/main/native/cpp/jni/DriverStationJNI.cpp index f050f02129..7015067d49 100644 --- a/hal/src/main/native/cpp/jni/DriverStationJNI.cpp +++ b/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(); } /* diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h index ae68b654ca..d7d69644f6 100644 --- a/hal/src/main/native/include/hal/DriverStation.h +++ b/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); diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index c99493d2ab..fc91ad64d6 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/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 currentCache{&caches[1]}; +static std::atomic 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) { diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp index 535d20df65..d759e261f3 100644 --- a/wpilibc/src/main/native/cppcs/RobotBase.cpp +++ b/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()); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java index e4fb96244c..83b9daabd9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/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();