diff --git a/wpilibc/src/main/native/cpp/Watchdog.cpp b/wpilibc/src/main/native/cpp/Watchdog.cpp index 69bdf71cb6..2180d0df3c 100644 --- a/wpilibc/src/main/native/cpp/Watchdog.cpp +++ b/wpilibc/src/main/native/cpp/Watchdog.cpp @@ -7,19 +7,24 @@ #include "frc/Watchdog.h" +#include + +#include #include #include #include #include #include "frc/DriverStation.h" +#include "frc2/Timer.h" using namespace frc; -constexpr std::chrono::milliseconds Watchdog::kMinPrintPeriod; - -class Watchdog::Thread : public wpi::SafeThread { +class Watchdog::Impl { public: + Impl(); + ~Impl(); + template struct DerefGreater { constexpr bool operator()(const T& lhs, const T& rhs) const { @@ -27,58 +32,96 @@ class Watchdog::Thread : public wpi::SafeThread { } }; + wpi::mutex m_mutex; + std::atomic m_notifier; wpi::PriorityQueue, DerefGreater> m_watchdogs; + void UpdateAlarm(); + private: - void Main() override; + void Main(); + + std::thread m_thread; }; -void Watchdog::Thread::Main() { - std::unique_lock lock(m_mutex); +Watchdog::Impl::Impl() { + int32_t status = 0; + m_notifier = HAL_InitializeNotifier(&status); + wpi_setGlobalHALError(status); + HAL_SetNotifierName(m_notifier, "Watchdog", &status); - 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; - } + m_thread = std::thread([=] { Main(); }); +} - // 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(); +Watchdog::Impl::~Impl() { + int32_t status = 0; + // atomically set handle to 0, then clean + HAL_NotifierHandle handle = m_notifier.exchange(0); + HAL_StopNotifier(handle, &status); + wpi_setGlobalHALError(status); - auto now = hal::fpga_clock::now(); - if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) { - watchdog->m_lastTimeoutPrintTime = now; - if (!watchdog->m_suppressTimeoutMessage) { - wpi::SmallString<128> buf; - wpi::raw_svector_ostream err(buf); - err << "Watchdog not fed within " - << wpi::format("%.6f", watchdog->m_timeout.count() / 1.0e9) - << "s\n"; - frc::DriverStation::ReportWarning(err.str()); - } - } + // Join the thread to ensure the handler has exited. + if (m_thread.joinable()) m_thread.join(); - // 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; + HAL_CleanNotifier(handle, &status); +} - lock.unlock(); - watchdog->m_callback(); - lock.lock(); +void Watchdog::Impl::UpdateAlarm() { + int32_t status = 0; + // Return if we are being destructed, or were not created successfully + auto notifier = m_notifier.load(); + if (notifier == 0) return; + if (m_watchdogs.empty()) + HAL_CancelNotifierAlarm(notifier, &status); + else + HAL_UpdateNotifierAlarm( + notifier, + static_cast(m_watchdogs.top()->m_expirationTime.to() * + 1e6), + &status); + wpi_setGlobalHALError(status); +} + +void Watchdog::Impl::Main() { + for (;;) { + int32_t status = 0; + HAL_NotifierHandle notifier = m_notifier.load(); + if (notifier == 0) break; + uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status); + if (curTime == 0 || status != 0) break; + + std::unique_lock lock(m_mutex); + + if (m_watchdogs.empty()) 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(); + + units::second_t now{curTime * 1e-6}; + if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) { + watchdog->m_lastTimeoutPrintTime = now; + if (!watchdog->m_suppressTimeoutMessage) { + wpi::SmallString<128> buf; + wpi::raw_svector_ostream err(buf); + err << "Watchdog not fed within " + << wpi::format("%.6f", watchdog->m_timeout.to()) << "s\n"; + frc::DriverStation::ReportWarning(err.str()); } - // 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; }); } + + // 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; + + lock.unlock(); + watchdog->m_callback(); + lock.lock(); + + UpdateAlarm(); } } @@ -86,12 +129,32 @@ Watchdog::Watchdog(double timeout, std::function callback) : Watchdog(units::second_t{timeout}, callback) {} Watchdog::Watchdog(units::second_t timeout, std::function callback) - : m_timeout(timeout), m_callback(callback), m_owner(&GetThreadOwner()) {} + : m_timeout(timeout), m_callback(callback), m_impl(GetImpl()) {} Watchdog::~Watchdog() { Disable(); } +Watchdog::Watchdog(Watchdog&& rhs) { *this = std::move(rhs); } + +Watchdog& Watchdog::operator=(Watchdog&& rhs) { + m_impl = rhs.m_impl; + std::scoped_lock lock(m_impl->m_mutex); + m_startTime = rhs.m_startTime; + m_timeout = rhs.m_timeout; + m_expirationTime = rhs.m_expirationTime; + m_callback = std::move(rhs.m_callback); + m_lastTimeoutPrintTime = rhs.m_lastTimeoutPrintTime; + m_suppressTimeoutMessage = rhs.m_suppressTimeoutMessage; + m_tracer = std::move(rhs.m_tracer); + m_isExpired = rhs.m_isExpired; + if (m_expirationTime != 0_s) { + m_impl->m_watchdogs.remove(&rhs); + m_impl->m_watchdogs.emplace(this); + } + return *this; +} + double Watchdog::GetTime() const { - return (hal::fpga_clock::now() - m_startTime).count() / 1.0e6; + return (frc2::Timer::GetFPGATimestamp() - m_startTime).to(); } void Watchdog::SetTimeout(double timeout) { @@ -99,36 +162,26 @@ void Watchdog::SetTimeout(double timeout) { } void Watchdog::SetTimeout(units::second_t timeout) { - using std::chrono::duration_cast; - using std::chrono::microseconds; - - m_startTime = hal::fpga_clock::now(); + m_startTime = frc2::Timer::GetFPGATimestamp(); m_tracer.ClearEpochs(); - // Locks mutex - auto thr = m_owner->GetThread(); - if (!thr) return; - + std::scoped_lock lock(m_impl->m_mutex); m_timeout = timeout; m_isExpired = false; - thr->m_watchdogs.remove(this); - m_expirationTime = m_startTime + duration_cast(m_timeout); - thr->m_watchdogs.emplace(this); - thr->m_cond.notify_all(); + m_impl->m_watchdogs.remove(this); + m_expirationTime = m_startTime + m_timeout; + m_impl->m_watchdogs.emplace(this); + m_impl->UpdateAlarm(); } double Watchdog::GetTimeout() const { - // Locks mutex - auto thr = m_owner->GetThread(); - - return m_timeout.count() / 1.0e9; + std::scoped_lock lock(m_impl->m_mutex); + return m_timeout.to(); } bool Watchdog::IsExpired() const { - // Locks mutex - auto thr = m_owner->GetThread(); - + std::scoped_lock lock(m_impl->m_mutex); return m_isExpired; } @@ -141,31 +194,26 @@ void Watchdog::PrintEpochs() { m_tracer.PrintEpochs(); } void Watchdog::Reset() { Enable(); } void Watchdog::Enable() { - using std::chrono::duration_cast; - using std::chrono::microseconds; - - m_startTime = hal::fpga_clock::now(); + m_startTime = frc2::Timer::GetFPGATimestamp(); m_tracer.ClearEpochs(); - // Locks mutex - auto thr = m_owner->GetThread(); - if (!thr) return; - + std::scoped_lock lock(m_impl->m_mutex); m_isExpired = false; - thr->m_watchdogs.remove(this); - m_expirationTime = m_startTime + duration_cast(m_timeout); - thr->m_watchdogs.emplace(this); - thr->m_cond.notify_all(); + m_impl->m_watchdogs.remove(this); + m_expirationTime = m_startTime + m_timeout; + m_impl->m_watchdogs.emplace(this); + m_impl->UpdateAlarm(); } void Watchdog::Disable() { - // Locks mutex - auto thr = m_owner->GetThread(); - if (!thr) return; + std::scoped_lock lock(m_impl->m_mutex); - thr->m_watchdogs.remove(this); - thr->m_cond.notify_all(); + if (m_expirationTime != 0_s) { + m_impl->m_watchdogs.remove(this); + m_expirationTime = 0_s; + m_impl->UpdateAlarm(); + } } void Watchdog::SuppressTimeoutMessage(bool suppress) { @@ -176,11 +224,7 @@ bool Watchdog::operator>(const Watchdog& rhs) { return m_expirationTime > rhs.m_expirationTime; } -wpi::SafeThreadOwner& Watchdog::GetThreadOwner() { - static wpi::SafeThreadOwner inst = [] { - wpi::SafeThreadOwner inst; - inst.Start(); - return inst; - }(); - return inst; +Watchdog::Impl* Watchdog::GetImpl() { + static Impl inst; + return &inst; } diff --git a/wpilibc/src/main/native/include/frc/Watchdog.h b/wpilibc/src/main/native/include/frc/Watchdog.h index 4323b9a7a9..7e0caba8e5 100644 --- a/wpilibc/src/main/native/include/frc/Watchdog.h +++ b/wpilibc/src/main/native/include/frc/Watchdog.h @@ -7,13 +7,10 @@ #pragma once -#include #include #include -#include #include -#include #include #include @@ -67,8 +64,8 @@ class Watchdog { ~Watchdog(); - Watchdog(Watchdog&&) = default; - Watchdog& operator=(Watchdog&&) = default; + Watchdog(Watchdog&& rhs); + Watchdog& operator=(Watchdog&& rhs); /** * Returns the time in seconds since the watchdog was last fed. @@ -149,25 +146,25 @@ class Watchdog { private: // Used for timeout print rate-limiting - static constexpr std::chrono::milliseconds kMinPrintPeriod{1000}; + static constexpr units::second_t kMinPrintPeriod = 1_s; - hal::fpga_clock::time_point m_startTime; - std::chrono::nanoseconds m_timeout; - hal::fpga_clock::time_point m_expirationTime; + units::second_t m_startTime = 0_s; + units::second_t m_timeout; + units::second_t m_expirationTime = 0_s; std::function m_callback; - hal::fpga_clock::time_point m_lastTimeoutPrintTime = hal::fpga_clock::epoch(); + units::second_t m_lastTimeoutPrintTime = 0_s; Tracer m_tracer; bool m_isExpired = false; bool m_suppressTimeoutMessage = false; - class Thread; - wpi::SafeThreadOwner* m_owner; + class Impl; + Impl* m_impl; bool operator>(const Watchdog& rhs); - static wpi::SafeThreadOwner& GetThreadOwner(); + static Impl* GetImpl(); }; } // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java index afb6cbe1f8..c8795ed5cf 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -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 { // 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 { private final Tracer m_tracer; - static { - startDaemonThread(Watchdog::schedulerFunc); - } - private static final PriorityQueue 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 { * @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 { 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 { * 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 { 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 { * 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 { 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 { 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 { 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 { 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; - } }