From d36d72bd4fe9056c963feb677800458e417b7daf Mon Sep 17 00:00:00 2001 From: Thad House Date: Thu, 23 Nov 2017 00:36:57 -0800 Subject: [PATCH] Fixes MotorSafetyHelper locking and race conditions (#762) Closes #760 --- wpilibc/src/main/native/cpp/DriverStation.cpp | 8 ++- .../src/main/native/cpp/MotorSafetyHelper.cpp | 16 +++-- .../edu/wpi/first/wpilibj/DriverStation.java | 4 ++ .../wpi/first/wpilibj/MotorSafetyHelper.java | 58 ++++++++++++++----- 4 files changed, 63 insertions(+), 23 deletions(-) diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index c74f57d648..1b2c71d8ac 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -738,14 +738,16 @@ void DriverStation::ReportJoystickUnpluggedWarning(llvm::StringRef message) { void DriverStation::Run() { m_isRunning = true; - int period = 0; + int safetyCounter = 0; while (m_isRunning) { HAL_WaitForDSData(); GetData(); - if (++period >= 4) { + if (IsDisabled()) safetyCounter = 0; + + if (++safetyCounter >= 4) { MotorSafetyHelper::CheckMotors(); - period = 0; + safetyCounter = 0; } if (m_userInDisabled) HAL_ObserveUserProgramDisabled(); if (m_userInAutonomous) HAL_ObserveUserProgramAutonomous(); diff --git a/wpilibc/src/main/native/cpp/MotorSafetyHelper.cpp b/wpilibc/src/main/native/cpp/MotorSafetyHelper.cpp index fd237c2491..ae51798111 100644 --- a/wpilibc/src/main/native/cpp/MotorSafetyHelper.cpp +++ b/wpilibc/src/main/native/cpp/MotorSafetyHelper.cpp @@ -96,11 +96,19 @@ bool MotorSafetyHelper::IsAlive() const { * shut down until its value is updated again. */ void MotorSafetyHelper::Check() { - DriverStation& ds = DriverStation::GetInstance(); - if (!m_enabled || ds.IsDisabled() || ds.IsTest()) return; + bool enabled; + double stopTime; - std::lock_guard lock(m_thisMutex); - if (m_stopTime < Timer::GetFPGATimestamp()) { + { + std::lock_guard lock(m_thisMutex); + enabled = m_enabled; + stopTime = m_stopTime; + } + + DriverStation& ds = DriverStation::GetInstance(); + if (!enabled || ds.IsDisabled() || ds.IsTest()) return; + + if (stopTime < Timer::GetFPGATimestamp()) { llvm::SmallString<128> buf; llvm::raw_svector_ostream desc(buf); m_safeObject->GetDescription(desc); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index 785bbeaa38..b966c35f62 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -945,6 +945,10 @@ public class DriverStation implements RobotState.Interface { HAL.waitForDSData(); getData(); + if (isDisabled()) { + safetyCounter = 0; + } + if (++safetyCounter >= 4) { MotorSafetyHelper.checkMotors(); safetyCounter = 0; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java index 874693eab2..752d23021a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java @@ -7,6 +7,9 @@ package edu.wpi.first.wpilibj; +import java.util.LinkedHashSet; +import java.util.Set; + /** * The MotorSafetyHelper object is constructed for every object that wants to implement the Motor * Safety protocol. The helper object has the code to actually do the timing and call the motors @@ -18,9 +21,10 @@ public final class MotorSafetyHelper { private double m_expiration; private boolean m_enabled; private double m_stopTime; + private final Object m_thisMutex = new Object(); private final MotorSafety m_safeObject; - private final MotorSafetyHelper m_nextHelper; - private static MotorSafetyHelper headHelper = null; + private static final Set m_helperList = new LinkedHashSet<>(); + private static final Object m_listMutex = new Object(); /** * The constructor for a MotorSafetyHelper object. The helper object is constructed for every @@ -36,15 +40,19 @@ public final class MotorSafetyHelper { m_enabled = false; m_expiration = MotorSafety.DEFAULT_SAFETY_EXPIRATION; m_stopTime = Timer.getFPGATimestamp(); - m_nextHelper = headHelper; - headHelper = this; + + synchronized (m_listMutex) { + m_helperList.add(this); + } } /** * Feed the motor safety object. Resets the timer on this object that is used to do the timeouts. */ public void feed() { - m_stopTime = Timer.getFPGATimestamp() + m_expiration; + synchronized (m_thisMutex) { + m_stopTime = Timer.getFPGATimestamp() + m_expiration; + } } /** @@ -53,7 +61,9 @@ public final class MotorSafetyHelper { * @param expirationTime The timeout value in seconds. */ public void setExpiration(double expirationTime) { - m_expiration = expirationTime; + synchronized (m_thisMutex) { + m_expiration = expirationTime; + } } /** @@ -62,7 +72,9 @@ public final class MotorSafetyHelper { * @return the timeout value in seconds. */ public double getExpiration() { - return m_expiration; + synchronized (m_thisMutex) { + return m_expiration; + } } /** @@ -71,7 +83,9 @@ public final class MotorSafetyHelper { * @return a true value if the motor is still operating normally and hasn't timed out. */ public boolean isAlive() { - return !m_enabled || m_stopTime > Timer.getFPGATimestamp(); + synchronized (m_thisMutex) { + return !m_enabled || m_stopTime > Timer.getFPGATimestamp(); + } } /** @@ -80,10 +94,18 @@ public final class MotorSafetyHelper { * motor is shut down until its value is updated again. */ public void check() { - if (!m_enabled || RobotState.isDisabled() || RobotState.isTest()) { + boolean enabled; + double stopTime; + + synchronized (m_thisMutex) { + enabled = m_enabled; + stopTime = m_stopTime; + } + + if (!enabled || RobotState.isDisabled() || RobotState.isTest()) { return; } - if (m_stopTime < Timer.getFPGATimestamp()) { + if (stopTime < Timer.getFPGATimestamp()) { DriverStation.reportError(m_safeObject.getDescription() + "... Output not updated often " + "enough.", false); @@ -98,7 +120,9 @@ public final class MotorSafetyHelper { * @param enabled True if motor safety is enforced for this object */ public void setSafetyEnabled(boolean enabled) { - m_enabled = enabled; + synchronized (m_thisMutex) { + m_enabled = enabled; + } } /** @@ -108,18 +132,20 @@ public final class MotorSafetyHelper { * @return True if motor safety is enforced for this device */ public boolean isSafetyEnabled() { - return m_enabled; + synchronized (m_thisMutex) { + return m_enabled; + } } /** * 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. */ - // TODO: these should be synchronized with the setting methods in case it's - // called from a different thread public static void checkMotors() { - for (MotorSafetyHelper msh = headHelper; msh != null; msh = msh.m_nextHelper) { - msh.check(); + synchronized (m_listMutex) { + for (MotorSafetyHelper elem : m_helperList) { + elem.check(); + } } } }