diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp index 3065e655cd..4280ce46f5 100644 --- a/wpilibc/src/main/native/cpp/Notifier.cpp +++ b/wpilibc/src/main/native/cpp/Notifier.cpp @@ -11,6 +11,7 @@ #include #include +#include #include #include "frc/Timer.h" @@ -54,6 +55,42 @@ Notifier::Notifier(std::function handler) { }); } +Notifier::Notifier(int priority, std::function handler) { + if (handler == nullptr) + wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr"); + m_handler = handler; + int32_t status = 0; + m_notifier = HAL_InitializeNotifier(&status); + wpi_setHALError(status); + + m_thread = std::thread([=] { + int32_t status = 0; + HAL_SetCurrentThreadPriority(true, priority, &status); + for (;;) { + HAL_NotifierHandle notifier = m_notifier.load(); + if (notifier == 0) break; + uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status); + if (curTime == 0 || status != 0) break; + + 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 + if (handler) handler(); + } + }); +} + Notifier::~Notifier() { int32_t status = 0; // atomically set handle to 0, then clean diff --git a/wpilibc/src/main/native/include/frc/Notifier.h b/wpilibc/src/main/native/include/frc/Notifier.h index 5079fa791d..bdd440a388 100644 --- a/wpilibc/src/main/native/include/frc/Notifier.h +++ b/wpilibc/src/main/native/include/frc/Notifier.h @@ -12,6 +12,7 @@ #include #include #include +#include #include #include @@ -34,11 +35,33 @@ class Notifier : public ErrorBase { */ explicit Notifier(std::function handler); - template + template < + typename Callable, typename Arg, typename... Args, + typename = std::enable_if_t>> Notifier(Callable&& f, Arg&& arg, Args&&... args) : Notifier(std::bind(std::forward(f), std::forward(arg), std::forward(args)...)) {} + /** + * Create a Notifier for timer event notification. + * + * This overload makes the underlying thread run with a real-time priority. + * This is useful for reducing scheduling jitter on processes which are + * sensitive to timing variance, like model-based control. + * + * @param priority The FIFO real-time scheduler priority ([0..100] where a + * lower number represents higher priority). + * @param handler The handler is called at the notification time which is set + * using StartSingle or StartPeriodic. + */ + explicit Notifier(int priority, std::function handler); + + template + Notifier(int priority, Callable&& f, Arg&& arg, Args&&... args) + : Notifier(priority, + std::bind(std::forward(f), std::forward(arg), + std::forward(args)...)) {} + /** * Free the resources for a timer event. */