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(); }