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.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 358f32a2..4de46d9e 100644
--- a/WPILib/DriverStation.cs
+++ b/WPILib/DriverStation.cs
@@ -39,15 +39,24 @@ 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 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;
//Driver station thread keep alive
@@ -60,7 +69,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 +95,26 @@ 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_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);
+
+
//Initializes the HAL semaphores
m_dataSem = new object();
-
+
m_packetDataAvailableMutex = InitializeMutexNormal();
m_packetDataAvailableSem = InitializeMultiWait();
@@ -121,20 +145,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 +187,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 +198,42 @@ 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
{
- Monitor.Enter(m_lockObject);
- 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;
+ m_readWriteLock.EnterWriteLock();
+ lockEntered = true;
+
+ 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
{
- Monitor.Exit(m_lockObject);
+ if (lockEntered) m_readWriteLock.ExitWriteLock();
+ }
+ lock (m_newControlDataLock)
+ {
+ m_newControlData = true;
}
}
@@ -231,16 +274,25 @@ private void ReportJoystickUnpluggedError(string message, [CallerMemberName] str
/// 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}");
@@ -263,6 +315,10 @@ public double GetStickAxis(int stick, int axis)
return value / 127.0d;
}
}
+ finally
+ {
+ if (lockEntered) m_readWriteLock.ExitReadLock();
+ }
}
@@ -275,16 +331,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();
+ }
+
}
///
@@ -297,23 +361,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;
@@ -321,6 +390,11 @@ public int GetStickPOV(int stick, int pov)
return m_joystickPOVs[stick].povs[pov];
}
+ finally
+ {
+ if (lockEntered) m_readWriteLock.ExitReadLock();
+ }
+
}
///
@@ -332,16 +406,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();
+ }
+
}
///
@@ -353,15 +435,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();
+ }
+
}
///
@@ -374,28 +465,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();
+ }
}
///
@@ -407,16 +509,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();
+ }
}
///
@@ -428,22 +537,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();
+ }
+
}
///
@@ -455,21 +565,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();
}
}
@@ -482,24 +592,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();
}
}
@@ -600,17 +708,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);
- }
}
}