Rate-limit Watchdog timeout prints to 1Hz (#1459)

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

@@ -13,6 +13,8 @@
 
 using namespace frc;
 
+constexpr std::chrono::milliseconds Watchdog::kMinPrintPeriod;
+
 class Watchdog::Thread : public wpi::SafeThread {
  public:
   template <typename T>
@@ -46,9 +48,14 @@ void Watchdog::Thread::Main() {
         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";
+        auto now = hal::fpga_clock::now();
+        if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) {
+          watchdog->m_lastTimeoutPrintTime = now;
+          wpi::outs() << "Watchdog not fed within "
+                      << wpi::format("%.6f",
+                                     watchdog->m_timeout.count() / 1.0e6)
+                      << "s\n";
+        }
         lock.unlock();
         watchdog->m_callback();
         lock.lock();
@@ -112,10 +119,14 @@ void Watchdog::AddEpoch(wpi::StringRef epochName) {
 }
 
 void Watchdog::PrintEpochs() {
-  for (const auto& epoch : m_epochs) {
-    wpi::outs() << '\t' << epoch.getKey() << ": "
-                << wpi::format("%.6f", epoch.getValue().count() / 1.0e6)
-                << "s\n";
+  auto now = hal::fpga_clock::now();
+  if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
+    m_lastEpochsPrintTime = now;
+    for (const auto& epoch : m_epochs) {
+      wpi::outs() << '\t' << epoch.getKey() << ": "
+                  << wpi::format("%.6f", epoch.getValue().count() / 1.0e6)
+                  << "s\n";
+    }
   }
 }
 

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

@@ -9,6 +9,7 @@
 
 #include <chrono>
 #include <functional>
+#include <utility>
 
 #include <hal/cpp/fpga_clock.h>
 #include <wpi/SafeThread.h>
@@ -37,6 +38,12 @@ class Watchdog {
    */
   Watchdog(double timeout, std::function<void()> callback);
 
+  template <typename Callable, typename Arg, typename... Args>
+  Watchdog(double timeout, Callable&& f, Arg&& arg, Args&&... args)
+      : Watchdog(timeout,
+                 std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
+                           std::forward<Args>(args)...)) {}
+
   ~Watchdog();
 
   Watchdog(Watchdog&&) = default;
@@ -98,10 +105,15 @@ class Watchdog {
   void Disable();
 
  private:
+  // Used for timeout print rate-limiting
+  static constexpr std::chrono::milliseconds kMinPrintPeriod{1000};
+
   hal::fpga_clock::time_point m_startTime;
   std::chrono::microseconds m_timeout;
   hal::fpga_clock::time_point m_expirationTime;
   std::function<void()> m_callback;
+  hal::fpga_clock::time_point m_lastTimeoutPrintTime = hal::fpga_clock::epoch();
+  hal::fpga_clock::time_point m_lastEpochsPrintTime = hal::fpga_clock::epoch();
 
   wpi::StringMap<std::chrono::microseconds> m_epochs;
   bool m_isExpired = false;

wpilibc/src/test/native/cpp/WatchdogTest.cpp

@@ -103,7 +103,6 @@ TEST(WatchdogTest, Epochs) {
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
   watchdog.AddEpoch("Epoch 3");
   watchdog.Disable();
-  watchdog.PrintEpochs();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 
@@ -115,7 +114,6 @@ TEST(WatchdogTest, Epochs) {
   std::this_thread::sleep_for(std::chrono::milliseconds(200));
   watchdog.AddEpoch("Epoch 2");
   watchdog.Disable();
-  watchdog.PrintEpochs();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java

@@ -24,10 +24,15 @@ import java.util.concurrent.locks.ReentrantLock;
  * <p>The watchdog is initialized disabled, so the user needs to call enable() before use.
  */
 public class Watchdog implements Closeable, Comparable<Watchdog> {
+  // Used for timeout print rate-limiting
+  private static final long kMinPrintPeriod = 1000000; // us
+
   private long m_startTime; // us
   private long m_timeout; // us
   private long m_expirationTime; // us
   private final Runnable m_callback;
+  private long m_lastTimeoutPrintTime; // us
+  private long m_lastEpochsPrintTime; // us
 
   @SuppressWarnings("PMD.UseConcurrentHashMap")
   private final Map<String, Long> m_epochs = new HashMap<>();
@@ -143,9 +148,13 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
    * Prints list of epochs added so far and their times.
    */
   public void printEpochs() {
-    m_epochs.forEach((key, value) -> {
-      System.out.format("\t" + key + ": %.6fs\n", value / 1.0e6);
-    });
+    long now = RobotController.getFPGATime();
+    if (now  - m_lastEpochsPrintTime > kMinPrintPeriod) {
+      m_lastEpochsPrintTime = now;
+      m_epochs.forEach((key, value) -> {
+        System.out.format("\t" + key + ": %.6fs\n", value / 1.0e6);
+      });
+    }
   }
 
   /**
@@ -217,7 +226,11 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
             // has occurred, so call its timeout function.
             Watchdog watchdog = m_watchdogs.poll();
 
-            System.out.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout / 1.0e6);
+            long now = RobotController.getFPGATime();
+            if (now  - watchdog.m_lastTimeoutPrintTime > kMinPrintPeriod) {
+              watchdog.m_lastTimeoutPrintTime = now;
+              System.out.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout / 1.0e6);
+            }
             m_queueMutex.unlock();
             watchdog.m_callback.run();
             m_queueMutex.lock();

wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java

@@ -24,7 +24,7 @@ class WatchdogTest {
       watchdogCounter.addAndGet(1);
     });
 
-    // Run 1
+    System.out.println("Run 1");
     watchdog.enable();
     try {
       Thread.sleep(200);
@@ -35,7 +35,7 @@ class WatchdogTest {
 
     assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
 
-    // Run 2
+    System.out.println("Run 2");
     watchdogCounter.set(0);
     watchdog.enable();
     try {
@@ -140,7 +140,7 @@ class WatchdogTest {
       watchdogCounter.addAndGet(1);
     });
 
-    // Run 1
+    System.out.println("Run 1");
     watchdog.enable();
     watchdog.addEpoch("Epoch 1");
     try {
@@ -156,11 +156,10 @@ class WatchdogTest {
     }
     watchdog.addEpoch("Epoch 3");
     watchdog.disable();
-    watchdog.printEpochs();
 
     assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
 
-    // Run 2
+    System.out.println("Run 2");
     watchdog.enable();
     watchdog.addEpoch("Epoch 1");
     try {
@@ -176,7 +175,6 @@ class WatchdogTest {
     }
     watchdog.addEpoch("Epoch 2");
     watchdog.disable();
-    watchdog.printEpochs();
 
     assertEquals(0, watchdogCounter.get(), "Watchdog triggered early");
   }