Rename our allwpilib (which is now 2020) to not have 2019 in the name
Change-Id: I3c07f85ed32ab8b97db765a9b43f2a6ce7da964a
diff --git a/wpilibc/src/main/native/cpp/Watchdog.cpp b/wpilibc/src/main/native/cpp/Watchdog.cpp
new file mode 100644
index 0000000..e5be4fa
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Watchdog.cpp
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Watchdog.h"
+
+#include <wpi/Format.h>
+#include <wpi/PriorityQueue.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+constexpr std::chrono::milliseconds Watchdog::kMinPrintPeriod;
+
+class Watchdog::Thread : public wpi::SafeThread {
+ public:
+ template <typename T>
+ struct DerefGreater {
+ 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 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();
+
+ auto now = hal::fpga_clock::now();
+ if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) {
+ watchdog->m_lastTimeoutPrintTime = now;
+ if (!watchdog->m_suppressTimeoutMessage) {
+ wpi::outs() << "Watchdog not fed within "
+ << wpi::format("%.6f",
+ watchdog->m_timeout.count() / 1.0e9)
+ << "s\n";
+ }
+ }
+
+ // 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();
+ }
+ // 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)
+ : Watchdog(units::second_t{timeout}, callback) {}
+
+Watchdog::Watchdog(units::second_t timeout, std::function<void()> callback)
+ : m_timeout(timeout), m_callback(callback), m_owner(&GetThreadOwner()) {}
+
+Watchdog::~Watchdog() { Disable(); }
+
+double Watchdog::GetTime() const {
+ return (hal::fpga_clock::now() - m_startTime).count() / 1.0e6;
+}
+
+void Watchdog::SetTimeout(double timeout) {
+ SetTimeout(units::second_t{timeout});
+}
+
+void Watchdog::SetTimeout(units::second_t timeout) {
+ using std::chrono::duration_cast;
+ using std::chrono::microseconds;
+
+ m_startTime = hal::fpga_clock::now();
+ m_epochs.clear();
+
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+ if (!thr) return;
+
+ m_timeout = timeout;
+ m_isExpired = false;
+
+ thr->m_watchdogs.remove(this);
+ m_expirationTime = m_startTime + duration_cast<microseconds>(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.0e9;
+}
+
+bool Watchdog::IsExpired() const {
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+
+ return m_isExpired;
+}
+
+void Watchdog::AddEpoch(wpi::StringRef epochName) {
+ auto currentTime = hal::fpga_clock::now();
+ m_epochs[epochName] = currentTime - m_startTime;
+ m_startTime = currentTime;
+}
+
+void Watchdog::PrintEpochs() {
+ 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";
+ }
+ }
+}
+
+void Watchdog::Reset() { Enable(); }
+
+void Watchdog::Enable() {
+ using std::chrono::duration_cast;
+ using std::chrono::microseconds;
+
+ m_startTime = hal::fpga_clock::now();
+ m_epochs.clear();
+
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+ if (!thr) return;
+
+ m_isExpired = false;
+
+ thr->m_watchdogs.remove(this);
+ m_expirationTime = m_startTime + duration_cast<microseconds>(m_timeout);
+ thr->m_watchdogs.emplace(this);
+ thr->m_cond.notify_all();
+}
+
+void Watchdog::Disable() {
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+ if (!thr) return;
+
+ thr->m_watchdogs.remove(this);
+ thr->m_cond.notify_all();
+}
+
+void Watchdog::SuppressTimeoutMessage(bool suppress) {
+ m_suppressTimeoutMessage = suppress;
+}
+
+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;
+}