diff --git a/hal/include/HAL/DriverStation.h b/hal/include/HAL/DriverStation.h index e2097f236a..f5f64a72b4 100644 --- a/hal/include/HAL/DriverStation.h +++ b/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); diff --git a/hal/lib/athena/FRCDriverStation.cpp b/hal/lib/athena/FRCDriverStation.cpp index 28855dbccc..284db425bb 100644 --- a/hal/lib/athena/FRCDriverStation.cpp +++ b/hal/lib/athena/FRCDriverStation.cpp @@ -5,6 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +#include #include #include #include @@ -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 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(timeout); + std::unique_lock 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 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 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" diff --git a/hal/lib/athena/HAL.cpp b/hal/lib/athena/HAL.cpp index 708ca1b929..efb8e66d9d 100644 --- a/hal/lib/athena/HAL.cpp +++ b/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" diff --git a/ni-libraries/lib/libFRC_NetworkCommunicationLV.so b/ni-libraries/lib/libFRC_NetworkCommunicationLV.so deleted file mode 100644 index 5d13e85487..0000000000 --- a/ni-libraries/lib/libFRC_NetworkCommunicationLV.so +++ /dev/null @@ -1,2 +0,0 @@ -OUTPUT_FORMAT(elf32-littlearm) -GROUP ( libFRC_NetworkCommunicationLV.so.17 ) diff --git a/ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17 b/ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17 deleted file mode 100644 index 97fc85ba59..0000000000 --- a/ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17 +++ /dev/null @@ -1,2 +0,0 @@ -OUTPUT_FORMAT(elf32-littlearm) -GROUP ( libFRC_NetworkCommunicationLV.so.17.0 ) diff --git a/ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17.0 b/ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17.0 deleted file mode 100644 index ea1c26f84c..0000000000 --- a/ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17.0 +++ /dev/null @@ -1,2 +0,0 @@ -OUTPUT_FORMAT(elf32-littlearm) -GROUP ( libFRC_NetworkCommunicationLV.so.17.0.0 ) diff --git a/ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17.0.0 b/ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17.0.0 deleted file mode 100644 index 0848ee6f5d..0000000000 Binary files a/ni-libraries/lib/libFRC_NetworkCommunicationLV.so.17.0.0 and /dev/null differ diff --git a/wpilibc/athena/include/DriverStation.h b/wpilibc/athena/include/DriverStation.h index 6bd2ea6d9d..e5bdf5c8d9 100644 --- a/wpilibc/athena/include/DriverStation.h +++ b/wpilibc/athena/include/DriverStation.h @@ -117,13 +117,6 @@ class DriverStation : public SensorBase, public RobotStateInterface { std::thread m_dsThread; std::atomic m_isRunning{false}; - // WPILib WaitForData control variables - bool m_waitForDataPredicate = false; - std::condition_variable_any m_waitForDataCond; - priority_mutex m_waitForDataMutex; - - mutable std::atomic m_newControlData{false}; - mutable priority_mutex m_joystickDataMutex; // Robot state status variables diff --git a/wpilibc/athena/src/DriverStation.cpp b/wpilibc/athena/src/DriverStation.cpp index fd91d955ea..bcb9abba92 100644 --- a/wpilibc/athena/src/DriverStation.cpp +++ b/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( - static_cast(timeout * 1e9)); -#else - auto timeoutTime = - std::chrono::steady_clock::now() + std::chrono::duration(timeout); -#endif - - std::unique_lock 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(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 lock(m_waitForDataMutex); - m_waitForDataPredicate = true; - } - m_waitForDataCond.notify_all(); if (++period >= 4) { MotorSafetyHelper::CheckMotors(); diff --git a/wpilibj/src/athena/cpp/lib/HAL.cpp b/wpilibj/src/athena/cpp/lib/HAL.cpp index d7659783b7..506044ab4b 100644 --- a/wpilibj/src/athena/cpp/lib/HAL.cpp +++ b/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(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(HAL_WaitForDSDataTimeout(timeout)); +} + /* * Class: edu_wpi_first_wpilibj_hal_HAL * Method: HAL_GetMatchTime diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java index 6fcef6c7e7..12e4c2c020 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/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(); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/HAL.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/HAL.java index 1c57fdd5db..7ea759c1f8 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/HAL.java +++ b/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;