[wpilibc] Add real-time priority constructor to Notifier (#2303)

Using this overload makes the thread backing the Notifier run at
real-time priority. This improves scheduling jitter substantially (5ms
+- 2ms down to 5ms +- 1ms).

A version isn't provided for Java because making threads real-time can
cause GC deadlocks.

wpilibc/src/main/native/cpp/Notifier.cpp

@@ -11,6 +11,7 @@
 
 #include <hal/FRCUsageReporting.h>
 #include <hal/Notifier.h>
+#include <hal/Threads.h>
 #include <wpi/SmallString.h>
 
 #include "frc/Timer.h"
@@ -54,6 +55,42 @@ Notifier::Notifier(std::function<void()> handler) {
   });
 }
 
+Notifier::Notifier(int priority, std::function<void()> 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<void()> 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

wpilibc/src/main/native/include/frc/Notifier.h

@@ -12,6 +12,7 @@
 #include <atomic>
 #include <functional>
 #include <thread>
+#include <type_traits>
 #include <utility>
 
 #include <hal/Types.h>
@@ -34,11 +35,33 @@ class Notifier : public ErrorBase {
    */
   explicit Notifier(std::function<void()> handler);
 
-  template <typename Callable, typename Arg, typename... Args>
+  template <
+      typename Callable, typename Arg, typename... Args,
+      typename = std::enable_if_t<std::is_invocable_v<Callable, Arg, Args...>>>
   Notifier(Callable&& f, Arg&& arg, Args&&... args)
       : Notifier(std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
                            std::forward<Args>(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<void()> handler);
+
+  template <typename Callable, typename Arg, typename... Args>
+  Notifier(int priority, Callable&& f, Arg&& arg, Args&&... args)
+      : Notifier(priority,
+                 std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
+                           std::forward<Args>(args)...)) {}
+
   /**
    * Free the resources for a timer event.
    */