From dd86fe349c0b9e1c4d9b4263f9e0236647115c7b Mon Sep 17 00:00:00 2001 From: Thad House Date: Fri, 8 Jan 2016 11:18:53 -0800 Subject: [PATCH 1/2] Updates Driver Station Locking Locking the joysticks now uses a ReaderWriterLock, and we now get the descriptor in the GetData loop. This should speed up the DriverStation, and make it more reliable. --- WPILib.Tests/WPILib.Tests.csproj | 2 +- WPILib/DriverStation.cs | 305 +++++++++++++++++++------------ 2 files changed, 190 insertions(+), 117 deletions(-) diff --git a/WPILib.Tests/WPILib.Tests.csproj b/WPILib.Tests/WPILib.Tests.csproj index cb0aba05..70138863 100644 --- a/WPILib.Tests/WPILib.Tests.csproj +++ b/WPILib.Tests/WPILib.Tests.csproj @@ -28,7 +28,7 @@ DEBUG;TRACE prompt 4 - x86 + AnyCPU pdbonly diff --git a/WPILib/DriverStation.cs b/WPILib/DriverStation.cs index 6420db82..ed46a3de 100644 --- a/WPILib/DriverStation.cs +++ b/WPILib/DriverStation.cs @@ -42,12 +42,16 @@ public enum Alliance private readonly HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[JoystickPorts]; private readonly HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[JoystickPorts]; private readonly HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[JoystickPorts]; + private readonly HALJoystickDescriptor[] m_joystickDescriptors = new HALJoystickDescriptor[JoystickPorts]; //Pointers to the semaphores to the HAL and FPGA private readonly object m_dataSem; private readonly IntPtr m_packetDataAvailableMutex; private readonly IntPtr m_packetDataAvailableSem; + + //New Control Data Fast Semaphore Lock + private readonly object m_newControlDataLock = new object(); private bool m_newControlData = false; //Driver station thread keep alive @@ -60,7 +64,7 @@ public enum Alliance private bool m_userInTest; //Thread lock objects - private readonly object m_lockObject = new object(); + private readonly ReaderWriterLockSlim m_readWriteLock; //Reporting interval time limiters private const double JoystickUnpluggedMessageInterval = 1.0; @@ -86,11 +90,18 @@ protected DriverStation() m_joystickButtons[i].count = 0; m_joystickAxes[i].count = 0; m_joystickPOVs[i].count = 0; + m_joystickDescriptors[i] = new HALJoystickDescriptor(); + m_joystickDescriptors[i].isXbox = 0; + m_joystickDescriptors[i].type = 0xFF; + m_joystickDescriptors[i].name.byte0 = 0; } + m_readWriteLock = new ReaderWriterLockSlim(LockRecursionPolicy.SupportsRecursion); + + //Initializes the HAL semaphores m_dataSem = new object(); - + m_packetDataAvailableMutex = InitializeMutexNormal(); m_packetDataAvailableSem = InitializeMultiWait(); @@ -121,20 +132,17 @@ private void Task() { //Wait for new DS data, grab the newest data, and return the semaphore. TakeMultiWait(m_packetDataAvailableSem, m_packetDataAvailableMutex); - lock (m_lockObject) - { - GetData(); - } + GetData(); try { Monitor.Enter(m_dataSem); Monitor.PulseAll(m_dataSem); } - finally + finally { Monitor.Exit(m_dataSem); } - + //Every 4 loops (80ms) check all of the motors to make sure they have been updated if (++safetyCounter >= 4) @@ -166,7 +174,7 @@ public void WaitForData(int timeout = Timeout.Infinite) Monitor.Enter(m_dataSem); Monitor.Wait(m_dataSem, timeout); } - finally + finally { Monitor.Exit(m_dataSem); } @@ -177,20 +185,25 @@ public void WaitForData(int timeout = Timeout.Infinite) /// protected void GetData() { + bool lockEntered = false; try { - Monitor.Enter(m_lockObject); + m_readWriteLock.EnterWriteLock(); + lockEntered = true; for (byte stick = 0; stick < JoystickPorts; stick++) { HALGetJoystickAxes(stick, ref m_joystickAxes[stick]); HALGetJoystickPOVs(stick, ref m_joystickPOVs[stick]); HALGetJoystickButtons(stick, ref m_joystickButtons[stick]); - } - m_newControlData = true; + HALGetJoystickDescriptor(stick, ref m_joystickDescriptors[stick]); } } finally { - Monitor.Exit(m_lockObject); + if (lockEntered) m_readWriteLock.ExitWriteLock(); + } + lock (m_newControlDataLock) + { + m_newControlData = true; } } @@ -230,16 +243,25 @@ private void ReportJoystickUnpluggedError(string message) /// Thrown if the stick or axis are out of range. public double GetStickAxis(int stick, int axis) { - lock (m_joystickAxes) + if (stick >= JoystickPorts) { - if (stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick Index is out of range, should be 0-{JoystickPorts}"); - } + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick Index is out of range, should be 0-{JoystickPorts}"); + } + + + + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; if (axis > m_joystickAxes[stick].count) { + m_readWriteLock.ExitReadLock(); + lockEntered = false; + if (axis >= MaxJoystickAxes) throw new ArgumentOutOfRangeException(nameof(axis), $"Joystick axis is out of range, should be between 0 and {m_joystickAxes[stick].count}"); @@ -262,6 +284,10 @@ public double GetStickAxis(int stick, int axis) return value / 127.0d; } } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); + } } @@ -274,16 +300,24 @@ public double GetStickAxis(int stick, int axis) /// Thrown if the stick is out of range. public int GetStickAxisCount(int stick) { - lock (m_lockObject) + if (stick < 0 || stick >= JoystickPorts) { - if (stick < 0 || stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick index is out of range, should be 0-{JoystickPorts}"); - } + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick index is out of range, should be 0-{JoystickPorts}"); + } + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; return m_joystickAxes[stick].count; } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); + } + } /// @@ -296,23 +330,28 @@ public int GetStickAxisCount(int stick) /// Thrown if the stick or povs are out of range. public int GetStickPOV(int stick, int pov) { - lock (m_lockObject) + if (stick < 0 || stick >= JoystickPorts) { - if (stick < 0 || stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick Index is out of range, should be 0-{JoystickPorts}"); - } + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick Index is out of range, should be 0-{JoystickPorts}"); + } - if (pov < 0 || pov >= MaxJoystickPOVs) - { - throw new ArgumentOutOfRangeException(nameof(pov), - $"Joystick POV is out of range, should be between 0 and {MaxJoystickPOVs}"); - } + if (pov < 0 || pov >= MaxJoystickPOVs) + { + throw new ArgumentOutOfRangeException(nameof(pov), + $"Joystick POV is out of range, should be between 0 and {MaxJoystickPOVs}"); + } + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; if (pov >= m_joystickPOVs[stick].count) { + m_readWriteLock.ExitReadLock(); + lockEntered = false; ReportJoystickUnpluggedError("WARNING: Joystick POV " + pov + " on port " + stick + " not available, check if controller is plugged in\n"); return -1; @@ -320,6 +359,11 @@ public int GetStickPOV(int stick, int pov) return m_joystickPOVs[stick].povs[pov]; } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); + } + } /// @@ -331,16 +375,24 @@ public int GetStickPOV(int stick, int pov) /// Thrown if the stick is out of range. public int GetStickPOVCount(int stick) { - lock (m_lockObject) + if (stick < 0 || stick >= JoystickPorts) { - if (stick < 0 || stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick Index is out of range, should be 0-{JoystickPorts}"); - } + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick Index is out of range, should be 0-{JoystickPorts}"); + } + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; return m_joystickPOVs[stick].count; } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); + } + } /// @@ -352,15 +404,24 @@ public int GetStickPOVCount(int stick) /// Thrown if the stick is out of range. public int GetStickButtons(int stick) { - lock (m_lockObject) + + if (stick < 0 || stick >= JoystickPorts) { - if (stick < 0 || stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick Index is out of range, should be 0-{JoystickPorts}"); - } + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick Index is out of range, should be 0-{JoystickPorts}"); + } + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; return (int)m_joystickButtons[stick].buttons; } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); + } + } /// @@ -373,28 +434,39 @@ public int GetStickButtons(int stick) /// Thrown if the stick is out of range. public bool GetStickButton(int stick, byte button) { - lock (m_lockObject) + if (stick < 0 || stick >= JoystickPorts) { - if (stick < 0 || stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick Index is out of range, should be 0-{JoystickPorts}"); - } + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick Index is out of range, should be 0-{JoystickPorts}"); + } + if (button <= 0) + { + ReportJoystickUnpluggedError("ERROR: Button indexes begin at 1 in WPILib for C#\n"); + return false; + } + + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; if (button > m_joystickButtons[stick].count) { + m_readWriteLock.ExitReadLock(); + lockEntered = false; ReportJoystickUnpluggedError("WARNING: Joystick Button " + button + " on port " + stick + " not available, check if controller is plugged in\n"); return false; } - if (button <= 0) - { - ReportJoystickUnpluggedError("ERROR: Button indexes begin at 1 in WPILib for C#\n"); - return false; - } + return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0; } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); + } } /// @@ -406,16 +478,23 @@ public bool GetStickButton(int stick, byte button) /// Thrown if the stick is out of range. public int GetStickButtonCount(int stick) { - lock (m_lockObject) - { - if (stick < 0 || stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick Index is out of range, should be 0-{JoystickPorts}"); - } + if (stick < 0 || stick >= JoystickPorts) + { + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick Index is out of range, should be 0-{JoystickPorts}"); + } + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; return m_joystickButtons[stick].count; } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); + } } /// @@ -427,22 +506,23 @@ public int GetStickButtonCount(int stick) /// Thrown if the stick is out of range. public bool GetJoystickIsXbox(int stick) { - lock (m_lockObject) + if (stick < 0 || stick >= JoystickPorts) { - if (stick < 0 || stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick Index is out of range, should be 0-{JoystickPorts}"); - } - //TODO: Remove this when calling for descriptor on empty stick no longer crashes - if (1 > m_joystickButtons[stick].count && 1 > m_joystickAxes[stick].count) - { - ReportJoystickUnpluggedError("WARNING: Joystick on port " + stick + - " not available, check if controller is plugged in\n"); - return false; - } - return HALGetJoystickIsXbox((byte)stick) == 1; + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick Index is out of range, should be 0-{JoystickPorts}"); } + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; + return m_joystickDescriptors[stick].isXbox != 0; + } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); + } + } /// @@ -454,21 +534,21 @@ public bool GetJoystickIsXbox(int stick) /// Thrown if the stick is out of range. public int GetJoystickType(int stick) { - lock (m_lockObject) + if (stick < 0 || stick >= JoystickPorts) { - if (stick < 0 || stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick Index is out of range, should be 0-{JoystickPorts}"); - } - //TODO: Remove this when calling for descriptor on empty stick - if (1 > m_joystickButtons[stick].count && 1 > m_joystickAxes[stick].count) - { - ReportJoystickUnpluggedError("WARNING: Joystick on port " + stick + - " not available, check if controller is plugged in\n"); - return -1; - } - return HALGetJoystickType((byte)stick); + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick Index is out of range, should be 0-{JoystickPorts}"); + } + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; + return m_joystickDescriptors[stick].type; + } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); } } @@ -481,24 +561,22 @@ public int GetJoystickType(int stick) /// Thrown if the stick is out of range. public string GetJoystickName(int stick) { - lock (m_lockObject) + if (stick < 0 || stick >= JoystickPorts) { - if (stick < 0 || stick >= JoystickPorts) - { - throw new ArgumentOutOfRangeException(nameof(stick), - $"Joystick Index is out of range, should be 0-{JoystickPorts}"); - } + throw new ArgumentOutOfRangeException(nameof(stick), + $"Joystick Index is out of range, should be 0-{JoystickPorts}"); + } - //TODO: Remove this when calling for descriptor on empty stick - if (1 > m_joystickButtons[stick].count && 1 > m_joystickAxes[stick].count) - { - ReportJoystickUnpluggedError("WARNING: Joystick on port " + stick + - " not available, check if controller is plugged in\n"); - return ""; - } - HALJoystickDescriptor desc = new HALJoystickDescriptor(); - HAL.Base.HAL.HALGetJoystickDescriptor((byte)stick, ref desc); - return desc.name.ToString(); + bool lockEntered = false; + try + { + m_readWriteLock.EnterReadLock(); + lockEntered = true; + return m_joystickDescriptors[stick].name.ToString(); + } + finally + { + if (lockEntered) m_readWriteLock.ExitReadLock(); } } @@ -599,17 +677,12 @@ public bool NewControlData { get { - try + lock (m_newControlDataLock) { - Monitor.Enter(m_lockObject); bool result = m_newControlData; m_newControlData = false; return result; } - finally - { - Monitor.Exit(m_lockObject); - } } } From 1379b5e719d5442170305df1cee83d29951d755c Mon Sep 17 00:00:00 2001 From: Thad House Date: Mon, 11 Jan 2016 16:10:23 -0800 Subject: [PATCH 2/2] Implements Driver Station Caching to speed up DS reads The GetData HAL methods take 5 ms combined to grab all the data. So there is easily a period where the data could be locked and contended for. This now grabs the data into a cache, leave all joystick data readable. It then grabs the write lock, does a quick reference swap, and unlocks. This way, the writing only takes a few nanoseconds, rather then 5 milliseconds. Should fix DS Joystick Tests The tests need to wait longer. Extends wait times Might need to actually figure out locking driver station. --- WPILib.Tests/TestDriverStation.cs | 8 ++--- WPILib/DriverStation.cs | 52 ++++++++++++++++++++++++------- 2 files changed, 45 insertions(+), 15 deletions(-) diff --git a/WPILib.Tests/TestDriverStation.cs b/WPILib.Tests/TestDriverStation.cs index 6728d183..64b26f8c 100644 --- a/WPILib.Tests/TestDriverStation.cs +++ b/WPILib.Tests/TestDriverStation.cs @@ -256,7 +256,7 @@ public void TestJoystickButtons([Range(0,32)] int numButtons) } }; DriverStationHelper.UpdateData(); - Thread.Sleep(10); + Thread.Sleep(150); int buttonCount = DriverStation.Instance.GetStickButtonCount(0); Assert.That(buttonCount, Is.EqualTo(numButtons)); for (int i = 0; i < buttonCount; i++) @@ -271,7 +271,7 @@ public void TestJoystickButtons([Range(0,32)] int numButtons) buttons[i] = true; } DriverStationHelper.UpdateData(); - Thread.Sleep(10); + Thread.Sleep(150); for (int i = 0; i < buttonCount; i++) { bool button = DriverStation.Instance.GetStickButton(0, (byte)(i + 1)); @@ -301,7 +301,7 @@ public void TestJoystickAxes([Range(0, 6)] int numAxes) } }; DriverStationHelper.UpdateData(); - Thread.Sleep(10); + Thread.Sleep(150); int axesCount = DriverStation.Instance.GetStickAxisCount(0); Assert.That(axesCount, Is.EqualTo(numAxes)); for (int i = 0; i < axesCount; i++) @@ -316,7 +316,7 @@ public void TestJoystickAxes([Range(0, 6)] int numAxes) axes[i] = -.598; } DriverStationHelper.UpdateData(); - Thread.Sleep(10); + Thread.Sleep(150); for (int i = 0; i < axesCount; i++) { double axis = DriverStation.Instance.GetStickAxis(0, i); diff --git a/WPILib/DriverStation.cs b/WPILib/DriverStation.cs index ed46a3de..5ac0a779 100644 --- a/WPILib/DriverStation.cs +++ b/WPILib/DriverStation.cs @@ -39,17 +39,22 @@ public enum Alliance }; //Private Fields - private readonly HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[JoystickPorts]; - private readonly HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[JoystickPorts]; - private readonly HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[JoystickPorts]; - private readonly HALJoystickDescriptor[] m_joystickDescriptors = new HALJoystickDescriptor[JoystickPorts]; + private HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[JoystickPorts]; + private HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[JoystickPorts]; + private HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[JoystickPorts]; + private HALJoystickDescriptor[] m_joystickDescriptors = new HALJoystickDescriptor[JoystickPorts]; + + private HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[JoystickPorts]; + private HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[JoystickPorts]; + private HALJoystickButtons[] m_joystickButtonsCache = new HALJoystickButtons[JoystickPorts]; + private HALJoystickDescriptor[] m_joystickDescriptorsCache = new HALJoystickDescriptor[JoystickPorts]; //Pointers to the semaphores to the HAL and FPGA private readonly object m_dataSem; private readonly IntPtr m_packetDataAvailableMutex; private readonly IntPtr m_packetDataAvailableSem; - + //New Control Data Fast Semaphore Lock private readonly object m_newControlDataLock = new object(); private bool m_newControlData = false; @@ -94,6 +99,14 @@ protected DriverStation() m_joystickDescriptors[i].isXbox = 0; m_joystickDescriptors[i].type = 0xFF; m_joystickDescriptors[i].name.byte0 = 0; + + m_joystickButtonsCache[i].count = 0; + m_joystickAxesCache[i].count = 0; + m_joystickPOVsCache[i].count = 0; + m_joystickDescriptorsCache[i] = new HALJoystickDescriptor(); + m_joystickDescriptorsCache[i].isXbox = 0; + m_joystickDescriptorsCache[i].type = 0xFF; + m_joystickDescriptorsCache[i].name.byte0 = 0; } m_readWriteLock = new ReaderWriterLockSlim(LockRecursionPolicy.SupportsRecursion); @@ -185,17 +198,34 @@ public void WaitForData(int timeout = Timeout.Infinite) /// protected void GetData() { + for (byte stick = 0; stick < JoystickPorts; stick++) + { + HALGetJoystickAxes(stick, ref m_joystickAxesCache[stick]); + HALGetJoystickPOVs(stick, ref m_joystickPOVsCache[stick]); + HALGetJoystickButtons(stick, ref m_joystickButtonsCache[stick]); + HALGetJoystickDescriptor(stick, ref m_joystickDescriptorsCache[stick]); + } bool lockEntered = false; try { m_readWriteLock.EnterWriteLock(); lockEntered = true; - for (byte stick = 0; stick < JoystickPorts; stick++) - { - HALGetJoystickAxes(stick, ref m_joystickAxes[stick]); - HALGetJoystickPOVs(stick, ref m_joystickPOVs[stick]); - HALGetJoystickButtons(stick, ref m_joystickButtons[stick]); - HALGetJoystickDescriptor(stick, ref m_joystickDescriptors[stick]); } + + HALJoystickAxes[] currentAxes = m_joystickAxes; + m_joystickAxes = m_joystickAxesCache; + m_joystickAxesCache = currentAxes; + + HALJoystickButtons[] currentButtons = m_joystickButtons; + m_joystickButtons = m_joystickButtonsCache; + m_joystickButtonsCache = currentButtons; + + HALJoystickPOVs[] currentPOVs = m_joystickPOVs; + m_joystickPOVs = m_joystickPOVsCache; + m_joystickPOVsCache = currentPOVs; + + HALJoystickDescriptor[] currentDescriptor = m_joystickDescriptors; + m_joystickDescriptors = m_joystickDescriptorsCache; + m_joystickDescriptorsCache = currentDescriptor; } finally {