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 eb3db6c458..a8c444f2f5 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/NotifierJNI.java +++ b/hal/src/main/java/org/wpilib/hardware/hal/NotifierJNI.java @@ -88,12 +88,26 @@ public class NotifierJNI extends JNIWrapper { public static native void cancelNotifierAlarm(int notifierHandle); /** - * Indicates the notifier alarm has been serviced. This must be called before waiting for the next - * alarm. + * Indicates the notifier alarm has been serviced and optionally sets a new alarm time. This must + * be called before waiting for the next alarm. + * + *

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 + * @param setAlarm true to set a new alarm time, false to leave the alarm unchanged + * @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_AcknowledgeNotifierAlarm" */ - public static native void acknowledgeNotifierAlarm(int notifierHandle); + public static native void acknowledgeNotifierAlarm( + int notifierHandle, boolean setAlarm, long alarmTime, long intervalTime, boolean absolute); /** * Gets the overrun count for a notifier. diff --git a/hal/src/main/native/cpp/jni/NotifierJNI.cpp b/hal/src/main/native/cpp/jni/NotifierJNI.cpp index eaf03e160a..6d052980cb 100644 --- a/hal/src/main/native/cpp/jni/NotifierJNI.cpp +++ b/hal/src/main/native/cpp/jni/NotifierJNI.cpp @@ -113,14 +113,18 @@ Java_org_wpilib_hardware_hal_NotifierJNI_cancelNotifierAlarm /* * Class: org_wpilib_hardware_hal_NotifierJNI * Method: acknowledgeNotifierAlarm - * Signature: (I)V + * Signature: (IZJJZ)V */ JNIEXPORT void JNICALL Java_org_wpilib_hardware_hal_NotifierJNI_acknowledgeNotifierAlarm - (JNIEnv* env, jclass cls, jint notifierHandle) + (JNIEnv* env, jclass cls, jint notifierHandle, jboolean setAlarm, + jlong alarmTime, jlong intervalTime, jboolean absolute) { int32_t status = 0; - HAL_AcknowledgeNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status); + HAL_AcknowledgeNotifierAlarm((HAL_NotifierHandle)notifierHandle, setAlarm, + static_cast(alarmTime), + static_cast(intervalTime), absolute, + &status); CheckStatus(env, status); } diff --git a/hal/src/main/native/include/wpi/hal/Notifier.h b/hal/src/main/native/include/wpi/hal/Notifier.h index 98ceb1187d..048e4cfe0c 100644 --- a/hal/src/main/native/include/wpi/hal/Notifier.h +++ b/hal/src/main/native/include/wpi/hal/Notifier.h @@ -103,13 +103,28 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, int32_t* status); /** - * Indicates the notifier alarm has been serviced. This must be called before - * waiting for the next alarm. + * Indicates the notifier alarm has been serviced and optionally sets a new + * alarm time. This must be called before waiting for the next alarm. + * + * 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] setAlarm true to set a new alarm time, false to leave the + * alarm unchanged + * @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_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle, + HAL_Bool setAlarm, uint64_t alarmTime, + uint64_t intervalTime, HAL_Bool absolute, int32_t* status); /** diff --git a/hal/src/main/native/sim/Notifier.cpp b/hal/src/main/native/sim/Notifier.cpp index 4b1be980c5..0f0d83b081 100644 --- a/hal/src/main/native/sim/Notifier.cpp +++ b/hal/src/main/native/sim/Notifier.cpp @@ -46,6 +46,10 @@ class NotifierThread : public wpi::util::SafeThread { public: void Main() override; + void SetAlarm(HAL_NotifierHandle notifierHandle, + std::shared_ptr& notifier, uint64_t alarmTime, + uint64_t intervalTime, bool absolute, int32_t* status); + void ProcessAlarms(wpi::util::SmallVectorImpl* signaled); bool m_paused = false; @@ -111,6 +115,30 @@ void NotifierThread::Main() { } } +void NotifierThread::SetAlarm(HAL_NotifierHandle notifierHandle, + std::shared_ptr& notifier, + uint64_t alarmTime, uint64_t intervalTime, + bool absolute, int32_t* status) { + if (!absolute) { + alarmTime += HAL_GetFPGATime(status); + } + + uint64_t prevWakeup = UINT64_MAX; + if (!m_alarmQueue.empty()) { + prevWakeup = m_alarmQueue.top().notifier->alarmTime; + m_alarmQueue.remove({notifierHandle, notifier}); + } + notifier->alarmTime = alarmTime; + notifier->intervalTime = intervalTime; + notifier->overrunCount = 0; + m_alarmQueue.push({notifierHandle, notifier}); + + // wake up notifier thread if needed + if (alarmTime < prevWakeup) { + m_cond.notify_all(); + } +} + void NotifierThread::ProcessAlarms( wpi::util::SmallVectorImpl* signaled) { int32_t status = 0; @@ -256,25 +284,8 @@ void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime, if (!notifier) { return; } - - if (!absolute) { - alarmTime += HAL_GetFPGATime(status); - } - - uint64_t prevWakeup = UINT64_MAX; - if (!thr->m_alarmQueue.empty()) { - prevWakeup = thr->m_alarmQueue.top().notifier->alarmTime; - thr->m_alarmQueue.remove({notifierHandle, notifier}); - } - notifier->alarmTime = alarmTime; - notifier->intervalTime = intervalTime; - notifier->overrunCount = 0; - thr->m_alarmQueue.push({notifierHandle, notifier}); - - // wake up notifier thread if needed - if (alarmTime < prevWakeup) { - thr->m_cond.notify_all(); - } + thr->SetAlarm(notifierHandle, notifier, alarmTime, intervalTime, absolute, + status); } void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, @@ -287,16 +298,23 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, thr->m_alarmQueue.remove({notifierHandle, notifier}); notifier->alarmTime = UINT64_MAX; + notifier->handlerSignaled.clear(); } void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle, + HAL_Bool setAlarm, uint64_t alarmTime, + uint64_t intervalTime, HAL_Bool absolute, int32_t* status) { - auto notifier = - notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle); + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Get(notifierHandle); if (!notifier) { return; } notifier->handlerSignaled.clear(); + if (setAlarm) { + thr->SetAlarm(notifierHandle, notifier, alarmTime, intervalTime, absolute, + status); + } } int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle, diff --git a/hal/src/main/native/systemcore/Notifier.cpp b/hal/src/main/native/systemcore/Notifier.cpp index 7bb9768ff4..f5501a3a64 100644 --- a/hal/src/main/native/systemcore/Notifier.cpp +++ b/hal/src/main/native/systemcore/Notifier.cpp @@ -39,6 +39,10 @@ class NotifierThread : public wpi::util::SafeThread { public: void Main() override; + void SetAlarm(HAL_NotifierHandle notifierHandle, + std::shared_ptr& notifier, uint64_t alarmTime, + uint64_t intervalTime, bool absolute, int32_t* status); + void ProcessAlarms(); UnlimitedHandleResource& notifier, + uint64_t alarmTime, uint64_t intervalTime, + bool absolute, int32_t* status) { + if (!absolute) { + alarmTime += HAL_GetFPGATime(status); + } + + uint64_t prevWakeup = UINT64_MAX; + if (!m_alarmQueue.empty()) { + prevWakeup = m_alarmQueue.top().notifier->alarmTime; + m_alarmQueue.remove({notifierHandle, notifier}); + } + notifier->alarmTime = alarmTime; + notifier->intervalTime = intervalTime; + notifier->overrunCount = 0; + m_alarmQueue.push({notifierHandle, notifier}); + + // wake up notifier thread if needed + if (alarmTime < prevWakeup) { + m_cond.notify_all(); + } +} + void NotifierThread::ProcessAlarms() { int32_t status = 0; uint64_t curTime = HAL_GetFPGATime(&status); @@ -182,25 +210,8 @@ void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime, if (!notifier) { return; } - - if (!absolute) { - alarmTime += HAL_GetFPGATime(status); - } - - uint64_t prevWakeup = UINT64_MAX; - if (!thr->m_alarmQueue.empty()) { - prevWakeup = thr->m_alarmQueue.top().notifier->alarmTime; - thr->m_alarmQueue.remove({notifierHandle, notifier}); - } - notifier->alarmTime = alarmTime; - notifier->intervalTime = intervalTime; - notifier->overrunCount = 0; - thr->m_alarmQueue.push({notifierHandle, notifier}); - - // wake up notifier thread if needed - if (alarmTime < prevWakeup) { - thr->m_cond.notify_all(); - } + thr->SetAlarm(notifierHandle, notifier, alarmTime, intervalTime, absolute, + status); } void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, @@ -213,16 +224,23 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle, thr->m_alarmQueue.remove({notifierHandle, notifier}); notifier->alarmTime = UINT64_MAX; + notifier->handlerSignaled.clear(); } void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle, + HAL_Bool setAlarm, uint64_t alarmTime, + uint64_t intervalTime, HAL_Bool absolute, int32_t* status) { - auto notifier = - notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle); + auto thr = notifierInstance->owner.GetThread(); + auto notifier = thr->m_handles.Get(notifierHandle); if (!notifier) { return; } notifier->handlerSignaled.clear(); + if (setAlarm) { + thr->SetAlarm(notifierHandle, notifier, alarmTime, intervalTime, absolute, + status); + } } int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle, diff --git a/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp b/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp index 15f338f33f..81f758a23c 100644 --- a/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/TimedRobot.cpp @@ -35,16 +35,14 @@ void TimedRobot::StartCompetition() { auto callback = m_callbacks.pop(); int32_t status = 0; - HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true, - &status); - WPILIB_CheckErrorStatus(status, "UpdateNotifierAlarm"); - - // 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; + HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true, + &status); + WPILIB_CheckErrorStatus(status, "SetNotifierAlarm"); } else { - HAL_AcknowledgeNotifierAlarm(m_notifier, &status); + HAL_AcknowledgeNotifierAlarm( + m_notifier, true, callback.expirationTime.count(), 0, true, &status); WPILIB_CheckErrorStatus(status, "AcknowledgeNotifierAlarm"); } diff --git a/wpilibc/src/main/native/cpp/system/Notifier.cpp b/wpilibc/src/main/native/cpp/system/Notifier.cpp index 4ceba5d624..b7914f22bc 100644 --- a/wpilibc/src/main/native/cpp/system/Notifier.cpp +++ b/wpilibc/src/main/native/cpp/system/Notifier.cpp @@ -46,7 +46,7 @@ Notifier::Notifier(std::function callback) { } // Ack notifier - HAL_AcknowledgeNotifierAlarm(notifier, &status); + HAL_AcknowledgeNotifierAlarm(notifier, false, 0, 0, false, &status); WPILIB_CheckErrorStatus(status, "AcknowledgeNotifier"); } }); @@ -99,7 +99,7 @@ Notifier::Notifier(int priority, std::function callback) { } // Ack notifier - HAL_AcknowledgeNotifierAlarm(notifier, &status); + HAL_AcknowledgeNotifierAlarm(notifier, false, 0, 0, false, &status); WPILIB_CheckErrorStatus(status, "AcknowledgeNotifier"); } }); diff --git a/wpilibc/src/main/native/cpp/system/Watchdog.cpp b/wpilibc/src/main/native/cpp/system/Watchdog.cpp index 9637669a97..0c2d7228f7 100644 --- a/wpilibc/src/main/native/cpp/system/Watchdog.cpp +++ b/wpilibc/src/main/native/cpp/system/Watchdog.cpp @@ -39,7 +39,7 @@ class Watchdog::Impl { DerefGreater> m_watchdogs; - void UpdateAlarm(); + void UpdateAlarm(bool acknowledge = false); private: void Main(); @@ -67,7 +67,7 @@ Watchdog::Impl::~Impl() { } } -void Watchdog::Impl::UpdateAlarm() { +void Watchdog::Impl::UpdateAlarm(bool acknowledge) { int32_t status = 0; // Return if we are being destructed, or were not created successfully auto notifier = m_notifier.load(); @@ -76,6 +76,12 @@ void Watchdog::Impl::UpdateAlarm() { } if (m_watchdogs.empty()) { HAL_CancelNotifierAlarm(notifier, &status); + } else if (acknowledge) { + HAL_AcknowledgeNotifierAlarm( + notifier, true, + static_cast(m_watchdogs.top()->m_expirationTime.value() * + 1e6), + 0, true, &status); } else { HAL_SetNotifierAlarm(notifier, static_cast( @@ -125,9 +131,7 @@ void Watchdog::Impl::Main() { watchdog->m_callback(); lock.lock(); - UpdateAlarm(); - - HAL_AcknowledgeNotifierAlarm(notifier, &status); + UpdateAlarm(true); } } diff --git a/wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp b/wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp index 86c75f657a..5cb87dceb4 100644 --- a/wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp +++ b/wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp @@ -71,7 +71,7 @@ PyNotifier::PyNotifier(std::function handler) { } // Ack notifier - HAL_AcknowledgeNotifierAlarm(notifier, &status); + HAL_AcknowledgeNotifierAlarm(notifier, false, 0, 0, false, &status); WPILIB_CheckErrorStatus(status, "AcknowledgeNotifier"); } } catch (...) { diff --git a/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java b/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java index abb259ee22..e971c818c8 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java @@ -139,14 +139,11 @@ public class TimedRobot extends IterativeRobotBase { // at the end of the loop. var callback = m_callbacks.poll(); - NotifierJNI.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true); - - // 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; + NotifierJNI.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true); } else { - NotifierJNI.acknowledgeNotifierAlarm(m_notifier); + NotifierJNI.acknowledgeNotifierAlarm(m_notifier, true, callback.expirationTime, 0, true); } try { diff --git a/wpilibj/src/main/java/org/wpilib/system/Notifier.java b/wpilibj/src/main/java/org/wpilib/system/Notifier.java index 10e137798e..b7fae63858 100644 --- a/wpilibj/src/main/java/org/wpilib/system/Notifier.java +++ b/wpilibj/src/main/java/org/wpilib/system/Notifier.java @@ -95,7 +95,7 @@ public class Notifier implements AutoCloseable { } // Acknowledge the alarm - NotifierJNI.acknowledgeNotifierAlarm(notifier); + NotifierJNI.acknowledgeNotifierAlarm(notifier, false, 0, 0, false); } }); m_thread.setName("Notifier"); diff --git a/wpilibj/src/main/java/org/wpilib/system/Watchdog.java b/wpilibj/src/main/java/org/wpilib/system/Watchdog.java index f251150c8d..949a15d4ed 100644 --- a/wpilibj/src/main/java/org/wpilib/system/Watchdog.java +++ b/wpilibj/src/main/java/org/wpilib/system/Watchdog.java @@ -120,7 +120,7 @@ public class Watchdog implements Closeable, Comparable { m_watchdogs.remove(this); m_expirationTime = m_startTime + m_timeout; m_watchdogs.add(this); - updateAlarm(); + updateAlarm(false); } finally { m_queueMutex.unlock(); } @@ -194,7 +194,7 @@ public class Watchdog implements Closeable, Comparable { m_watchdogs.remove(this); m_expirationTime = m_startTime + m_timeout; m_watchdogs.add(this); - updateAlarm(); + updateAlarm(false); } finally { m_queueMutex.unlock(); } @@ -205,7 +205,7 @@ public class Watchdog implements Closeable, Comparable { m_queueMutex.lock(); try { m_watchdogs.remove(this); - updateAlarm(); + updateAlarm(false); } finally { m_queueMutex.unlock(); } @@ -223,9 +223,12 @@ public class Watchdog implements Closeable, Comparable { } @SuppressWarnings("resource") - private static void updateAlarm() { + private static void updateAlarm(boolean acknowledge) { if (m_watchdogs.isEmpty()) { NotifierJNI.cancelNotifierAlarm(m_notifier); + } else if (acknowledge) { + NotifierJNI.acknowledgeNotifierAlarm( + m_notifier, true, (long) (m_watchdogs.peek().m_expirationTime * 1e6), 0, true); } else { NotifierJNI.setNotifierAlarm( m_notifier, (long) (m_watchdogs.peek().m_expirationTime * 1e6), 0, true); @@ -277,9 +280,7 @@ public class Watchdog implements Closeable, Comparable { watchdog.m_callback.run(); m_queueMutex.lock(); - updateAlarm(); - - NotifierJNI.acknowledgeNotifierAlarm(m_notifier); + updateAlarm(true); } finally { m_queueMutex.unlock(); }