[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