MotorSafety: Use Watchdog instead of DS class polling (#1442)

wpilibc/src/main/native/cpp/DriverStation.cpp

@@ -19,7 +19,6 @@
 #include <wpi/StringRef.h>
 
 #include "frc/AnalogInput.h"
-#include "frc/MotorSafety.h"
 #include "frc/Timer.h"
 #include "frc/Utility.h"
 #include "frc/WPIErrors.h"
@@ -549,17 +548,10 @@ void DriverStation::ReportJoystickUnpluggedWarning(const wpi::Twine& message) {
 
 void DriverStation::Run() {
   m_isRunning = true;
-  int safetyCounter = 0;
   while (m_isRunning) {
     HAL_WaitForDSData();
     GetData();
 
-    if (IsDisabled()) safetyCounter = 0;
-
-    if (++safetyCounter >= 4) {
-      MotorSafety::CheckMotors();
-      safetyCounter = 0;
-    }
     if (m_userInDisabled) HAL_ObserveUserProgramDisabled();
     if (m_userInAutonomous) HAL_ObserveUserProgramAutonomous();
     if (m_userInTeleop) HAL_ObserveUserProgramTeleop();

wpilibc/src/main/native/cpp/MotorSafety.cpp

@@ -10,66 +10,53 @@
 #include <algorithm>
 #include <utility>
 
-#include <wpi/SmallPtrSet.h>
 #include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
 
-#include "frc/DriverStation.h"
 #include "frc/WPIErrors.h"
 
 using namespace frc;
 
-static wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
-static wpi::mutex listMutex;
-
-MotorSafety::MotorSafety() {
-  std::lock_guard<wpi::mutex> lock(listMutex);
-  instanceList.insert(this);
-}
-
-MotorSafety::~MotorSafety() {
-  std::lock_guard<wpi::mutex> lock(listMutex);
-  instanceList.erase(this);
-}
-
 MotorSafety::MotorSafety(MotorSafety&& rhs)
-    : ErrorBase(std::move(rhs)),
-      m_expiration(std::move(rhs.m_expiration)),
-      m_enabled(std::move(rhs.m_enabled)),
-      m_stopTime(std::move(rhs.m_stopTime)) {}
+    : ErrorBase(std::move(rhs)), m_enabled(std::move(rhs.m_enabled)) {
+  m_watchdog = Watchdog(rhs.m_watchdog.GetTimeout(), [this] { TimeoutFunc(); });
+}
 
 MotorSafety& MotorSafety::operator=(MotorSafety&& rhs) {
   ErrorBase::operator=(std::move(rhs));
 
-  m_expiration = std::move(rhs.m_expiration);
+  m_watchdog = Watchdog(rhs.m_watchdog.GetTimeout(), [this] { TimeoutFunc(); });
   m_enabled = std::move(rhs.m_enabled);
-  m_stopTime = std::move(rhs.m_stopTime);
 
   return *this;
 }
 
 void MotorSafety::Feed() {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
+  m_watchdog.Reset();
 }
 
 void MotorSafety::SetExpiration(double expirationTime) {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  m_expiration = expirationTime;
+  m_watchdog.SetTimeout(expirationTime);
 }
 
 double MotorSafety::GetExpiration() const {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  return m_expiration;
+  return m_watchdog.GetTimeout();
 }
 
 bool MotorSafety::IsAlive() const {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
+  return !m_enabled || !m_watchdog.IsExpired();
 }
 
 void MotorSafety::SetSafetyEnabled(bool enabled) {
   std::lock_guard<wpi::mutex> lock(m_thisMutex);
+  if (enabled) {
+    m_watchdog.Enable();
+  } else {
+    m_watchdog.Disable();
+  }
   m_enabled = enabled;
 }
 
@@ -78,35 +65,16 @@ bool MotorSafety::IsSafetyEnabled() const {
   return m_enabled;
 }
 
-void MotorSafety::Check() {
-  bool enabled;
-  double stopTime;
-
-  {
-    std::lock_guard<wpi::mutex> lock(m_thisMutex);
-    enabled = m_enabled;
-    stopTime = m_stopTime;
-  }
-
-  DriverStation& ds = DriverStation::GetInstance();
-  if (!enabled || ds.IsDisabled() || ds.IsTest()) {
+void MotorSafety::TimeoutFunc() {
+  auto& ds = DriverStation::GetInstance();
+  if (ds.IsDisabled() || ds.IsTest()) {
     return;
   }
 
-  std::lock_guard<wpi::mutex> lock(m_thisMutex);
-  if (stopTime < Timer::GetFPGATimestamp()) {
-    wpi::SmallString<128> buf;
-    wpi::raw_svector_ostream desc(buf);
-    GetDescription(desc);
-    desc << "... Output not updated often enough.";
-    wpi_setWPIErrorWithContext(Timeout, desc.str());
-    StopMotor();
-  }
-}
-
-void MotorSafety::CheckMotors() {
-  std::lock_guard<wpi::mutex> lock(listMutex);
-  for (auto elem : instanceList) {
-    elem->Check();
-  }
+  wpi::SmallString<128> buf;
+  wpi::raw_svector_ostream desc(buf);
+  GetDescription(desc);
+  desc << "... Output not updated often enough.";
+  wpi_setWPIErrorWithContext(Timeout, desc.str());
+  StopMotor();
 }

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

@@ -10,8 +10,10 @@
 #include <wpi/mutex.h>
 #include <wpi/raw_ostream.h>
 
+#include "frc/DriverStation.h"
 #include "frc/ErrorBase.h"
 #include "frc/Timer.h"
+#include "frc/Watchdog.h"
 
 namespace frc {
 
@@ -23,8 +25,8 @@ namespace frc {
  */
 class MotorSafety : public ErrorBase {
  public:
-  MotorSafety();
-  virtual ~MotorSafety();
+  MotorSafety() = default;
+  virtual ~MotorSafety() = default;
 
   MotorSafety(MotorSafety&& rhs);
   MotorSafety& operator=(MotorSafety&& rhs);
@@ -75,39 +77,20 @@ class MotorSafety : public ErrorBase {
    */
   bool IsSafetyEnabled() const;
 
-  /**
-   * Check if this motor has exceeded its timeout.
-   *
-   * This method is called periodically to determine if this motor has exceeded
-   * its timeout value. If it has, the stop method is called, and the motor is
-   * shut down until its value is updated again.
-   */
-  void Check();
-
-  /**
-   * Check the motors to see if any have timed out.
-   *
-   * This static method is called periodically to poll all the motors and stop
-   * any that have timed out.
-   */
-  static void CheckMotors();
-
   virtual void StopMotor() = 0;
   virtual void GetDescription(wpi::raw_ostream& desc) const = 0;
 
  private:
   static constexpr double kDefaultSafetyExpiration = 0.1;
 
-  // The expiration time for this object
-  double m_expiration = kDefaultSafetyExpiration;
+  Watchdog m_watchdog{kDefaultSafetyExpiration, [this] { TimeoutFunc(); }};
 
   // True if motor safety is enabled for this motor
   bool m_enabled = false;
 
-  // The FPGA clock value when the motor has expired
-  double m_stopTime = Timer::GetFPGATimestamp();
-
   mutable wpi::mutex m_thisMutex;
+
+  void TimeoutFunc();
 };
 
 }  // namespace frc