Make Watchdog use single thread dispatch (#1347)

Notifier has one thread per instance because the callbacks must be
asynchronous. Watchdog callbacks can be synchronous, so this overhead
can be done away with via a scheduler thread akin to what the HAL
Notifier does.

wpilibc/src/main/native/cpp/IterativeRobotBase.cpp

@@ -23,7 +23,7 @@ using namespace frc;
 
 IterativeRobotBase::IterativeRobotBase(double period)
     : m_period(period),
-      m_watchdog(period, [&] { PrintLoopOverrunMessage(); }) {}
+      m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {}
 
 void IterativeRobotBase::RobotInit() {
   wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";

wpilibc/src/main/native/cpp/Watchdog.cpp

@@ -7,53 +7,156 @@
 
 #include "frc/Watchdog.h"
 
+#include <wpi/Format.h>
+#include <wpi/PriorityQueue.h>
 #include <wpi/raw_ostream.h>
 
-#include "frc/Timer.h"
-
 using namespace frc;
 
-Watchdog::Watchdog(double timeout, std::function<void()> callback)
-    : m_timeout(timeout),
-      m_callback(callback),
-      m_notifier(&Watchdog::TimeoutFunc, this) {
-  Enable();
+class Watchdog::Thread : public wpi::SafeThread {
+ public:
+  template <typename T>
+  struct DerefGreater : public std::binary_function<T, T, bool> {
+    constexpr bool operator()(const T& lhs, const T& rhs) const {
+      return *lhs > *rhs;
+    }
+  };
+
+  wpi::PriorityQueue<Watchdog*, std::vector<Watchdog*>, DerefGreater<Watchdog*>>
+      m_watchdogs;
+
+ private:
+  void Main() override;
+};
+
+void Watchdog::Thread::Main() {
+  std::unique_lock<wpi::mutex> lock(m_mutex);
+
+  while (m_active) {
+    if (m_watchdogs.size() > 0) {
+      if (m_cond.wait_until(lock, m_watchdogs.top()->m_expirationTime) ==
+          std::cv_status::timeout) {
+        if (m_watchdogs.size() == 0 ||
+            m_watchdogs.top()->m_expirationTime > hal::fpga_clock::now()) {
+          continue;
+        }
+
+        // If the condition variable timed out, that means a Watchdog timeout
+        // has occurred, so call its timeout function.
+        auto watchdog = m_watchdogs.top();
+        m_watchdogs.pop();
+
+        wpi::outs() << "Watchdog not fed within "
+                    << wpi::format("%.6f", watchdog->m_timeout.count() / 1.0e6)
+                    << "s\n";
+        lock.unlock();
+        watchdog->m_callback();
+        lock.lock();
+        watchdog->m_isExpired = true;
+      }
+      // Otherwise, a Watchdog removed itself from the queue (it notifies the
+      // scheduler of this) or a spurious wakeup occurred, so just rewait with
+      // the soonest watchdog timeout.
+    } else {
+      m_cond.wait(lock, [&] { return m_watchdogs.size() > 0 || !m_active; });
+    }
+  }
 }
 
+Watchdog::Watchdog(double timeout, std::function<void()> callback)
+    : m_timeout(static_cast<int64_t>(timeout * 1.0e6)),
+      m_callback(callback),
+      m_owner(&GetThreadOwner()) {}
+
+Watchdog::~Watchdog() { Disable(); }
+
 double Watchdog::GetTime() const {
-  return Timer::GetFPGATimestamp() - m_startTime;
+  return (hal::fpga_clock::now() - m_startTime).count() / 1.0e6;
 }
 
-bool Watchdog::IsExpired() const { return m_isExpired; }
+void Watchdog::SetTimeout(double timeout) {
+  m_startTime = hal::fpga_clock::now();
+  m_epochs.clear();
+
+  // Locks mutex
+  auto thr = m_owner->GetThread();
+  if (!thr) return;
+
+  m_timeout = std::chrono::microseconds(static_cast<int64_t>(timeout * 1.0e6));
+  m_isExpired = false;
+
+  thr->m_watchdogs.remove(this);
+  m_expirationTime = m_startTime + m_timeout;
+  thr->m_watchdogs.emplace(this);
+  thr->m_cond.notify_all();
+}
+
+double Watchdog::GetTimeout() const {
+  // Locks mutex
+  auto thr = m_owner->GetThread();
+
+  return m_timeout.count() / 1.0e6;
+}
+
+bool Watchdog::IsExpired() const {
+  // Locks mutex
+  auto thr = m_owner->GetThread();
+
+  return m_isExpired;
+}
 
 void Watchdog::AddEpoch(wpi::StringRef epochName) {
-  double currentTime = Timer::GetFPGATimestamp();
+  auto currentTime = hal::fpga_clock::now();
   m_epochs[epochName] = currentTime - m_startTime;
   m_startTime = currentTime;
 }
 
 void Watchdog::PrintEpochs() {
   for (const auto& epoch : m_epochs) {
-    wpi::outs() << "\t" << epoch.getKey() << ": " << epoch.getValue() << "s\n";
+    wpi::outs() << '\t' << epoch.getKey() << ": "
+                << wpi::format("%.6f", epoch.getValue().count() / 1.0e6)
+                << "s\n";
   }
 }
 
 void Watchdog::Reset() { Enable(); }
 
 void Watchdog::Enable() {
-  m_startTime = Timer::GetFPGATimestamp();
-  m_isExpired = false;
+  m_startTime = hal::fpga_clock::now();
   m_epochs.clear();
-  m_notifier.StartPeriodic(m_timeout);
+
+  // Locks mutex
+  auto thr = m_owner->GetThread();
+  if (!thr) return;
+
+  m_isExpired = false;
+
+  thr->m_watchdogs.remove(this);
+  m_expirationTime = m_startTime + m_timeout;
+  thr->m_watchdogs.emplace(this);
+  thr->m_cond.notify_all();
 }
 
-void Watchdog::Disable() { m_notifier.Stop(); }
+void Watchdog::Disable() {
+  // Locks mutex
+  auto thr = m_owner->GetThread();
+  if (!thr) return;
 
-void Watchdog::TimeoutFunc() {
-  if (!m_isExpired) {
-    wpi::outs() << "Watchdog not fed after " << m_timeout << "s\n";
-    m_callback();
-    m_isExpired = true;
-    Disable();
-  }
+  m_isExpired = false;
+
+  thr->m_watchdogs.remove(this);
+  thr->m_cond.notify_all();
+}
+
+bool Watchdog::operator>(const Watchdog& rhs) {
+  return m_expirationTime > rhs.m_expirationTime;
+}
+
+wpi::SafeThreadOwner<Watchdog::Thread>& Watchdog::GetThreadOwner() {
+  static wpi::SafeThreadOwner<Thread> inst = [] {
+    wpi::SafeThreadOwner<Watchdog::Thread> inst;
+    inst.Start();
+    return inst;
+  }();
+  return inst;
 }

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

@@ -7,13 +7,14 @@
 
 #pragma once
 
+#include <chrono>
 #include <functional>
 
+#include <hal/cpp/fpga_clock.h>
+#include <wpi/SafeThread.h>
 #include <wpi/StringMap.h>
 #include <wpi/StringRef.h>
 
-#include "frc/Notifier.h"
-
 namespace frc {
 
 /**
@@ -30,19 +31,35 @@ class Watchdog {
   /**
    * Watchdog constructor.
    *
-   * @param timeout  The watchdog's timeout in seconds.
+   * @param timeout  The watchdog's timeout in seconds with microsecond
+   *                 resolution.
    * @param callback This function is called when the timeout expires.
    */
-  explicit Watchdog(double timeout, std::function<void()> callback = [] {});
+  Watchdog(double timeout, std::function<void()> callback);
+
+  ~Watchdog();
 
   Watchdog(Watchdog&&) = default;
   Watchdog& operator=(Watchdog&&) = default;
 
   /**
-   * Get the time in seconds since the watchdog was last fed.
+   * Returns the time in seconds since the watchdog was last fed.
    */
   double GetTime() const;
 
+  /**
+   * Sets the watchdog's timeout.
+   *
+   * @param timeout The watchdog's timeout in seconds with microsecond
+   *                resolution.
+   */
+  void SetTimeout(double timeout);
+
+  /**
+   * Returns the watchdog's timeout in seconds.
+   */
+  double GetTimeout() const;
+
   /**
    * Returns true if the watchdog timer has expired.
    */
@@ -76,20 +93,25 @@ class Watchdog {
   void Enable();
 
   /**
-   * Disable the watchdog.
+   * Disables the watchdog timer.
    */
   void Disable();
 
  private:
-  double m_timeout;
+  hal::fpga_clock::time_point m_startTime;
+  std::chrono::microseconds m_timeout;
+  hal::fpga_clock::time_point m_expirationTime;
   std::function<void()> m_callback;
-  Notifier m_notifier;
 
-  double m_startTime = 0.0;
-  wpi::StringMap<double> m_epochs;
+  wpi::StringMap<std::chrono::microseconds> m_epochs;
   bool m_isExpired = false;
 
-  void TimeoutFunc();
+  class Thread;
+  wpi::SafeThreadOwner<Thread>* m_owner;
+
+  bool operator>(const Watchdog& rhs);
+
+  static wpi::SafeThreadOwner<Thread>& GetThreadOwner();
 };
 
 }  // namespace frc