From 02c8d5c9db5c993056d397ae7a1dc18e2bb09546 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 29 Nov 2025 11:00:18 -0800 Subject: [PATCH] [hal] Revamp notifiers (#8424) This changes the HAL notifier interface to: - Use wpiutil signal objects. This means waiting is done through the `WPI_WaitObject` API instead of a dedicated function and allows for higher level code to simultaneously wait on notifiers and other events. - Interval timers are supported at the HAL layer - Handlers are now required to acknowledge notifications. This is invisible to users unless they're directly using the HAL API. - For interval timers, an overrun count is maintained to detect if the handler didn't acknowledge The underlying implementation still uses condition variables for the actual waiting. In basic testing using this approach seemed to be lower jitter than timerfd. Currently, the simulation and systemcore implementations are nearly identical except for a few additional sim hook bits. This could be refactored, but keeping them separate may make sense to keep the systemcore implementation easy to read and reason about, or if we ever choose to use a different underlying timer implementation on systemcore. The simulation side API is unchanged in form but does change in function--waiting for notifiers now only waits for currently running (or newly signaled) notifiers to acknowledge. To avoid a race condition in sim stepTiming, users of the low level API must make any alarm updates (especially for one-shot alarms) prior to acknowledging the previous alarm. The only current use of the interval timer feature is the `Notifier` class. The `TimedRobot` implementation still uses a single notifier and its own interval timing logic to ensure consistent callback order. Using separate notifiers for each user-level interval would substantially increase complexity. `Watchdog` also doesn't use the interval timer, as it's looking for an amount of time since the last `set` call rather than a recurring interval time. To reduce flicker, the sim GUI uses a fade out when a timeout goes from set to unset. This fixes tsan for wpilib and commands, and also fixes some spurious test failures. --- .github/workflows/sanitizers.yml | 2 +- .../org/wpilib/hardware/hal/NotifierJNI.java | 80 ++-- hal/src/main/native/cpp/jni/NotifierJNI.cpp | 75 +-- .../main/native/include/wpi/hal/Notifier.h | 102 +++-- .../include/wpi/hal/simulation/NotifierData.h | 5 +- hal/src/main/native/sim/Notifier.cpp | 432 +++++++++--------- hal/src/main/native/systemcore/Notifier.cpp | 263 ++++++----- .../python/hal/simulation/resethandles.cpp | 2 +- hal/src/main/python/semiwrap/Notifier.yml | 14 +- .../semiwrap/simulation/NotifierData.yml | 5 +- .../src/main/native/cpp/TimingGui.cpp | 53 ++- .../main/native/cpp/framework/TimedRobot.cpp | 30 +- .../src/main/native/cpp/system/Notifier.cpp | 92 ++-- .../src/main/native/cpp/system/Watchdog.cpp | 25 +- .../include/wpi/framework/TimedRobot.hpp | 2 +- .../native/include/wpi/system/Notifier.hpp | 33 +- wpilibc/src/main/python/semiwrap/Notifier.yml | 1 + .../main/python/wpilib/src/rpy/Notifier.cpp | 77 +--- .../src/main/python/wpilib/src/rpy/Notifier.h | 31 +- .../java/org/wpilib/framework/TimedRobot.java | 29 +- .../main/java/org/wpilib/system/Notifier.java | 80 +--- .../main/java/org/wpilib/system/Watchdog.java | 17 +- 22 files changed, 738 insertions(+), 712 deletions(-) diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index d9b726a6da..f79099f0a3 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -23,7 +23,7 @@ jobs: - name: tsan cmake-flags: "-DCMAKE_BUILD_TYPE=Tsan" ctest-env: "TSAN_OPTIONS=second_deadlock_stack=1" - ctest-flags: "-E 'cscore|cameraserver|wpilibc|commandsv2'" + ctest-flags: "-E 'cscore|cameraserver'" - name: ubsan cmake-flags: "-DCMAKE_BUILD_TYPE=Ubsan" ctest-env: "" diff --git a/hal/src/main/java/org/wpilib/hardware/hal/NotifierJNI.java b/hal/src/main/java/org/wpilib/hardware/hal/NotifierJNI.java index 96c1127000..eb3db6c458 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/NotifierJNI.java +++ b/hal/src/main/java/org/wpilib/hardware/hal/NotifierJNI.java @@ -14,23 +14,23 @@ package org.wpilib.hardware.hal; */ public class NotifierJNI extends JNIWrapper { /** - * Initializes a notifier. + * Creates a notifier. * - *

A notifier is an FPGA controller timer that triggers at requested intervals based on the - * FPGA time. This can be used to make precise control loops. + *

A notifier is an timer that alarms at an initial and (optionally) repeated intervals. This + * can be used to make precise control loops. * * @return the created notifier - * @see "HAL_InitializeNotifier" + * @see "HAL_CreateNotifier" */ - public static native int initializeNotifier(); + public static native int createNotifier(); /** * Sets the HAL notifier thread priority. * - *

The HAL notifier thread is responsible for managing the FPGA's notifier interrupt and waking - * up user's Notifiers when it's their time to run. Giving the HAL notifier thread real-time - * priority helps ensure the user's real-time Notifiers, if any, are notified to run in a timely - * manner. + *

The HAL notifier thread is responsible for managing the system's notifier interrupt and + * waking up user's Notifiers when it's their time to run. Giving the HAL notifier thread + * real-time priority helps ensure the user's real-time Notifiers, if any, are notified to run in + * a timely manner. * * @param realTime Set to true to set a real-time priority, false for standard priority. * @param priority Priority to set the thread to. For real-time, this is 1-99 with 99 being @@ -50,40 +50,37 @@ public class NotifierJNI extends JNIWrapper { public static native void setNotifierName(int notifierHandle, String name); /** - * Stops a notifier from running. + * Destroys a notifier. * - *

This will cause any call into waitForNotifierAlarm to return with time = 0. + *

Destruction wakes up any waiters. * * @param notifierHandle the notifier handle - * @see "HAL_StopNotifier" + * @see "HAL_DestroyNotifier" */ - public static native void stopNotifier(int notifierHandle); + public static native void destroyNotifier(int notifierHandle); /** - * Cleans a notifier. + * Updates the initial and interval alarm times for a notifier. * - *

Note this also stops a notifier if it is already running. + *

The alarmTime is an absolute time (using the WPI now() time base) if absolute is true, or + * relative to the current time if absolute is false. + * + *

If intervalTime is non-zero, the notifier will alarm periodically following alarmTime at the + * given interval. + * + *

If an absolute alarmTime is in the past, the notifier will alarm immediately. * * @param notifierHandle the notifier handle - * @see "HAL_CleanNotifier" + * @param alarmTime the first alarm time (in microseconds) + * @param intervalTime the periodic interval time (in microseconds) + * @param absolute true if the alarm time is absolute + * @see "HAL_SetNotifierAlarm" */ - public static native void cleanNotifier(int notifierHandle); + public static native void setNotifierAlarm( + int notifierHandle, long alarmTime, long intervalTime, boolean absolute); /** - * Updates the trigger time for a notifier. - * - *

Note that this time is an absolute time relative to getFPGATime() - * - * @param notifierHandle the notifier handle - * @param triggerTime the updated trigger time - * @see "HAL_UpdateNotifierAlarm" - */ - public static native void updateNotifierAlarm(int notifierHandle, long triggerTime); - - /** - * Cancels the next notifier alarm. - * - *

This does not cause waitForNotifierAlarm to return. + * Cancels all future notifier alarms for a notifier. * * @param notifierHandle the notifier handle * @see "HAL_CancelNotifierAlarm" @@ -91,16 +88,23 @@ public class NotifierJNI extends JNIWrapper { public static native void cancelNotifierAlarm(int notifierHandle); /** - * Waits for the next alarm for the specific notifier. - * - *

This is a blocking call until either the time elapses or stopNotifier gets called. If the - * latter occurs, this function will return zero and any loops using this function should exit. - * Failing to do so can lead to use-after-frees. + * Indicates the notifier alarm has been serviced. This must be called before waiting for the next + * alarm. * * @param notifierHandle the notifier handle - * @return the FPGA time the notifier returned */ - public static native long waitForNotifierAlarm(int notifierHandle); + public static native void acknowledgeNotifierAlarm(int notifierHandle); + + /** + * Gets the overrun count for a notifier. + * + *

An overrun occurs when a notifier's alarm is not serviced before the next scheduled alarm + * time. + * + * @param notifierHandle the notifier handle + * @return overrun count + */ + public static native int getNotifierOverrun(int notifierHandle); /** Utility class. */ private NotifierJNI() {} diff --git a/hal/src/main/native/cpp/jni/NotifierJNI.cpp b/hal/src/main/native/cpp/jni/NotifierJNI.cpp index 32fac3d6f0..eaf03e160a 100644 --- a/hal/src/main/native/cpp/jni/NotifierJNI.cpp +++ b/hal/src/main/native/cpp/jni/NotifierJNI.cpp @@ -11,6 +11,7 @@ #include "org_wpilib_hardware_hal_NotifierJNI.h" #include "wpi/hal/Notifier.h" #include "wpi/util/jni_util.hpp" +#include "wpi/util/string.h" using namespace wpi::hal; @@ -18,15 +19,15 @@ extern "C" { /* * Class: org_wpilib_hardware_hal_NotifierJNI - * Method: initializeNotifier + * Method: createNotifier * Signature: ()I */ JNIEXPORT jint JNICALL -Java_org_wpilib_hardware_hal_NotifierJNI_initializeNotifier +Java_org_wpilib_hardware_hal_NotifierJNI_createNotifier (JNIEnv* env, jclass) { int32_t status = 0; - HAL_NotifierHandle notifierHandle = HAL_InitializeNotifier(&status); + HAL_NotifierHandle notifierHandle = HAL_CreateNotifier(&status); if (notifierHandle <= 0 || !CheckStatusForceThrow(env, status)) { return 0; // something went wrong in HAL @@ -58,51 +59,40 @@ Java_org_wpilib_hardware_hal_NotifierJNI_setNotifierName (JNIEnv* env, jclass cls, jint notifierHandle, jstring name) { int32_t status = 0; - HAL_SetNotifierName((HAL_NotifierHandle)notifierHandle, - wpi::util::java::JStringRef{env, name}.c_str(), &status); + wpi::util::java::JStringRef jname{env, name}; + WPI_String wpiName = wpi::util::make_string(jname); + HAL_SetNotifierName((HAL_NotifierHandle)notifierHandle, &wpiName, &status); CheckStatus(env, status); } /* * Class: org_wpilib_hardware_hal_NotifierJNI - * Method: stopNotifier + * Method: destroyNotifier * Signature: (I)V */ JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_NotifierJNI_stopNotifier - (JNIEnv* env, jclass cls, jint notifierHandle) -{ - int32_t status = 0; - HAL_StopNotifier((HAL_NotifierHandle)notifierHandle, &status); - CheckStatus(env, status); -} - -/* - * Class: org_wpilib_hardware_hal_NotifierJNI - * Method: cleanNotifier - * Signature: (I)V - */ -JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_NotifierJNI_cleanNotifier +Java_org_wpilib_hardware_hal_NotifierJNI_destroyNotifier (JNIEnv* env, jclass, jint notifierHandle) { if (notifierHandle != HAL_kInvalidHandle) { - HAL_CleanNotifier((HAL_NotifierHandle)notifierHandle); + HAL_DestroyNotifier((HAL_NotifierHandle)notifierHandle); } } /* * Class: org_wpilib_hardware_hal_NotifierJNI - * Method: updateNotifierAlarm - * Signature: (IJ)V + * Method: setNotifierAlarm + * Signature: (IJJZ)V */ JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_NotifierJNI_updateNotifierAlarm - (JNIEnv* env, jclass cls, jint notifierHandle, jlong triggerTime) +Java_org_wpilib_hardware_hal_NotifierJNI_setNotifierAlarm + (JNIEnv* env, jclass cls, jint notifierHandle, jlong alarmTime, + jlong intervalTime, jboolean absolute) { int32_t status = 0; - HAL_UpdateNotifierAlarm((HAL_NotifierHandle)notifierHandle, - static_cast(triggerTime), &status); + HAL_SetNotifierAlarm((HAL_NotifierHandle)notifierHandle, + static_cast(alarmTime), + static_cast(intervalTime), absolute, &status); CheckStatus(env, status); } @@ -122,20 +112,35 @@ Java_org_wpilib_hardware_hal_NotifierJNI_cancelNotifierAlarm /* * Class: org_wpilib_hardware_hal_NotifierJNI - * Method: waitForNotifierAlarm - * Signature: (I)J + * Method: acknowledgeNotifierAlarm + * Signature: (I)V */ -JNIEXPORT jlong JNICALL -Java_org_wpilib_hardware_hal_NotifierJNI_waitForNotifierAlarm +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_NotifierJNI_acknowledgeNotifierAlarm (JNIEnv* env, jclass cls, jint notifierHandle) { int32_t status = 0; - uint64_t time = - HAL_WaitForNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status); + HAL_AcknowledgeNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status); + + CheckStatus(env, status); +} + +/* + * Class: org_wpilib_hardware_hal_NotifierJNI + * Method: getNotifierOverrun + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_NotifierJNI_getNotifierOverrun + (JNIEnv* env, jclass cls, jint notifierHandle) +{ + int32_t status = 0; + int32_t count = + HAL_GetNotifierOverrun((HAL_NotifierHandle)notifierHandle, &status); CheckStatus(env, status); - return (jlong)time; + return (jint)count; } } // extern "C" diff --git a/hal/src/main/native/include/wpi/hal/Notifier.h b/hal/src/main/native/include/wpi/hal/Notifier.h index ebed422a41..98ceb1187d 100644 --- a/hal/src/main/native/include/wpi/hal/Notifier.h +++ b/hal/src/main/native/include/wpi/hal/Notifier.h @@ -6,8 +6,12 @@ #include +#ifdef __cplusplus +#include +#endif + #include "wpi/hal/Types.h" -#include "wpi/util/nodiscard.h" +#include "wpi/util/string.h" /** * @defgroup hal_notifier Notifier Functions @@ -20,20 +24,20 @@ extern "C" { #endif /** - * Initializes a notifier. + * Creates a notifier. * - * A notifier is an FPGA controller timer that triggers at requested intervals - * based on the FPGA time. This can be used to make precise control loops. + * A notifier is an timer that alarms at an initial and (optionally) repeated + * intervals. This can be used to make precise control loops. * * @param[out] status Error status variable. 0 on success. * @return the created notifier */ -HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status); +HAL_NotifierHandle HAL_CreateNotifier(int32_t* status); /** * Sets the HAL notifier thread priority. * - * The HAL notifier thread is responsible for managing the FPGA's notifier + * The HAL notifier thread is responsible for managing the system's notifier * interrupt and waking up user's Notifiers when it's their time to run. * Giving the HAL notifier thread real-time priority helps ensure the user's * real-time Notifiers, if any, are notified to run in a timely manner. @@ -56,45 +60,41 @@ HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority, * @param[in] name name * @param[out] status Error status variable. 0 on success. */ -void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name, - int32_t* status); +void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, + const struct WPI_String* name, int32_t* status); /** - * Stops a notifier from running. + * Destroys a notifier. * - * This will cause any call into HAL_WaitForNotifierAlarm to return with time = - * 0. - * - * @param[in] notifierHandle the notifier handle - * @param[out] status Error status variable. 0 on success. - */ -void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status); - -/** - * Cleans a notifier. - * - * Note this also stops a notifier if it is already running. + * Destruction wakes up any waiters. * * @param[in] notifierHandle the notifier handle */ -void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle); +void HAL_DestroyNotifier(HAL_NotifierHandle notifierHandle); /** - * Updates the trigger time for a notifier. + * Updates the initial and interval alarm times for a notifier. * - * Note that this time is an absolute time relative to HAL_GetFPGATime() + * The alarmTime is an absolute time (using the WPI_Now() time base) if + * absolute is true, or relative to the current time if absolute is false. + * + * If intervalTime is non-zero, the notifier will alarm periodically following + * alarmTime at the given interval. + * + * If an absolute alarmTime is in the past, the notifier will alarm immediately. * * @param[in] notifierHandle the notifier handle - * @param[in] triggerTime the updated trigger time + * @param[in] alarmTime the first alarm time (in microseconds) + * @param[in] intervalTime the periodic interval time (in microseconds) + * @param[in] absolute true if the alarm time is absolute * @param[out] status Error status variable. 0 on success. */ -void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle, - uint64_t triggerTime, int32_t* status); +void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime, + uint64_t intervalTime, HAL_Bool absolute, + int32_t* status); /** - * Cancels the next notifier alarm. - * - * This does not cause HAL_WaitForNotifierAlarm to return. + * Cancels all future notifier alarms for a notifier. * * @param[in] notifierHandle the notifier handle * @param[out] status Error status variable. 0 on success. @@ -103,22 +103,44 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status); /** - * Waits for the next alarm for the specific notifier. - * - * This is a blocking call until either the time elapses or HAL_StopNotifier - * gets called. If the latter occurs, this function will return zero and any - * loops using this function should exit. Failing to do so can lead to - * use-after-frees. + * Indicates the notifier alarm has been serviced. This must be called before + * waiting for the next alarm. * * @param[in] notifierHandle the notifier handle - * @param[out] status Error status variable. 0 on success. - * @return the FPGA time the notifier returned + * @param[out] status Error status variable. 0 on success. */ -WPI_NODISCARD -uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle, +void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status); +/** + * Gets the overrun count for a notifier. + * + * An overrun occurs when a notifier's alarm is not serviced before the next + * scheduled alarm time. + * + * @param[in] notifierHandle the notifier handle + * @param[out] status Error status variable. 0 on success. + * @return overrun count + */ +int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle, + int32_t* status); + #ifdef __cplusplus } // extern "C" #endif /** @} */ + +#ifdef __cplusplus +/** + * Sets the name of a notifier. + * + * @param[in] notifierHandle the notifier handle + * @param[in] name name + * @param[out] status Error status variable. 0 on success. + */ +inline void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, + std::string_view name, int32_t* status) { + WPI_String nameStr = wpi::util::make_string(name); + HAL_SetNotifierName(notifierHandle, &nameStr, status); +} +#endif diff --git a/hal/src/main/native/include/wpi/hal/simulation/NotifierData.h b/hal/src/main/native/include/wpi/hal/simulation/NotifierData.h index f05542bdb9..434430b09a 100644 --- a/hal/src/main/native/include/wpi/hal/simulation/NotifierData.h +++ b/hal/src/main/native/include/wpi/hal/simulation/NotifierData.h @@ -13,8 +13,9 @@ extern "C" { struct HALSIM_NotifierInfo { HAL_NotifierHandle handle; char name[64]; - uint64_t timeout; - HAL_Bool waitTimeValid; + uint64_t alarmTime; + uint64_t intervalTime; + int32_t overrunCount; }; uint64_t HALSIM_GetNextNotifierTimeout(void); diff --git a/hal/src/main/native/sim/Notifier.cpp b/hal/src/main/native/sim/Notifier.cpp index 2b0c389975..4b1be980c5 100644 --- a/hal/src/main/native/sim/Notifier.cpp +++ b/hal/src/main/native/sim/Notifier.cpp @@ -4,317 +4,330 @@ #include "wpi/hal/Notifier.h" +#include + #include #include -#include #include +#include #include #include #include +#include #include "HALInitializer.h" #include "NotifierInternal.h" #include "wpi/hal/Errors.h" #include "wpi/hal/HALBase.h" -#include "wpi/hal/cpp/fpga_clock.h" +#include "wpi/hal/Threads.h" +#include "wpi/hal/Types.h" #include "wpi/hal/handles/UnlimitedHandleResource.h" #include "wpi/hal/simulation/NotifierData.h" +#include "wpi/util/SafeThread.hpp" #include "wpi/util/SmallVector.hpp" #include "wpi/util/StringExtras.hpp" -#include "wpi/util/condition_variable.hpp" -#include "wpi/util/mutex.hpp" +#include "wpi/util/Synchronization.h" +#include "wpi/util/priority_queue.hpp" namespace { struct Notifier { std::string name; - uint64_t waitTime = UINT64_MAX; - bool active = true; - bool waitTimeValid = false; // True if waitTime is set and in the future - bool waitingForAlarm = false; // True if in HAL_WaitForNotifierAlarm() - uint64_t waitCount = 0; // Counts calls to HAL_WaitForNotifierAlarm() - wpi::util::mutex mutex; - wpi::util::condition_variable cond; + std::atomic alarmTime = UINT64_MAX; + uint64_t intervalTime = 0; + std::atomic userOverrunCount = 0; + int32_t overrunCount = 0; + std::atomic_flag handlerSignaled{}; }; } // namespace using namespace wpi::hal; -static wpi::util::mutex notifiersWaiterMutex; -static wpi::util::condition_variable notifiersWaiterCond; - -class NotifierHandleContainer - : public UnlimitedHandleResource { +class NotifierThread : public wpi::util::SafeThread { public: - ~NotifierHandleContainer() { - ForEach([](HAL_NotifierHandle handle, Notifier* notifier) { - { - std::scoped_lock lock(notifier->mutex); - notifier->active = false; - notifier->waitTimeValid = false; - } - notifier->cond.notify_all(); // wake up any waiting threads - }); - notifiersWaiterCond.notify_all(); - } + void Main() override; + + void ProcessAlarms(wpi::util::SmallVectorImpl* signaled); + + bool m_paused = false; + + UnlimitedHandleResource + m_handles; + + struct Alarm { + HAL_NotifierHandle handle; + std::shared_ptr notifier; + bool operator==(const Alarm& rhs) const { return handle == rhs.handle; } + bool operator>(const Alarm& rhs) const { + return notifier->alarmTime > rhs.notifier->alarmTime; + } + }; + wpi::util::priority_queue, std::greater> + m_alarmQueue; }; -static NotifierHandleContainer* notifierHandles; -static std::atomic notifiersPaused{false}; +class NotifierInstance { + public: + NotifierInstance() { owner.Start(); } + wpi::util::SafeThreadOwner owner; +}; -namespace wpi::hal { -namespace init { +static NotifierInstance* notifierInstance; + +namespace wpi::hal::init { void InitializeNotifier() { - static NotifierHandleContainer nH; - notifierHandles = &nH; + static NotifierInstance n; + notifierInstance = &n; } -} // namespace init +} // namespace wpi::hal::init -void PauseNotifiers() { - notifiersPaused = true; -} - -void ResumeNotifiers() { - notifiersPaused = false; - WakeupNotifiers(); -} - -void WakeupNotifiers() { - notifierHandles->ForEach([](HAL_NotifierHandle handle, Notifier* notifier) { - notifier->cond.notify_all(); - }); -} - -void WaitNotifiers() { - std::unique_lock ulock(notifiersWaiterMutex); - wpi::util::SmallVector waiters; - - // Wait for all Notifiers to hit HAL_WaitForNotifierAlarm() - notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) { - std::scoped_lock lock(notifier->mutex); - if (notifier->active && !notifier->waitingForAlarm) { - waiters.emplace_back(handle); +void NotifierThread::Main() { + std::unique_lock lock(m_mutex); + while (m_active) { + if (m_paused || m_alarmQueue.empty()) { + // No alarms, wait indefinitely + m_cond.wait(lock); + continue; } - }); - for (;;) { - int count = 0; - int end = waiters.size(); - while (count < end) { - auto& it = waiters[count]; - if (auto notifier = notifierHandles->Get(it)) { - std::scoped_lock lock(notifier->mutex); - if (notifier->active && !notifier->waitingForAlarm) { - ++count; - continue; - } - } - // No longer need to wait for it, put at end so it can be erased - std::swap(it, waiters[--end]); + + // Wait until next alarm + const Alarm& alarm = m_alarmQueue.top(); + int32_t status = 0; + uint64_t curTime = HAL_GetFPGATime(&status); + if (alarm.notifier->alarmTime > curTime) { + m_cond.wait_for( + lock, std::chrono::microseconds{alarm.notifier->alarmTime - curTime}); } - if (count == 0) { + if (!m_active) { break; } - waiters.resize(count); - notifiersWaiterCond.wait_for(ulock, std::chrono::duration(1)); + + // Check paused again as we may have been paused while waiting + if (m_paused) { + continue; + } + + ProcessAlarms(nullptr); } } -void WakeupWaitNotifiers() { - std::unique_lock ulock(notifiersWaiterMutex); +void NotifierThread::ProcessAlarms( + wpi::util::SmallVectorImpl* signaled) { int32_t status = 0; uint64_t curTime = HAL_GetFPGATime(&status); - wpi::util::SmallVector, 8> waiters; - // Wake up Notifiers that have expired timeouts - notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) { - std::scoped_lock lock(notifier->mutex); + // Process alarms + while (!m_alarmQueue.empty() && + m_alarmQueue.top().notifier->alarmTime <= curTime) { + Alarm alarm = m_alarmQueue.pop(); + HAL_NotifierHandle handle = alarm.handle; + Notifier& notifier = *alarm.notifier; - // Only wait for the Notifier if it has a valid timeout that's expired - if (notifier->active && notifier->waitTimeValid && - curTime >= notifier->waitTime) { - waiters.emplace_back(handle, notifier->waitCount); - notifier->cond.notify_all(); - } - }); - for (;;) { - int count = 0; - int end = waiters.size(); - while (count < end) { - auto& it = waiters[count]; - if (auto notifier = notifierHandles->Get(it.first)) { - std::scoped_lock lock(notifier->mutex); - - // waitCount is used here instead of waitingForAlarm because we want to - // wait until HAL_WaitForNotifierAlarm() is exited, then reentered - if (notifier->active && notifier->waitCount == it.second) { - ++count; - continue; - } + if (notifier.intervalTime > 0) { + // Schedule next alarm + notifier.alarmTime += notifier.intervalTime; + if (curTime >= notifier.alarmTime) { + // We missed at least one interval + int32_t missed = static_cast((curTime - notifier.alarmTime) / + notifier.intervalTime) + + 1; + notifier.overrunCount += missed; + notifier.alarmTime += + missed * notifier.intervalTime; // Skip missed intervals } - // No longer need to wait for it, put at end so it can be erased - it.swap(waiters[--end]); + // Reinsert into queue + m_alarmQueue.push(std::move(alarm)); + } else { + // Disable one-shot alarm + notifier.alarmTime = UINT64_MAX; } - if (count == 0) { - break; + + // If the last call was acknowledged, signal the handler + if (!notifier.handlerSignaled.test_and_set()) { + if (signaled) { + signaled->emplace_back(handle); + } + // copy the overrun count for the handler to read, reset the local count + notifier.userOverrunCount = notifier.overrunCount; + notifier.overrunCount = 0; + wpi::util::SetSignalObject(handle); } - waiters.resize(count); - notifiersWaiterCond.wait_for(ulock, std::chrono::duration(1)); } } -} // namespace wpi::hal + +void wpi::hal::PauseNotifiers() { + auto thr = notifierInstance->owner.GetThread(); + thr->m_paused = true; +} + +void wpi::hal::ResumeNotifiers() { + auto thr = notifierInstance->owner.GetThread(); + thr->m_paused = false; + thr->m_cond.notify_all(); +} + +void wpi::hal::WakeupNotifiers() { + auto thr = notifierInstance->owner.GetThread(); + thr->ProcessAlarms(nullptr); +} + +static void DoWaitNotifiers( + wpi::util::detail::SafeThreadProxy& thr, + wpi::util::SmallVectorImpl& signaled) { + // Wait for signaled notifiers to acknowledge their last alarm + for (;;) { + signaled.erase(std::remove_if(signaled.begin(), signaled.end(), + [&](HAL_NotifierHandle handle) { + auto notifier = thr->m_handles.Get(handle); + return !notifier || + !notifier->handlerSignaled.test(); + }), + signaled.end()); + if (signaled.empty()) { + break; + } + thr->m_cond.wait_for(thr.GetLock(), std::chrono::milliseconds{1}); + } +} + +void wpi::hal::WaitNotifiers() { + auto thr = notifierInstance->owner.GetThread(); + + wpi::util::SmallVector signaled; + thr->m_handles.ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) { + if (notifier->handlerSignaled.test()) { + signaled.emplace_back(handle); + } + }); + DoWaitNotifiers(thr, signaled); +} + +void wpi::hal::WakeupWaitNotifiers() { + auto thr = notifierInstance->owner.GetThread(); + + wpi::util::SmallVector signaled; + thr->ProcessAlarms(&signaled); + DoWaitNotifiers(thr, signaled); +} extern "C" { -HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) { +HAL_NotifierHandle HAL_CreateNotifier(int32_t* status) { wpi::hal::init::CheckInit(); std::shared_ptr notifier = std::make_shared(); - HAL_NotifierHandle handle = notifierHandles->Allocate(notifier); + HAL_NotifierHandle handle = + notifierInstance->owner.GetThread()->m_handles.Allocate(notifier); if (handle == HAL_kInvalidHandle) { *status = HAL_HANDLE_ERROR; return HAL_kInvalidHandle; } + wpi::util::CreateSignalObject(handle); return handle; } HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority, int32_t* status) { - return true; + auto native = notifierInstance->owner.GetNativeThreadHandle(); + return HAL_SetThreadPriority(&native, realTime, priority, status); } -void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name, - int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); +void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, + const WPI_String* name, int32_t* status) { + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Get(notifierHandle); if (!notifier) { return; } - std::scoped_lock lock(notifier->mutex); - notifier->name = name; + notifier->name = wpi::util::to_string_view(name); } -void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); +void HAL_DestroyNotifier(HAL_NotifierHandle notifierHandle) { + wpi::util::DestroySignalObject(notifierHandle); + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Free(notifierHandle); + thr->m_alarmQueue.remove({notifierHandle, notifier}); +} + +void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime, + uint64_t intervalTime, HAL_Bool absolute, + int32_t* status) { + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Get(notifierHandle); if (!notifier) { return; } - { - std::scoped_lock lock(notifier->mutex); - notifier->active = false; - notifier->waitTimeValid = false; - } - notifier->cond.notify_all(); -} - -void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle) { - auto notifier = notifierHandles->Free(notifierHandle); - if (!notifier) { - return; + if (!absolute) { + alarmTime += HAL_GetFPGATime(status); } - // Just in case HAL_StopNotifier() wasn't called... - { - std::scoped_lock lock(notifier->mutex); - notifier->active = false; - notifier->waitTimeValid = false; + uint64_t prevWakeup = UINT64_MAX; + if (!thr->m_alarmQueue.empty()) { + prevWakeup = thr->m_alarmQueue.top().notifier->alarmTime; + thr->m_alarmQueue.remove({notifierHandle, notifier}); } - notifier->cond.notify_all(); -} + notifier->alarmTime = alarmTime; + notifier->intervalTime = intervalTime; + notifier->overrunCount = 0; + thr->m_alarmQueue.push({notifierHandle, notifier}); -void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle, - uint64_t triggerTime, int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); - if (!notifier) { - return; + // wake up notifier thread if needed + if (alarmTime < prevWakeup) { + thr->m_cond.notify_all(); } - - { - std::scoped_lock lock(notifier->mutex); - notifier->waitTime = triggerTime; - notifier->waitTimeValid = (triggerTime != UINT64_MAX); - } - - // We wake up any waiters to change how long they're sleeping for - notifier->cond.notify_all(); } void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Get(notifierHandle); if (!notifier) { return; } - { - std::scoped_lock lock(notifier->mutex); - notifier->waitTimeValid = false; - } + thr->m_alarmQueue.remove({notifierHandle, notifier}); + notifier->alarmTime = UINT64_MAX; } -uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle, +void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); + auto notifier = + notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle); if (!notifier) { - return 0; + return; } + notifier->handlerSignaled.clear(); +} - std::unique_lock ulock(notifiersWaiterMutex); - std::unique_lock lock(notifier->mutex); - notifier->waitingForAlarm = true; - ++notifier->waitCount; - ulock.unlock(); - notifiersWaiterCond.notify_all(); - while (notifier->active) { - uint64_t curTime = HAL_GetFPGATime(status); - if (notifier->waitTimeValid && curTime >= notifier->waitTime) { - notifier->waitTimeValid = false; - notifier->waitingForAlarm = false; - return curTime; - } - - double waitDuration; - if (!notifier->waitTimeValid || notifiersPaused) { - // If not running, wait 1000 seconds - waitDuration = 1000.0; - } else { - waitDuration = (notifier->waitTime - curTime) * 1e-6; - } - - notifier->cond.wait_for(lock, std::chrono::duration(waitDuration)); +int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle, + int32_t* status) { + auto notifier = + notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle); + if (!notifier) { + return -1; } - notifier->waitingForAlarm = false; - return 0; + return notifier->userOverrunCount; } uint64_t HALSIM_GetNextNotifierTimeout(void) { - uint64_t timeout = UINT64_MAX; - notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) { - std::scoped_lock lock(notifier->mutex); - if (notifier->active && notifier->waitTimeValid && - timeout > notifier->waitTime) { - timeout = notifier->waitTime; - } - }); - return timeout; + auto thr = notifierInstance->owner.GetThread(); + if (thr->m_alarmQueue.empty()) { + return UINT64_MAX; + } + return thr->m_alarmQueue.top().notifier->alarmTime; } int32_t HALSIM_GetNumNotifiers(void) { + auto thr = notifierInstance->owner.GetThread(); int32_t count = 0; - notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) { - std::scoped_lock lock(notifier->mutex); - if (notifier->active) { - ++count; - } - }); + thr->m_handles.ForEach([&](auto, auto) { ++count; }); return count; } int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) { + auto thr = notifierInstance->owner.GetThread(); int32_t num = 0; - notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) { - std::scoped_lock lock(notifier->mutex); - if (!notifier->active) { - return; - } + thr->m_handles.ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) { if (num < size) { arr[num].handle = handle; if (notifier->name.empty()) { @@ -326,8 +339,9 @@ int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) { sizeof(arr[num].name) - 1); arr[num].name[sizeof(arr[num].name) - 1] = '\0'; } - arr[num].timeout = notifier->waitTime; - arr[num].waitTimeValid = notifier->waitTimeValid; + arr[num].alarmTime = notifier->alarmTime; + arr[num].intervalTime = notifier->intervalTime; + arr[num].overrunCount = notifier->overrunCount; } ++num; }); diff --git a/hal/src/main/native/systemcore/Notifier.cpp b/hal/src/main/native/systemcore/Notifier.cpp index 9288aad5ee..7bb9768ff4 100644 --- a/hal/src/main/native/systemcore/Notifier.cpp +++ b/hal/src/main/native/systemcore/Notifier.cpp @@ -6,182 +6,233 @@ #include #include -#include -#include +#include #include #include #include +#include #include "HALInitializer.h" #include "wpi/hal/Errors.h" #include "wpi/hal/HALBase.h" -#include "wpi/hal/cpp/fpga_clock.h" +#include "wpi/hal/Threads.h" +#include "wpi/hal/Types.h" #include "wpi/hal/handles/UnlimitedHandleResource.h" -#include "wpi/hal/simulation/NotifierData.h" -#include "wpi/util/SmallVector.hpp" -#include "wpi/util/StringExtras.hpp" -#include "wpi/util/condition_variable.hpp" -#include "wpi/util/mutex.hpp" +#include "wpi/util/SafeThread.hpp" +#include "wpi/util/Synchronization.h" +#include "wpi/util/priority_queue.hpp" namespace { struct Notifier { std::string name; - uint64_t waitTime = UINT64_MAX; - bool active = true; - bool waitTimeValid = false; // True if waitTime is set and in the future - wpi::util::mutex mutex; - wpi::util::condition_variable cond; + std::atomic alarmTime = UINT64_MAX; + uint64_t intervalTime = 0; + std::atomic userOverrunCount = 0; + int32_t overrunCount = 0; + std::atomic_flag handlerSignaled{}; }; } // namespace using namespace wpi::hal; -static wpi::util::mutex notifiersWaiterMutex; -static wpi::util::condition_variable notifiersWaiterCond; - -class NotifierHandleContainer - : public UnlimitedHandleResource { +class NotifierThread : public wpi::util::SafeThread { public: - ~NotifierHandleContainer() { - ForEach([](HAL_NotifierHandle handle, Notifier* notifier) { - { - std::scoped_lock lock(notifier->mutex); - notifier->active = false; - notifier->waitTimeValid = false; - } - notifier->cond.notify_all(); // wake up any waiting threads - }); - notifiersWaiterCond.notify_all(); - } + void Main() override; + + void ProcessAlarms(); + + UnlimitedHandleResource + m_handles; + + struct Alarm { + HAL_NotifierHandle handle; + std::shared_ptr notifier; + bool operator==(const Alarm& rhs) const { return handle == rhs.handle; } + bool operator>(const Alarm& rhs) const { + return notifier->alarmTime > rhs.notifier->alarmTime; + } + }; + wpi::util::priority_queue, std::greater> + m_alarmQueue; }; -static NotifierHandleContainer* notifierHandles; +class NotifierInstance { + public: + NotifierInstance() { owner.Start(); } + wpi::util::SafeThreadOwner owner; +}; + +static NotifierInstance* notifierInstance; namespace wpi::hal::init { void InitializeNotifier() { - static NotifierHandleContainer nH; - notifierHandles = &nH; + static NotifierInstance n; + notifierInstance = &n; } } // namespace wpi::hal::init +void NotifierThread::Main() { + std::unique_lock lock(m_mutex); + while (m_active) { + if (m_alarmQueue.empty()) { + // No alarms, wait indefinitely + m_cond.wait(lock); + continue; + } + + // Wait until next alarm + const Alarm& alarm = m_alarmQueue.top(); + int32_t status = 0; + uint64_t curTime = HAL_GetFPGATime(&status); + if (alarm.notifier->alarmTime > curTime) { + m_cond.wait_for( + lock, std::chrono::microseconds{alarm.notifier->alarmTime - curTime}); + } + if (!m_active) { + break; + } + + ProcessAlarms(); + } +} + +void NotifierThread::ProcessAlarms() { + int32_t status = 0; + uint64_t curTime = HAL_GetFPGATime(&status); + + while (!m_alarmQueue.empty() && + m_alarmQueue.top().notifier->alarmTime <= curTime) { + Alarm alarm = m_alarmQueue.pop(); + HAL_NotifierHandle handle = alarm.handle; + Notifier& notifier = *alarm.notifier; + + if (notifier.intervalTime > 0) { + // Schedule next alarm + notifier.alarmTime += notifier.intervalTime; + if (curTime >= notifier.alarmTime) { + // We missed at least one interval + int32_t missed = static_cast((curTime - notifier.alarmTime) / + notifier.intervalTime) + + 1; + notifier.overrunCount += missed; + notifier.alarmTime += + missed * notifier.intervalTime; // Skip missed intervals + } + // Reinsert into queue + m_alarmQueue.push(std::move(alarm)); + } else { + // Disable one-shot alarm + notifier.alarmTime = UINT64_MAX; + } + + // If the last call was acknowledged, signal the handler + if (!notifier.handlerSignaled.test_and_set()) { + // copy the overrun count for the handler to read, reset the local count + notifier.userOverrunCount = notifier.overrunCount; + notifier.overrunCount = 0; + wpi::util::SetSignalObject(handle); + } + } +} + extern "C" { -HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) { +HAL_NotifierHandle HAL_CreateNotifier(int32_t* status) { wpi::hal::init::CheckInit(); std::shared_ptr notifier = std::make_shared(); - HAL_NotifierHandle handle = notifierHandles->Allocate(notifier); + HAL_NotifierHandle handle = + notifierInstance->owner.GetThread()->m_handles.Allocate(notifier); if (handle == HAL_kInvalidHandle) { *status = HAL_HANDLE_ERROR; return HAL_kInvalidHandle; } + wpi::util::CreateSignalObject(handle); return handle; } HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority, int32_t* status) { - // There is no thread, so this can be removed. - return true; + auto native = notifierInstance->owner.GetNativeThreadHandle(); + return HAL_SetThreadPriority(&native, realTime, priority, status); } -void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name, - int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); +void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, + const WPI_String* name, int32_t* status) { + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Get(notifierHandle); if (!notifier) { return; } - std::scoped_lock lock(notifier->mutex); - notifier->name = name; + notifier->name = wpi::util::to_string_view(name); } -void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); +void HAL_DestroyNotifier(HAL_NotifierHandle notifierHandle) { + wpi::util::DestroySignalObject(notifierHandle); + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Free(notifierHandle); + thr->m_alarmQueue.remove({notifierHandle, notifier}); +} + +void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime, + uint64_t intervalTime, HAL_Bool absolute, + int32_t* status) { + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Get(notifierHandle); if (!notifier) { return; } - { - std::scoped_lock lock(notifier->mutex); - notifier->active = false; - notifier->waitTimeValid = false; - } - notifier->cond.notify_all(); -} - -void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle) { - auto notifier = notifierHandles->Free(notifierHandle); - if (!notifier) { - return; + if (!absolute) { + alarmTime += HAL_GetFPGATime(status); } - // Just in case HAL_StopNotifier() wasn't called... - { - std::scoped_lock lock(notifier->mutex); - notifier->active = false; - notifier->waitTimeValid = false; + uint64_t prevWakeup = UINT64_MAX; + if (!thr->m_alarmQueue.empty()) { + prevWakeup = thr->m_alarmQueue.top().notifier->alarmTime; + thr->m_alarmQueue.remove({notifierHandle, notifier}); } - notifier->cond.notify_all(); -} + notifier->alarmTime = alarmTime; + notifier->intervalTime = intervalTime; + notifier->overrunCount = 0; + thr->m_alarmQueue.push({notifierHandle, notifier}); -void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle, - uint64_t triggerTime, int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); - if (!notifier) { - return; + // wake up notifier thread if needed + if (alarmTime < prevWakeup) { + thr->m_cond.notify_all(); } - - { - std::scoped_lock lock(notifier->mutex); - notifier->waitTime = triggerTime; - notifier->waitTimeValid = (triggerTime != UINT64_MAX); - } - - // We wake up any waiters to change how long they're sleeping for - notifier->cond.notify_all(); } void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Get(notifierHandle); if (!notifier) { return; } - { - std::scoped_lock lock(notifier->mutex); - notifier->waitTimeValid = false; - } + thr->m_alarmQueue.remove({notifierHandle, notifier}); + notifier->alarmTime = UINT64_MAX; } -uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle, +void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status) { - auto notifier = notifierHandles->Get(notifierHandle); + auto notifier = + notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle); if (!notifier) { - return 0; + return; } + notifier->handlerSignaled.clear(); +} - std::unique_lock ulock(notifiersWaiterMutex); - std::unique_lock lock(notifier->mutex); - ulock.unlock(); - notifiersWaiterCond.notify_all(); - while (notifier->active) { - uint64_t curTime = HAL_GetFPGATime(status); - if (notifier->waitTimeValid && curTime >= notifier->waitTime) { - notifier->waitTimeValid = false; - return curTime; - } - - double waitDuration; - if (!notifier->waitTimeValid) { - // If not running, wait 1000 seconds - waitDuration = 1000.0; - } else { - waitDuration = (notifier->waitTime - curTime) * 1e-6; - } - - notifier->cond.wait_for(lock, std::chrono::duration(waitDuration)); +int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle, + int32_t* status) { + auto notifier = + notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle); + if (!notifier) { + return -1; } - return 0; + return notifier->userOverrunCount; } } // extern "C" diff --git a/hal/src/main/python/hal/simulation/resethandles.cpp b/hal/src/main/python/hal/simulation/resethandles.cpp index b6bf48eb64..a371718bb9 100644 --- a/hal/src/main/python/hal/simulation/resethandles.cpp +++ b/hal/src/main/python/hal/simulation/resethandles.cpp @@ -15,7 +15,7 @@ void HALSIM_ResetGlobalHandles() { HALSIM_GetNotifierInfo(info, sz); for (int i = 0; i < sz; i++) { - HAL_CleanNotifier(info->handle); + HAL_DestroyNotifier(info->handle); } } diff --git a/hal/src/main/python/semiwrap/Notifier.yml b/hal/src/main/python/semiwrap/Notifier.yml index f128b5e715..660eb3a73d 100644 --- a/hal/src/main/python/semiwrap/Notifier.yml +++ b/hal/src/main/python/semiwrap/Notifier.yml @@ -2,11 +2,15 @@ strip_prefixes: - HAL_ functions: - HAL_InitializeNotifier: + HAL_CreateNotifier: HAL_SetNotifierName: - HAL_StopNotifier: - HAL_CleanNotifier: - HAL_UpdateNotifierAlarm: + overloads: + HAL_NotifierHandle, const struct WPI_String*, int32_t*: + ignore: true + HAL_NotifierHandle, std::string_view, int32_t*: + HAL_DestroyNotifier: + HAL_SetNotifierAlarm: HAL_CancelNotifierAlarm: - HAL_WaitForNotifierAlarm: + HAL_GetNotifierOverrun: + HAL_AcknowledgeNotifierAlarm: HAL_SetNotifierThreadPriority: diff --git a/hal/src/main/python/semiwrap/simulation/NotifierData.yml b/hal/src/main/python/semiwrap/simulation/NotifierData.yml index 3279e9ce21..6501f1ce13 100644 --- a/hal/src/main/python/semiwrap/simulation/NotifierData.yml +++ b/hal/src/main/python/semiwrap/simulation/NotifierData.yml @@ -10,5 +10,6 @@ classes: attributes: handle: name: - timeout: - waitTimeValid: + alarmTime: + intervalTime: + overrunCount: diff --git a/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp b/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp index 41331b38ab..cc801da3c5 100644 --- a/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp @@ -54,20 +54,61 @@ static void DisplayTiming() { ImGui::PopItemWidth(); static std::vector notifiers; + struct NotifierLastValue { + uint64_t alarmTime = UINT64_MAX; + double displayTime = 0; + }; + constexpr double fadeDuration = 0.25; + double curGuiTime = ImGui::GetTime(); + + static std::vector notifierFades; int32_t num = HALSIM_GetNotifierInfo(notifiers.data(), notifiers.size()); if (static_cast(num) > notifiers.size()) { notifiers.resize(num); - HALSIM_GetNotifierInfo(notifiers.data(), notifiers.size()); + notifierFades.resize(num); + num = HALSIM_GetNotifierInfo(notifiers.data(), notifiers.size()); } + for (int32_t i = 0; i < num; ++i) { + if (notifiers[i].alarmTime != UINT64_MAX) { + notifierFades[i].alarmTime = notifiers[i].alarmTime; + notifierFades[i].displayTime = curGuiTime; + } else if (curGuiTime > (notifierFades[i].displayTime + fadeDuration)) { + notifierFades[i].alarmTime = UINT64_MAX; + notifierFades[i].displayTime = 0; + } + } + if (num > 0) { ImGui::Separator(); } - ImGui::PushItemWidth(ImGui::GetFontSize() * 4); - for (int32_t i = 0; i < num; ++i) { - ImGui::LabelText(notifiers[i].name, "%.3f", - notifiers[i].timeout / 1000000.0); + if (ImGui::BeginTable("Notifiers", 4, ImGuiTableFlags_Borders)) { + ImGui::TableSetupColumn("Name", ImGuiTableColumnFlags_WidthStretch); + ImGui::TableSetupColumn("Alarm", ImGuiTableColumnFlags_WidthStretch | + ImGuiTableColumnFlags_DefaultSort); + ImGui::TableSetupColumn("Interval", ImGuiTableColumnFlags_WidthStretch); + ImGui::TableSetupColumn("Overruns", ImGuiTableColumnFlags_WidthStretch); + ImGui::TableHeadersRow(); + for (int32_t i = 0; i < num; ++i) { + ImGui::TableNextRow(); + ImGui::TableNextColumn(); + ImGui::TextUnformatted(notifiers[i].name); + ImGui::TableNextColumn(); + if (notifierFades[i].alarmTime != UINT64_MAX) { + ImGui::PushStyleVar( + ImGuiStyleVar_Alpha, + 1.0 - (curGuiTime - notifierFades[i].displayTime) / fadeDuration); + ImGui::Text("%.3f", notifierFades[i].alarmTime / 1000000.0); + ImGui::PopStyleVar(); + } + ImGui::TableNextColumn(); + if (notifiers[i].intervalTime != 0) { + ImGui::Text("%.3f", notifiers[i].intervalTime / 1000000.0); + } + ImGui::TableNextColumn(); + ImGui::Text("%d", notifiers[i].overrunCount); + } + ImGui::EndTable(); } - ImGui::PopItemWidth(); } void TimingGui::Initialize() { diff --git a/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp b/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp index 7396c795dc..15f338f33f 100644 --- a/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp @@ -25,6 +25,8 @@ void TimedRobot::StartCompetition() { std::puts("\n********** Robot program startup complete **********"); HAL_ObserveUserProgramStarting(); + bool first = true; + // Loop forever, calling the appropriate mode-dependent function while (true) { // We don't have to check there's an element in the queue first because @@ -33,17 +35,25 @@ void TimedRobot::StartCompetition() { auto callback = m_callbacks.pop(); int32_t status = 0; - HAL_UpdateNotifierAlarm(m_notifier, callback.expirationTime.count(), - &status); + HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true, + &status); WPILIB_CheckErrorStatus(status, "UpdateNotifierAlarm"); - std::chrono::microseconds currentTime{ - HAL_WaitForNotifierAlarm(m_notifier, &status)}; - if (currentTime.count() == 0 || status != 0) { + // Acknowledge previous alarm after setting the next one to avoid a race + // against getting the next notifier timeout in HALSIM StepTiming. + if (first) { + first = false; + } else { + HAL_AcknowledgeNotifierAlarm(m_notifier, &status); + WPILIB_CheckErrorStatus(status, "AcknowledgeNotifierAlarm"); + } + + if (WPI_WaitForObject(m_notifier) == 0) { break; } m_loopStartTimeUs = RobotController::GetFPGATime(); + std::chrono::microseconds currentTime{m_loopStartTimeUs}; callback.func(); @@ -71,8 +81,8 @@ void TimedRobot::StartCompetition() { } void TimedRobot::EndCompetition() { - int32_t status = 0; - HAL_StopNotifier(m_notifier, &status); + HAL_DestroyNotifier(m_notifier); + m_notifier = HAL_kInvalidHandle; } TimedRobot::TimedRobot(wpi::units::second_t period) @@ -81,7 +91,7 @@ TimedRobot::TimedRobot(wpi::units::second_t period) AddPeriodic([=, this] { LoopFunc(); }, period); int32_t status = 0; - m_notifier = HAL_InitializeNotifier(&status); + m_notifier = HAL_CreateNotifier(&status); WPILIB_CheckErrorStatus(status, "InitializeNotifier"); HAL_SetNotifierName(m_notifier, "TimedRobot", &status); @@ -93,9 +103,7 @@ TimedRobot::TimedRobot(wpi::units::hertz_t frequency) TimedRobot::~TimedRobot() { if (m_notifier != HAL_kInvalidHandle) { - int32_t status = 0; - HAL_StopNotifier(m_notifier, &status); - WPILIB_ReportError(status, "StopNotifier"); + HAL_DestroyNotifier(m_notifier); } } diff --git a/wpilibc/src/main/native/cpp/system/Notifier.cpp b/wpilibc/src/main/native/cpp/system/Notifier.cpp index 08aa2e066d..4ceba5d624 100644 --- a/wpilibc/src/main/native/cpp/system/Notifier.cpp +++ b/wpilibc/src/main/native/cpp/system/Notifier.cpp @@ -6,13 +6,11 @@ #include -#include - #include "wpi/hal/DriverStation.h" #include "wpi/hal/Notifier.h" #include "wpi/hal/Threads.h" #include "wpi/system/Errors.hpp" -#include "wpi/system/Timer.hpp" +#include "wpi/util/Synchronization.h" using namespace wpi; @@ -22,8 +20,8 @@ Notifier::Notifier(std::function callback) { } m_callback = callback; int32_t status = 0; - m_notifier = HAL_InitializeNotifier(&status); - WPILIB_CheckErrorStatus(status, "InitializeNotifier"); + m_notifier = HAL_CreateNotifier(&status); + WPILIB_CheckErrorStatus(status, "CreateNotifier"); m_thread = std::thread([=, this] { for (;;) { @@ -32,8 +30,7 @@ Notifier::Notifier(std::function callback) { if (notifier == 0) { break; } - uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status); - if (curTime == 0 || status != 0) { + if (WPI_WaitForObject(notifier) == 0) { break; } @@ -41,19 +38,16 @@ Notifier::Notifier(std::function callback) { { std::scoped_lock lock(m_processMutex); callback = m_callback; - if (m_periodic) { - m_expirationTime += m_period; - UpdateAlarm(); - } else { - // Need to update the alarm to cause it to wait again - UpdateAlarm(UINT64_MAX); - } } // Call callback if (callback) { callback(); } + + // Ack notifier + HAL_AcknowledgeNotifierAlarm(notifier, &status); + WPILIB_CheckErrorStatus(status, "AcknowledgeNotifier"); } }); } @@ -64,7 +58,7 @@ Notifier::Notifier(int priority, std::function callback) { } m_callback = callback; int32_t status = 0; - m_notifier = HAL_InitializeNotifier(&status); + m_notifier = HAL_CreateNotifier(&status); WPILIB_CheckErrorStatus(status, "InitializeNotifier"); m_thread = std::thread([=, this] { @@ -75,8 +69,7 @@ Notifier::Notifier(int priority, std::function callback) { if (notifier == 0) { break; } - uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status); - if (curTime == 0 || status != 0) { + if (WPI_WaitForObject(notifier) == 0) { break; } @@ -84,13 +77,6 @@ Notifier::Notifier(int priority, std::function callback) { { std::scoped_lock lock(m_processMutex); callback = m_callback; - if (m_periodic) { - m_expirationTime += m_period; - UpdateAlarm(); - } else { - // need to update the alarm to cause it to wait again - UpdateAlarm(UINT64_MAX); - } } // call callback @@ -111,32 +97,29 @@ Notifier::Notifier(int priority, std::function callback) { throw; } } + + // Ack notifier + HAL_AcknowledgeNotifierAlarm(notifier, &status); + WPILIB_CheckErrorStatus(status, "AcknowledgeNotifier"); } }); } Notifier::~Notifier() { - int32_t status = 0; // atomically set handle to 0, then clean HAL_NotifierHandle handle = m_notifier.exchange(0); - HAL_StopNotifier(handle, &status); - WPILIB_ReportError(status, "StopNotifier"); + HAL_DestroyNotifier(handle); // Join the thread to ensure the callback has exited. if (m_thread.joinable()) { m_thread.join(); } - - HAL_CleanNotifier(handle); } Notifier::Notifier(Notifier&& rhs) : m_thread(std::move(rhs.m_thread)), m_notifier(rhs.m_notifier.load()), - m_callback(std::move(rhs.m_callback)), - m_expirationTime(std::move(rhs.m_expirationTime)), - m_period(std::move(rhs.m_period)), - m_periodic(std::move(rhs.m_periodic)) { + m_callback(std::move(rhs.m_callback)) { rhs.m_notifier = HAL_kInvalidHandle; } @@ -145,19 +128,12 @@ Notifier& Notifier::operator=(Notifier&& rhs) { m_notifier = rhs.m_notifier.load(); rhs.m_notifier = HAL_kInvalidHandle; m_callback = std::move(rhs.m_callback); - m_expirationTime = std::move(rhs.m_expirationTime); - m_period = std::move(rhs.m_period); - m_periodic = std::move(rhs.m_periodic); - return *this; } void Notifier::SetName(std::string_view name) { - fmt::memory_buffer buf; - fmt::format_to(fmt::appender{buf}, "{}", name); - buf.push_back('\0'); // null terminate int32_t status = 0; - HAL_SetNotifierName(m_notifier, buf.data(), &status); + HAL_SetNotifierName(m_notifier, name, &status); } void Notifier::SetCallback(std::function callback) { @@ -166,19 +142,15 @@ void Notifier::SetCallback(std::function callback) { } void Notifier::StartSingle(wpi::units::second_t delay) { - std::scoped_lock lock(m_processMutex); - m_periodic = false; - m_period = delay; - m_expirationTime = Timer::GetFPGATimestamp() + m_period; - UpdateAlarm(); + int32_t status = 0; + HAL_SetNotifierAlarm(m_notifier, static_cast(delay * 1e6), 0, false, + &status); } void Notifier::StartPeriodic(wpi::units::second_t period) { - std::scoped_lock lock(m_processMutex); - m_periodic = true; - m_period = period; - m_expirationTime = Timer::GetFPGATimestamp() + m_period; - UpdateAlarm(); + int32_t status = 0; + HAL_SetNotifierAlarm(m_notifier, static_cast(period * 1e6), + static_cast(period * 1e6), false, &status); } void Notifier::StartPeriodic(wpi::units::hertz_t frequency) { @@ -186,26 +158,16 @@ void Notifier::StartPeriodic(wpi::units::hertz_t frequency) { } void Notifier::Stop() { - std::scoped_lock lock(m_processMutex); - m_periodic = false; int32_t status = 0; HAL_CancelNotifierAlarm(m_notifier, &status); WPILIB_CheckErrorStatus(status, "CancelNotifierAlarm"); } -void Notifier::UpdateAlarm(uint64_t triggerTime) { +int32_t Notifier::GetOverrun() const { int32_t status = 0; - // Return if we are being destructed, or were not created successfully - auto notifier = m_notifier.load(); - if (notifier == 0) { - return; - } - HAL_UpdateNotifierAlarm(notifier, triggerTime, &status); - WPILIB_CheckErrorStatus(status, "UpdateNotifierAlarm"); -} - -void Notifier::UpdateAlarm() { - UpdateAlarm(static_cast(m_expirationTime * 1e6)); + int32_t overrun = HAL_GetNotifierOverrun(m_notifier, &status); + WPILIB_CheckErrorStatus(status, "GetNotifierOverrun"); + return overrun; } bool Notifier::SetHALThreadPriority(bool realTime, int32_t priority) { diff --git a/wpilibc/src/main/native/cpp/system/Watchdog.cpp b/wpilibc/src/main/native/cpp/system/Watchdog.cpp index 73a6fc5352..9637669a97 100644 --- a/wpilibc/src/main/native/cpp/system/Watchdog.cpp +++ b/wpilibc/src/main/native/cpp/system/Watchdog.cpp @@ -11,9 +11,11 @@ #include +#include "wpi/hal/HALBase.h" #include "wpi/hal/Notifier.h" #include "wpi/system/Errors.hpp" #include "wpi/system/Timer.hpp" +#include "wpi/util/Synchronization.h" #include "wpi/util/mutex.hpp" #include "wpi/util/priority_queue.hpp" @@ -47,7 +49,7 @@ class Watchdog::Impl { Watchdog::Impl::Impl() { int32_t status = 0; - m_notifier = HAL_InitializeNotifier(&status); + m_notifier = HAL_CreateNotifier(&status); WPILIB_CheckErrorStatus(status, "starting watchdog notifier"); HAL_SetNotifierName(m_notifier, "Watchdog", &status); @@ -55,18 +57,14 @@ Watchdog::Impl::Impl() { } Watchdog::Impl::~Impl() { - int32_t status = 0; // atomically set handle to 0, then clean HAL_NotifierHandle handle = m_notifier.exchange(0); - HAL_StopNotifier(handle, &status); - WPILIB_ReportError(status, "stopping watchdog notifier"); + HAL_DestroyNotifier(handle); // Join the thread to ensure the handler has exited. if (m_thread.joinable()) { m_thread.join(); } - - HAL_CleanNotifier(handle); } void Watchdog::Impl::UpdateAlarm() { @@ -79,11 +77,10 @@ void Watchdog::Impl::UpdateAlarm() { if (m_watchdogs.empty()) { HAL_CancelNotifierAlarm(notifier, &status); } else { - HAL_UpdateNotifierAlarm( - notifier, - static_cast(m_watchdogs.top()->m_expirationTime.value() * - 1e6), - &status); + HAL_SetNotifierAlarm(notifier, + static_cast( + m_watchdogs.top()->m_expirationTime.value() * 1e6), + 0, true, &status); } WPILIB_CheckErrorStatus(status, "updating watchdog notifier alarm"); } @@ -95,10 +92,10 @@ void Watchdog::Impl::Main() { if (notifier == 0) { break; } - uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status); - if (curTime == 0 || status != 0) { + if (WPI_WaitForObject(notifier) == 0) { break; } + uint64_t curTime = HAL_GetFPGATime(&status); std::unique_lock lock(m_mutex); @@ -129,6 +126,8 @@ void Watchdog::Impl::Main() { lock.lock(); UpdateAlarm(); + + HAL_AcknowledgeNotifierAlarm(notifier, &status); } } diff --git a/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp b/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp index 0a56b6dab3..facc6e2025 100644 --- a/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp +++ b/wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp @@ -119,7 +119,7 @@ class TimedRobot : public IterativeRobotBase { } }; - wpi::hal::Handle m_notifier; + wpi::hal::Handle m_notifier; std::chrono::microseconds m_startTime; uint64_t m_loopStartTimeUs = 0; diff --git a/wpilibc/src/main/native/include/wpi/system/Notifier.hpp b/wpilibc/src/main/native/include/wpi/system/Notifier.hpp index 14f25d067e..d84b23c02e 100644 --- a/wpilibc/src/main/native/include/wpi/system/Notifier.hpp +++ b/wpilibc/src/main/native/include/wpi/system/Notifier.hpp @@ -130,6 +130,16 @@ class Notifier { */ void Stop(); + /** + * Gets the overrun count. + * + * An overrun occurs when a notifier's alarm is not serviced before the next + * scheduled alarm time. + * + * @return overrun count + */ + int32_t GetOverrun() const; + /** * Sets the HAL notifier thread priority. * @@ -148,18 +158,6 @@ class Notifier { static bool SetHALThreadPriority(bool realTime, int32_t priority); private: - /** - * Update the HAL alarm time. - * - * @param triggerTime the time at which the next alarm will be triggered - */ - void UpdateAlarm(uint64_t triggerTime); - - /** - * Update the HAL alarm time based on m_expirationTime. - */ - void UpdateAlarm(); - // The thread waiting on the HAL alarm std::thread m_thread; @@ -171,17 +169,6 @@ class Notifier { // The user-provided callback std::function m_callback; - - // The time at which the callback should be called. Has the same zero as - // Timer::GetFPGATimestamp(). - wpi::units::second_t m_expirationTime = 0_s; - - // If periodic, stores the callback period; if single, stores the time until - // the callback call. - wpi::units::second_t m_period = 0_s; - - // True if the callback is periodic - bool m_periodic = false; }; } // namespace wpi diff --git a/wpilibc/src/main/python/semiwrap/Notifier.yml b/wpilibc/src/main/python/semiwrap/Notifier.yml index 6e167ebbaa..3f35b07c15 100644 --- a/wpilibc/src/main/python/semiwrap/Notifier.yml +++ b/wpilibc/src/main/python/semiwrap/Notifier.yml @@ -18,4 +18,5 @@ classes: ignore: true wpi::units::second_t: Stop: + GetOverrun: SetHALThreadPriority: diff --git a/wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp b/wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp index b84bd2ed28..86c75f657a 100644 --- a/wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp +++ b/wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp @@ -7,11 +7,12 @@ #include #include + #include "wpi/hal/Notifier.h" #include "wpi/hal/Threads.h" - #include "wpi/system/Errors.hpp" #include "wpi/system/Timer.hpp" +#include "wpi/util/Synchronization.h" #include #include @@ -37,8 +38,8 @@ PyNotifier::PyNotifier(std::function handler) { } m_handler = handler; int32_t status = 0; - m_notifier = HAL_InitializeNotifier(&status); - WPILIB_CheckErrorStatus(status, "InitializeNotifier"); + m_notifier = HAL_CreateNotifier(&status); + WPILIB_CheckErrorStatus(status, "CreateNotifier"); std::function target([this] { py::gil_scoped_release release; @@ -50,8 +51,7 @@ PyNotifier::PyNotifier(std::function handler) { if (notifier == 0) { break; } - uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status); - if (curTime == 0 || status != 0) { + if (WPI_WaitForObject(notifier) == 0) { break; } @@ -59,13 +59,6 @@ PyNotifier::PyNotifier(std::function handler) { { std::scoped_lock lock(m_processMutex); handler = m_handler; - if (m_periodic) { - m_expirationTime += m_period; - UpdateAlarm(); - } else { - // need to update the alarm to cause it to wait again - UpdateAlarm(UINT64_MAX); - } } // call callback @@ -76,6 +69,10 @@ PyNotifier::PyNotifier(std::function handler) { handler(); } + + // Ack notifier + HAL_AcknowledgeNotifierAlarm(notifier, &status); + WPILIB_CheckErrorStatus(status, "AcknowledgeNotifier"); } } catch (...) { _hang_thread_if_finalizing(); @@ -95,27 +92,20 @@ PyNotifier::PyNotifier(std::function handler) { } PyNotifier::~PyNotifier() { - int32_t status = 0; // atomically set handle to 0, then clean HAL_NotifierHandle handle = m_notifier.exchange(0); - HAL_StopNotifier(handle, &status); - WPILIB_ReportError(status, "StopNotifier"); + HAL_DestroyNotifier(handle); // Join the thread to ensure the handler has exited. if (m_thread) { m_thread.attr("join")(); } - - HAL_CleanNotifier(handle); } PyNotifier::PyNotifier(PyNotifier &&rhs) : m_thread(std::move(rhs.m_thread)), m_notifier(rhs.m_notifier.load()), - m_handler(std::move(rhs.m_handler)), - m_expirationTime(std::move(rhs.m_expirationTime)), - m_period(std::move(rhs.m_period)), - m_periodic(std::move(rhs.m_periodic)) { + m_handler(std::move(rhs.m_handler)) { rhs.m_notifier = HAL_kInvalidHandle; } @@ -124,19 +114,12 @@ PyNotifier &PyNotifier::operator=(PyNotifier &&rhs) { m_notifier = rhs.m_notifier.load(); rhs.m_notifier = HAL_kInvalidHandle; m_handler = std::move(rhs.m_handler); - m_expirationTime = std::move(rhs.m_expirationTime); - m_period = std::move(rhs.m_period); - m_periodic = std::move(rhs.m_periodic); - return *this; } void PyNotifier::SetName(std::string_view name) { - fmt::memory_buffer buf; - fmt::format_to(fmt::appender{buf}, "{}", name); - buf.push_back('\0'); // null terminate int32_t status = 0; - HAL_SetNotifierName(m_notifier, buf.data(), &status); + HAL_SetNotifierName(m_notifier, name, &status); } void PyNotifier::SetCallback(std::function handler) { @@ -145,45 +128,31 @@ void PyNotifier::SetCallback(std::function handler) { } void PyNotifier::StartSingle(wpi::units::second_t delay) { - std::scoped_lock lock(m_processMutex); - m_periodic = false; - m_period = delay; - m_expirationTime = Timer::GetFPGATimestamp() + m_period; - UpdateAlarm(); + int32_t status = 0; + HAL_SetNotifierAlarm(m_notifier, static_cast(delay * 1e6), 0, false, + &status); } void PyNotifier::StartPeriodic(wpi::units::second_t period) { - std::scoped_lock lock(m_processMutex); - m_periodic = true; - m_period = period; - m_expirationTime = Timer::GetFPGATimestamp() + m_period; - UpdateAlarm(); + int32_t status = 0; + HAL_SetNotifierAlarm(m_notifier, static_cast(period * 1e6), + static_cast(period * 1e6), false, &status); } void PyNotifier::Stop() { - std::scoped_lock lock(m_processMutex); - m_periodic = false; int32_t status = 0; HAL_CancelNotifierAlarm(m_notifier, &status); WPILIB_CheckErrorStatus(status, "CancelNotifierAlarm"); } -void PyNotifier::UpdateAlarm(uint64_t triggerTime) { +int32_t PyNotifier::GetOverrun() const { int32_t status = 0; - // Return if we are being destructed, or were not created successfully - auto notifier = m_notifier.load(); - if (notifier == 0) { - return; - } - HAL_UpdateNotifierAlarm(notifier, triggerTime, &status); - WPILIB_CheckErrorStatus(status, "UpdateNotifierAlarm"); -} - -void PyNotifier::UpdateAlarm() { - UpdateAlarm(static_cast(m_expirationTime * 1e6)); + int32_t overrun = HAL_GetNotifierOverrun(m_notifier, &status); + WPILIB_CheckErrorStatus(status, "GetNotifierOverrun"); + return overrun; } bool PyNotifier::SetHALThreadPriority(bool realTime, int32_t priority) { int32_t status = 0; return HAL_SetNotifierThreadPriority(realTime, priority, &status); -} \ No newline at end of file +} diff --git a/wpilibc/src/main/python/wpilib/src/rpy/Notifier.h b/wpilibc/src/main/python/wpilib/src/rpy/Notifier.h index ebd39b4fc8..80e22adc21 100644 --- a/wpilibc/src/main/python/wpilib/src/rpy/Notifier.h +++ b/wpilibc/src/main/python/wpilib/src/rpy/Notifier.h @@ -91,6 +91,16 @@ class PyNotifier { */ void Stop(); + /** + * Gets the overrun count. + * + * An overrun occurs when a notifier's alarm is not serviced before the next + * scheduled alarm time. + * + * @return overrun count + */ + int32_t GetOverrun() const; + /** * Sets the HAL notifier thread priority. * @@ -109,18 +119,6 @@ class PyNotifier { static bool SetHALThreadPriority(bool realTime, int32_t priority); private: - /** - * Update the HAL alarm time. - * - * @param triggerTime the time at which the next alarm will be triggered - */ - void UpdateAlarm(uint64_t triggerTime); - - /** - * Update the HAL alarm time based on m_expirationTime. - */ - void UpdateAlarm(); - // The thread waiting on the HAL alarm py::object m_thread; @@ -132,15 +130,6 @@ private: // Address of the handler std::function m_handler; - - // The absolute expiration time - wpi::units::second_t m_expirationTime = 0_s; - - // The relative time (either periodic or single) - wpi::units::second_t m_period = 0_s; - - // True if this is a periodic event - bool m_periodic = false; }; } // namespace wpi diff --git a/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java b/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java index 52d735e4ac..abb259ee22 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java @@ -13,6 +13,7 @@ import org.wpilib.hardware.hal.NotifierJNI; import org.wpilib.system.RobotController; import org.wpilib.units.measure.Frequency; import org.wpilib.units.measure.Time; +import org.wpilib.util.WPIUtilJNI; /** * TimedRobot implements the IterativeRobotBase robot program framework. @@ -69,7 +70,7 @@ public class TimedRobot extends IterativeRobotBase { // The C pointer to the notifier object. We don't use it directly, it is // just passed to the JNI bindings. - private final int m_notifier = NotifierJNI.initializeNotifier(); + private final int m_notifier = NotifierJNI.createNotifier(); private long m_startTimeUs; private long m_loopStartTimeUs; @@ -115,8 +116,7 @@ public class TimedRobot extends IterativeRobotBase { @Override public void close() { - NotifierJNI.stopNotifier(m_notifier); - NotifierJNI.cleanNotifier(m_notifier); + NotifierJNI.destroyNotifier(m_notifier); } /** Provide an alternate "main loop" via startCompetition(). */ @@ -130,6 +130,8 @@ public class TimedRobot extends IterativeRobotBase { System.out.println("********** Robot program startup complete **********"); DriverStationJNI.observeUserProgramStarting(); + boolean first = true; + // Loop forever, calling the appropriate mode-dependent function while (true) { // We don't have to check there's an element in the queue first because @@ -137,14 +139,25 @@ public class TimedRobot extends IterativeRobotBase { // at the end of the loop. var callback = m_callbacks.poll(); - NotifierJNI.updateNotifierAlarm(m_notifier, callback.expirationTime); + NotifierJNI.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true); - long currentTime = NotifierJNI.waitForNotifierAlarm(m_notifier); - if (currentTime == 0) { + // Acknowledge previous alarm after setting the next one to avoid a race + // against getting the next notifier timeout in HALSIM StepTiming. + if (first) { + first = false; + } else { + NotifierJNI.acknowledgeNotifierAlarm(m_notifier); + } + + try { + WPIUtilJNI.waitForObject(m_notifier); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); break; } - m_loopStartTimeUs = RobotController.getFPGATime(); + long currentTime = RobotController.getFPGATime(); + m_loopStartTimeUs = currentTime; callback.func.run(); @@ -174,7 +187,7 @@ public class TimedRobot extends IterativeRobotBase { /** Ends the main loop in startCompetition(). */ @Override public void endCompetition() { - NotifierJNI.stopNotifier(m_notifier); + NotifierJNI.destroyNotifier(m_notifier); } /** diff --git a/wpilibj/src/main/java/org/wpilib/system/Notifier.java b/wpilibj/src/main/java/org/wpilib/system/Notifier.java index df0f2c364c..10e137798e 100644 --- a/wpilibj/src/main/java/org/wpilib/system/Notifier.java +++ b/wpilibj/src/main/java/org/wpilib/system/Notifier.java @@ -13,6 +13,7 @@ import org.wpilib.driverstation.DriverStation; import org.wpilib.hardware.hal.NotifierJNI; import org.wpilib.units.measure.Frequency; import org.wpilib.units.measure.Time; +import org.wpilib.util.WPIUtilJNI; /** * Notifiers run a user-provided callback function on a separate thread. @@ -33,24 +34,13 @@ public class Notifier implements AutoCloseable { // The user-provided callback. private Runnable m_callback; - // The time, in seconds, at which the callback should be called. Has the same - // zero as RobotController.getFPGATime(). - private double m_expirationTime; - - // If periodic, stores the callback period in seconds; if single, stores the time until - // the callback call in seconds. - private double m_period; - - // True if the callback is periodic - private boolean m_periodic; - @Override public void close() { int handle = m_notifier.getAndSet(0); if (handle == 0) { return; } - NotifierJNI.stopNotifier(handle); + NotifierJNI.destroyNotifier(handle); // Join the thread to ensure the callback has exited. if (m_thread.isAlive()) { try { @@ -60,28 +50,9 @@ public class Notifier implements AutoCloseable { Thread.currentThread().interrupt(); } } - NotifierJNI.cleanNotifier(handle); m_thread = null; } - /** - * Update the alarm hardware to reflect the next alarm. - * - * @param triggerTimeMicroS the time in microseconds at which the next alarm will be triggered - */ - private void updateAlarm(long triggerTimeMicroS) { - int notifier = m_notifier.get(); - if (notifier == 0) { - return; - } - NotifierJNI.updateNotifierAlarm(notifier, triggerTimeMicroS); - } - - /** Update the alarm hardware to reflect the next alarm. */ - private void updateAlarm() { - updateAlarm((long) (m_expirationTime * 1e6)); - } - /** * Create a Notifier with the given callback. * @@ -93,7 +64,7 @@ public class Notifier implements AutoCloseable { requireNonNullParam(callback, "callback", "Notifier"); m_callback = callback; - m_notifier.set(NotifierJNI.initializeNotifier()); + m_notifier.set(NotifierJNI.createNotifier()); m_thread = new Thread( @@ -103,8 +74,10 @@ public class Notifier implements AutoCloseable { if (notifier == 0) { break; } - long curTime = NotifierJNI.waitForNotifierAlarm(notifier); - if (curTime == 0) { + try { + WPIUtilJNI.waitForObject(notifier); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); break; } @@ -112,13 +85,6 @@ public class Notifier implements AutoCloseable { m_processLock.lock(); try { threadHandler = m_callback; - if (m_periodic) { - m_expirationTime += m_period; - updateAlarm(); - } else { - // Need to update the alarm to cause it to wait again - updateAlarm(-1); - } } finally { m_processLock.unlock(); } @@ -127,6 +93,9 @@ public class Notifier implements AutoCloseable { if (threadHandler != null) { threadHandler.run(); } + + // Acknowledge the alarm + NotifierJNI.acknowledgeNotifierAlarm(notifier); } }); m_thread.setName("Notifier"); @@ -179,15 +148,7 @@ public class Notifier implements AutoCloseable { * @param delay Time in seconds to wait before the callback is called. */ public void startSingle(double delay) { - m_processLock.lock(); - try { - m_periodic = false; - m_period = delay; - m_expirationTime = RobotController.getFPGATime() * 1e-6 + delay; - updateAlarm(); - } finally { - m_processLock.unlock(); - } + NotifierJNI.setNotifierAlarm(m_notifier.get(), (long) (delay * 1e6), 0, false); } /** @@ -209,15 +170,8 @@ public class Notifier implements AutoCloseable { * call to this method. */ public void startPeriodic(double period) { - m_processLock.lock(); - try { - m_periodic = true; - m_period = period; - m_expirationTime = RobotController.getFPGATime() * 1e-6 + period; - updateAlarm(); - } finally { - m_processLock.unlock(); - } + long periodMicroS = (long) (period * 1e6); + NotifierJNI.setNotifierAlarm(m_notifier.get(), periodMicroS, periodMicroS, false); } /** @@ -256,13 +210,7 @@ public class Notifier implements AutoCloseable { * complete. */ public void stop() { - m_processLock.lock(); - try { - m_periodic = false; - NotifierJNI.cancelNotifierAlarm(m_notifier.get()); - } finally { - m_processLock.unlock(); - } + NotifierJNI.cancelNotifierAlarm(m_notifier.get()); } /** diff --git a/wpilibj/src/main/java/org/wpilib/system/Watchdog.java b/wpilibj/src/main/java/org/wpilib/system/Watchdog.java index a3d0600b8f..f251150c8d 100644 --- a/wpilibj/src/main/java/org/wpilib/system/Watchdog.java +++ b/wpilibj/src/main/java/org/wpilib/system/Watchdog.java @@ -10,8 +10,10 @@ import java.io.Closeable; import java.util.PriorityQueue; import java.util.concurrent.locks.ReentrantLock; import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.HALUtil; import org.wpilib.hardware.hal.NotifierJNI; import org.wpilib.units.measure.Time; +import org.wpilib.util.WPIUtilJNI; /** * A class that's a wrapper around a watchdog timer. @@ -42,7 +44,7 @@ public class Watchdog implements Closeable, Comparable { private static int m_notifier; static { - m_notifier = NotifierJNI.initializeNotifier(); + m_notifier = NotifierJNI.createNotifier(); NotifierJNI.setNotifierName(m_notifier, "Watchdog"); startDaemonThread(Watchdog::schedulerFunc); } @@ -225,8 +227,8 @@ public class Watchdog implements Closeable, Comparable { if (m_watchdogs.isEmpty()) { NotifierJNI.cancelNotifierAlarm(m_notifier); } else { - NotifierJNI.updateNotifierAlarm( - m_notifier, (long) (m_watchdogs.peek().m_expirationTime * 1e6)); + NotifierJNI.setNotifierAlarm( + m_notifier, (long) (m_watchdogs.peek().m_expirationTime * 1e6), 0, true); } } @@ -239,10 +241,13 @@ public class Watchdog implements Closeable, Comparable { private static void schedulerFunc() { while (!Thread.currentThread().isInterrupted()) { - long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier); - if (curTime == 0) { + try { + WPIUtilJNI.waitForObject(m_notifier); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); break; } + long curTime = HALUtil.getFPGATime(); m_queueMutex.lock(); try { @@ -273,6 +278,8 @@ public class Watchdog implements Closeable, Comparable { m_queueMutex.lock(); updateAlarm(); + + NotifierJNI.acknowledgeNotifierAlarm(m_notifier); } finally { m_queueMutex.unlock(); }