diff --git a/wpilibc/athena/include/DriverStation.h b/wpilibc/athena/include/DriverStation.h index 00d87068a9..39e0be218d 100644 --- a/wpilibc/athena/include/DriverStation.h +++ b/wpilibc/athena/include/DriverStation.h @@ -97,6 +97,7 @@ class DriverStation : public SensorBase, public RobotStateInterface { void ReportJoystickUnpluggedError(std::string message); void ReportJoystickUnpluggedWarning(std::string message); void Run(); + void UpdateControlWord(bool force, HAL_ControlWord& controlWord) const; std::unique_ptr m_joystickAxes; std::unique_ptr m_joystickPOVs; @@ -120,5 +121,8 @@ class DriverStation : public SensorBase, public RobotStateInterface { bool m_userInAutonomous = false; bool m_userInTeleop = false; bool m_userInTest = false; + mutable HAL_ControlWord m_controlWordCache; + mutable std::chrono::steady_clock::time_point m_lastControlWordUpdate; + mutable priority_mutex m_controlWordMutex; double m_nextMessageTime = 0; }; diff --git a/wpilibc/athena/src/DriverStation.cpp b/wpilibc/athena/src/DriverStation.cpp index 73ec86b778..3aad6377e7 100644 --- a/wpilibc/athena/src/DriverStation.cpp +++ b/wpilibc/athena/src/DriverStation.cpp @@ -277,7 +277,7 @@ int DriverStation::GetJoystickAxisType(uint32_t stick, uint8_t axis) const { */ bool DriverStation::IsEnabled() const { HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); + UpdateControlWord(false, controlWord); return controlWord.enabled && controlWord.dsAttached; } @@ -288,7 +288,7 @@ bool DriverStation::IsEnabled() const { */ bool DriverStation::IsDisabled() const { HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); + UpdateControlWord(false, controlWord); return !(controlWord.enabled && controlWord.dsAttached); } @@ -299,7 +299,7 @@ bool DriverStation::IsDisabled() const { */ bool DriverStation::IsAutonomous() const { HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); + UpdateControlWord(false, controlWord); return controlWord.autonomous; } @@ -310,7 +310,7 @@ bool DriverStation::IsAutonomous() const { */ bool DriverStation::IsOperatorControl() const { HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); + UpdateControlWord(false, controlWord); return !(controlWord.autonomous || controlWord.test); } @@ -321,7 +321,7 @@ bool DriverStation::IsOperatorControl() const { */ bool DriverStation::IsTest() const { HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); + UpdateControlWord(false, controlWord); return controlWord.test; } @@ -332,7 +332,7 @@ bool DriverStation::IsTest() const { */ bool DriverStation::IsDSAttached() const { HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); + UpdateControlWord(false, controlWord); return controlWord.dsAttached; } @@ -357,7 +357,7 @@ bool DriverStation::IsNewControlData() const { */ bool DriverStation::IsFMSAttached() const { HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); + UpdateControlWord(false, controlWord); return controlWord.fmsAttached; } @@ -500,6 +500,9 @@ void DriverStation::GetData() { HAL_GetJoystickButtons(stick, &m_joystickButtonsCache[stick]); HAL_GetJoystickDescriptor(stick, &m_joystickDescriptorCache[stick]); } + // Force a control word update, to make sure the data is the newest. + HAL_ControlWord controlWord; + UpdateControlWord(true, controlWord); // Obtain a write lock on the data, swap the cached data into the // main data arrays std::lock_guard lock(m_joystickDataMutex); @@ -597,3 +600,25 @@ void DriverStation::Run() { if (m_userInTest) HAL_ObserveUserProgramTest(); } } + +/** + * Gets ControlWord data from the cache. If 50ms has passed, or the force + * parameter is set, the cached data is updated. Otherwise the data is just + * copied from the cache. + * + * @param force True to force an update to the cache, otherwise update if 50ms + * have passed. + * @param controlWord Structure to put the return control word data into. + */ +void DriverStation::UpdateControlWord(bool force, + HAL_ControlWord& controlWord) const { + auto now = std::chrono::steady_clock::now(); + std::lock_guard lock(m_controlWordMutex); + // Update every 50 ms or on force. + if ((now - m_lastControlWordUpdate > std::chrono::milliseconds(50)) || + force) { + HAL_GetControlWord(&m_controlWordCache); + m_lastControlWordUpdate = now; + } + controlWord = m_controlWordCache; +} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java index dabf577d52..d18c6a0843 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java @@ -94,6 +94,10 @@ public class DriverStation implements RobotState.Interface { private final Object m_joystickMutex; private volatile boolean m_threadKeepAlive = true; + private final Object m_controlWordMutex; + private ControlWord m_controlWordCache; + private long m_lastControlWordUpdate; + private boolean m_userInDisabled = false; private boolean m_userInAutonomous = false; private boolean m_userInTeleop = false; @@ -130,6 +134,10 @@ public class DriverStation implements RobotState.Interface { m_joystickPOVsCache[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs); } + m_controlWordMutex = new Object(); + m_controlWordCache = new ControlWord(); + m_lastControlWordUpdate = 0; + m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation"); m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2); @@ -434,8 +442,10 @@ public class DriverStation implements RobotState.Interface { * @return True if the robot is enabled, false otherwise. */ public boolean isEnabled() { - ControlWord controlWord = HAL.getControlWord(); - return controlWord.getEnabled() && controlWord.getDSAttached(); + synchronized (m_controlWordMutex) { + updateControlWord(false); + return m_controlWordCache.getEnabled() && m_controlWordCache.getDSAttached(); + } } /** @@ -454,8 +464,10 @@ public class DriverStation implements RobotState.Interface { * @return True if autonomous mode should be enabled, false otherwise. */ public boolean isAutonomous() { - ControlWord controlWord = HAL.getControlWord(); - return controlWord.getAutonomous(); + synchronized (m_controlWordMutex) { + updateControlWord(false); + return m_controlWordCache.getAutonomous(); + } } /** @@ -475,8 +487,10 @@ public class DriverStation implements RobotState.Interface { * @return True if test mode should be enabled, false otherwise. */ public boolean isTest() { - ControlWord controlWord = HAL.getControlWord(); - return controlWord.getTest(); + synchronized (m_controlWordMutex) { + updateControlWord(false); + return m_controlWordCache.getTest(); + } } /** @@ -485,8 +499,10 @@ public class DriverStation implements RobotState.Interface { * @return True if Driver Station is attached, false otherwise. */ public boolean isDSAttached() { - ControlWord controlWord = HAL.getControlWord(); - return controlWord.getDSAttached(); + synchronized (m_controlWordMutex) { + updateControlWord(false); + return m_controlWordCache.getDSAttached(); + } } /** @@ -510,8 +526,10 @@ public class DriverStation implements RobotState.Interface { * @return True if the robot is competing on a field being controlled by a Field Management System */ public boolean isFMSAttached() { - ControlWord controlWord = HAL.getControlWord(); - return controlWord.getFMSAttached(); + synchronized (m_controlWordMutex) { + updateControlWord(false); + return m_controlWordCache.getFMSAttached(); + } } /** @@ -694,6 +712,9 @@ public class DriverStation implements RobotState.Interface { m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer); m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0); } + // Force a control word update, to make sure the data is the newest. + updateControlWord(true); + // lock joystick mutex to swap cache data synchronized (m_joystickMutex) { // move cache to actual data @@ -769,4 +790,20 @@ public class DriverStation implements RobotState.Interface { } } } + + /** + * Updates the data in the control word cache. Updates if the force parameter is set, or if + * 50ms have passed since the last update. + * + * @param force True to force an update to the cache, otherwise update if 50ms have passed. + */ + private void updateControlWord(boolean force) { + long now = System.currentTimeMillis(); + synchronized (m_controlWordMutex) { + if ((now - m_lastControlWordUpdate > 50) || force) { + HAL.getControlWord(m_controlWordCache); + now = m_lastControlWordUpdate; + } + } + } } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/ControlWord.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/ControlWord.java index 92452e25dc..3eef577878 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/ControlWord.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/ControlWord.java @@ -11,15 +11,15 @@ package edu.wpi.first.wpilibj.hal; * A wrapper for the HALControlWord bitfield. */ public class ControlWord { - private final boolean m_enabled; - private final boolean m_autonomous; - private final boolean m_test; - private final boolean m_emergencyStop; - private final boolean m_fmsAttached; - private final boolean m_dsAttached; + private boolean m_enabled; + private boolean m_autonomous; + private boolean m_test; + private boolean m_emergencyStop; + private boolean m_fmsAttached; + private boolean m_dsAttached; - protected ControlWord(boolean enabled, boolean autonomous, boolean test, boolean emergencyStop, - boolean fmsAttached, boolean dsAttached) { + void update(boolean enabled, boolean autonomous, boolean test, boolean emergencyStop, + boolean fmsAttached, boolean dsAttached) { m_enabled = enabled; m_autonomous = autonomous; m_test = test; diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/HAL.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/HAL.java index 0c944b227d..c01a5f3c68 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/HAL.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/HAL.java @@ -57,9 +57,9 @@ public class HAL extends JNIWrapper { private static native int nativeGetControlWord(); @SuppressWarnings("JavadocMethod") - public static ControlWord getControlWord() { + public static void getControlWord(ControlWord controlWord) { int word = nativeGetControlWord(); - return new ControlWord((word & 1) != 0, ((word >> 1) & 1) != 0, ((word >> 2) & 1) != 0, + controlWord.update((word & 1) != 0, ((word >> 1) & 1) != 0, ((word >> 2) & 1) != 0, ((word >> 3) & 1) != 0, ((word >> 4) & 1) != 0, ((word >> 5) & 1) != 0); }