Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Change Watchdog to use HAL notifier (#2602)

Browse Source 
This makes it follow simulation timing instead of wall clock time.
This commit is contained in:
Peter Johnson
2020-07-21 22:58:16 -07:00
committed by GitHub
parent 33d8363297
commit 23ba3ca19e
3 changed files with 206 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files

146
wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
View File

@@ -9,10 +9,10 @@ package edu.wpi.first.wpilibj;
import java.io.Closeable;
import java.util.PriorityQueue;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.locks.Condition;
import java.util.concurrent.locks.ReentrantLock;
import edu.wpi.first.hal.NotifierJNI;
/**
* A class that's a wrapper around a watchdog timer.
*
@@ -26,11 +26,11 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
// Used for timeout print rate-limiting
private static final long kMinPrintPeriod = 1000000; // microseconds
private long m_startTime; // microseconds
private long m_timeout; // microseconds
private long m_expirationTime; // microseconds
private double m_startTime; // seconds
private double m_timeout; // seconds
private double m_expirationTime; // seconds
private final Runnable m_callback;
private long m_lastTimeoutPrintTime; // microseconds
private double m_lastTimeoutPrintTime; // seconds
boolean m_isExpired;
@@ -38,13 +38,15 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
private final Tracer m_tracer;
static {
startDaemonThread(Watchdog::schedulerFunc);
}
private static final PriorityQueue<Watchdog> m_watchdogs = new PriorityQueue<>();
private static ReentrantLock m_queueMutex = new ReentrantLock();
private static Condition m_schedulerWaiter = m_queueMutex.newCondition();
private static int m_notifier;
static {
m_notifier = NotifierJNI.initializeNotifier();
NotifierJNI.setNotifierName(m_notifier, "Watchdog");
startDaemonThread(Watchdog::schedulerFunc);
}
/**
* Watchdog constructor.
@@ -53,7 +55,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
* @param callback This function is called when the timeout expires.
*/
public Watchdog(double timeout, Runnable callback) {
m_timeout = (long) (timeout * 1.0e6);
m_timeout = timeout;
m_callback = callback;
m_tracer = new Tracer();
}
@@ -67,14 +69,14 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
public int compareTo(Watchdog rhs) {
// Elements with sooner expiration times are sorted as lesser. The head of
// Java's PriorityQueue is the least element.
return Long.compare(m_expirationTime, rhs.m_expirationTime);
return Double.compare(m_expirationTime, rhs.m_expirationTime);
}
/**
* Returns the time in seconds since the watchdog was last fed.
*/
public double getTime() {
return (RobotController.getFPGATime() - m_startTime) / 1.0e6;
return Timer.getFPGATimestamp() - m_startTime;
}
/**
@@ -84,18 +86,18 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
* resolution.
*/
public void setTimeout(double timeout) {
m_startTime = RobotController.getFPGATime();
m_startTime = Timer.getFPGATimestamp();
m_tracer.clearEpochs();
m_queueMutex.lock();
try {
m_timeout = (long) (timeout * 1.0e6);
m_timeout = timeout;
m_isExpired = false;
m_watchdogs.remove(this);
m_expirationTime = m_startTime + m_timeout;
m_watchdogs.add(this);
m_schedulerWaiter.signalAll();
updateAlarm();
} finally {
m_queueMutex.unlock();
}
@@ -107,7 +109,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
public double getTimeout() {
m_queueMutex.lock();
try {
return m_timeout / 1.0e6;
return m_timeout;
} finally {
m_queueMutex.unlock();
}
@@ -157,7 +159,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
* Enables the watchdog timer.
*/
public void enable() {
m_startTime = RobotController.getFPGATime();
m_startTime = Timer.getFPGATimestamp();
m_tracer.clearEpochs();
m_queueMutex.lock();
@@ -167,7 +169,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
m_watchdogs.remove(this);
m_expirationTime = m_startTime + m_timeout;
m_watchdogs.add(this);
m_schedulerWaiter.signalAll();
updateAlarm();
} finally {
m_queueMutex.unlock();
}
@@ -180,7 +182,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
m_queueMutex.lock();
try {
m_watchdogs.remove(this);
m_schedulerWaiter.signalAll();
updateAlarm();
} finally {
m_queueMutex.unlock();
}
@@ -197,6 +199,15 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
m_suppressTimeoutMessage = suppress;
}
private static void updateAlarm() {
if (m_watchdogs.size() == 0) {
NotifierJNI.cancelNotifierAlarm(m_notifier);
} else {
NotifierJNI.updateNotifierAlarm(m_notifier,
(long) (m_watchdogs.peek().m_expirationTime * 1e6));
}
}
private static Thread startDaemonThread(Runnable target) {
Thread inst = new Thread(target);
inst.setDaemon(true);
@@ -204,74 +215,47 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
return inst;
}
@SuppressWarnings("PMD.AvoidDeeplyNestedIfStmts")
private static void schedulerFunc() {
m_queueMutex.lock();
while (!Thread.currentThread().isInterrupted()) {
long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
if (curTime == 0) {
break;
}
try {
while (!Thread.currentThread().isInterrupted()) {
if (m_watchdogs.size() > 0) {
boolean timedOut = !awaitUntil(m_schedulerWaiter, m_watchdogs.peek().m_expirationTime);
if (timedOut) {
if (m_watchdogs.size() == 0 || m_watchdogs.peek().m_expirationTime
> RobotController.getFPGATime()) {
continue;
}
m_queueMutex.lock();
try {
if (m_watchdogs.size() == 0) {
continue;
}
// If the condition variable timed out, that means a Watchdog timeout
// has occurred, so call its timeout function.
Watchdog watchdog = m_watchdogs.poll();
// If the condition variable timed out, that means a Watchdog timeout
// has occurred, so call its timeout function.
Watchdog watchdog = m_watchdogs.poll();
long now = RobotController.getFPGATime();
if (now - watchdog.m_lastTimeoutPrintTime > kMinPrintPeriod) {
watchdog.m_lastTimeoutPrintTime = now;
if (!watchdog.m_suppressTimeoutMessage) {
DriverStation.reportWarning(
String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout / 1.0e6),
false);
}
}
// Set expiration flag before calling the callback so any
// manipulation of the flag in the callback (e.g., calling
// Disable()) isn't clobbered.
watchdog.m_isExpired = true;
m_queueMutex.unlock();
watchdog.m_callback.run();
m_queueMutex.lock();
}
// 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 {
while (m_watchdogs.size() == 0) {
m_schedulerWaiter.awaitUninterruptibly();
double now = curTime * 1e-6;
if (now - watchdog.m_lastTimeoutPrintTime > kMinPrintPeriod) {
watchdog.m_lastTimeoutPrintTime = now;
if (!watchdog.m_suppressTimeoutMessage) {
DriverStation.reportWarning(
String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout),
false);
}
}
// Set expiration flag before calling the callback so any
// manipulation of the flag in the callback (e.g., calling
// Disable()) isn't clobbered.
watchdog.m_isExpired = true;
m_queueMutex.unlock();
watchdog.m_callback.run();
m_queueMutex.lock();
updateAlarm();
} finally {
m_queueMutex.unlock();
}
} finally {
m_queueMutex.unlock();
}
}
/**
* Wrapper emulating functionality of C++'s std::condition_variable::wait_until().
*
* @param cond The condition variable on which to wait.
* @param time The time at which to stop waiting.
* @return False if the deadline has elapsed upon return, else true.
*/
private static boolean awaitUntil(Condition cond, long time) {
long delta = time - RobotController.getFPGATime();
try {
return cond.await(delta, TimeUnit.MICROSECONDS);
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
ex.printStackTrace();
}
return true;
}
}