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.

Author: ThadHouse

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);

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)
         /// </summary>
         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
             {