001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj;
006
007import edu.wpi.first.hal.NotifierJNI;
008import java.io.Closeable;
009import java.util.PriorityQueue;
010import java.util.concurrent.locks.ReentrantLock;
011
012/**
013 * A class that's a wrapper around a watchdog timer.
014 *
015 * <p>When the timer expires, a message is printed to the console and an optional user-provided
016 * callback is invoked.
017 *
018 * <p>The watchdog is initialized disabled, so the user needs to call enable() before use.
019 */
020public class Watchdog implements Closeable, Comparable<Watchdog> {
021  // Used for timeout print rate-limiting
022  private static final long kMinPrintPeriodMicroS = (long) 1e6;
023
024  private double m_startTimeSeconds;
025  private double m_timeoutSeconds;
026  private double m_expirationTimeSeconds;
027  private final Runnable m_callback;
028  private double m_lastTimeoutPrintSeconds;
029
030  boolean m_isExpired;
031
032  boolean m_suppressTimeoutMessage;
033
034  private final Tracer m_tracer;
035
036  private static final PriorityQueue<Watchdog> m_watchdogs = new PriorityQueue<>();
037  private static ReentrantLock m_queueMutex = new ReentrantLock();
038  private static int m_notifier;
039
040  static {
041    m_notifier = NotifierJNI.initializeNotifier();
042    NotifierJNI.setNotifierName(m_notifier, "Watchdog");
043    startDaemonThread(Watchdog::schedulerFunc);
044  }
045
046  /**
047   * Watchdog constructor.
048   *
049   * @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution.
050   * @param callback This function is called when the timeout expires.
051   */
052  public Watchdog(double timeoutSeconds, Runnable callback) {
053    m_timeoutSeconds = timeoutSeconds;
054    m_callback = callback;
055    m_tracer = new Tracer();
056  }
057
058  @Override
059  public void close() {
060    disable();
061  }
062
063  @Override
064  public boolean equals(Object obj) {
065    if (obj instanceof Watchdog) {
066      return Double.compare(m_expirationTimeSeconds, ((Watchdog) obj).m_expirationTimeSeconds) == 0;
067    }
068    return false;
069  }
070
071  @Override
072  public int hashCode() {
073    return Double.hashCode(m_expirationTimeSeconds);
074  }
075
076  @Override
077  public int compareTo(Watchdog rhs) {
078    // Elements with sooner expiration times are sorted as lesser. The head of
079    // Java's PriorityQueue is the least element.
080    return Double.compare(m_expirationTimeSeconds, rhs.m_expirationTimeSeconds);
081  }
082
083  /**
084   * Returns the time in seconds since the watchdog was last fed.
085   *
086   * @return The time in seconds since the watchdog was last fed.
087   */
088  public double getTime() {
089    return Timer.getFPGATimestamp() - m_startTimeSeconds;
090  }
091
092  /**
093   * Sets the watchdog's timeout.
094   *
095   * @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution.
096   */
097  public void setTimeout(double timeoutSeconds) {
098    m_startTimeSeconds = Timer.getFPGATimestamp();
099    m_tracer.clearEpochs();
100
101    m_queueMutex.lock();
102    try {
103      m_timeoutSeconds = timeoutSeconds;
104      m_isExpired = false;
105
106      m_watchdogs.remove(this);
107      m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds;
108      m_watchdogs.add(this);
109      updateAlarm();
110    } finally {
111      m_queueMutex.unlock();
112    }
113  }
114
115  /**
116   * Returns the watchdog's timeout in seconds.
117   *
118   * @return The watchdog's timeout in seconds.
119   */
120  public double getTimeout() {
121    m_queueMutex.lock();
122    try {
123      return m_timeoutSeconds;
124    } finally {
125      m_queueMutex.unlock();
126    }
127  }
128
129  /**
130   * Returns true if the watchdog timer has expired.
131   *
132   * @return True if the watchdog timer has expired.
133   */
134  public boolean isExpired() {
135    m_queueMutex.lock();
136    try {
137      return m_isExpired;
138    } finally {
139      m_queueMutex.unlock();
140    }
141  }
142
143  /**
144   * Adds time since last epoch to the list printed by printEpochs().
145   *
146   * @see Tracer#addEpoch(String)
147   * @param epochName The name to associate with the epoch.
148   */
149  public void addEpoch(String epochName) {
150    m_tracer.addEpoch(epochName);
151  }
152
153  /**
154   * Prints list of epochs added so far and their times.
155   *
156   * @see Tracer#printEpochs()
157   */
158  public void printEpochs() {
159    m_tracer.printEpochs();
160  }
161
162  /**
163   * Resets the watchdog timer.
164   *
165   * <p>This also enables the timer if it was previously disabled.
166   */
167  public void reset() {
168    enable();
169  }
170
171  /** Enables the watchdog timer. */
172  public void enable() {
173    m_startTimeSeconds = Timer.getFPGATimestamp();
174    m_tracer.clearEpochs();
175
176    m_queueMutex.lock();
177    try {
178      m_isExpired = false;
179
180      m_watchdogs.remove(this);
181      m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds;
182      m_watchdogs.add(this);
183      updateAlarm();
184    } finally {
185      m_queueMutex.unlock();
186    }
187  }
188
189  /** Disables the watchdog timer. */
190  public void disable() {
191    m_queueMutex.lock();
192    try {
193      m_watchdogs.remove(this);
194      updateAlarm();
195    } finally {
196      m_queueMutex.unlock();
197    }
198  }
199
200  /**
201   * Enable or disable suppression of the generic timeout message.
202   *
203   * <p>This may be desirable if the user-provided callback already prints a more specific message.
204   *
205   * @param suppress Whether to suppress generic timeout message.
206   */
207  public void suppressTimeoutMessage(boolean suppress) {
208    m_suppressTimeoutMessage = suppress;
209  }
210
211  @SuppressWarnings("resource")
212  private static void updateAlarm() {
213    if (m_watchdogs.isEmpty()) {
214      NotifierJNI.cancelNotifierAlarm(m_notifier);
215    } else {
216      NotifierJNI.updateNotifierAlarm(
217          m_notifier, (long) (m_watchdogs.peek().m_expirationTimeSeconds * 1e6));
218    }
219  }
220
221  private static Thread startDaemonThread(Runnable target) {
222    Thread inst = new Thread(target);
223    inst.setDaemon(true);
224    inst.start();
225    return inst;
226  }
227
228  private static void schedulerFunc() {
229    while (!Thread.currentThread().isInterrupted()) {
230      long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
231      if (curTime == 0) {
232        break;
233      }
234
235      m_queueMutex.lock();
236      try {
237        if (m_watchdogs.isEmpty()) {
238          continue;
239        }
240
241        // If the condition variable timed out, that means a Watchdog timeout
242        // has occurred, so call its timeout function.
243        Watchdog watchdog = m_watchdogs.poll();
244
245        double now = curTime * 1e-6;
246        if (now - watchdog.m_lastTimeoutPrintSeconds > kMinPrintPeriodMicroS) {
247          watchdog.m_lastTimeoutPrintSeconds = now;
248          if (!watchdog.m_suppressTimeoutMessage) {
249            DriverStation.reportWarning(
250                String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeoutSeconds), false);
251          }
252        }
253
254        // Set expiration flag before calling the callback so any
255        // manipulation of the flag in the callback (e.g., calling
256        // Disable()) isn't clobbered.
257        watchdog.m_isExpired = true;
258
259        m_queueMutex.unlock();
260        watchdog.m_callback.run();
261        m_queueMutex.lock();
262
263        updateAlarm();
264      } finally {
265        m_queueMutex.unlock();
266      }
267    }
268  }
269}