Optimize Solenoid Gets. Fixes artf4730.

Implement GetAllSolenoids in the HAL so that SolenoidBase doesn't have to
read each solenoid individually.

Change-Id: I85559565949f7a7119ead410187235636a63f0ed

hal/include/HAL/Solenoid.hpp

@@ -9,6 +9,7 @@ extern "C"
 	bool checkSolenoidModule(uint8_t module);
 
 	bool getSolenoid(void* solenoid_port_pointer, int32_t *status);
+	uint8_t getAllSolenoids(void* solenoid_port_pointer, int32_t *status);
 	void setSolenoid(void* solenoid_port_pointer, bool value, int32_t *status);
 
 	int getPCMSolenoidBlackList(void* solenoid_port_pointer, int32_t *status);

hal/include/ctre/PCM.h

@@ -31,10 +31,17 @@ public:
      *
      * @Return	-	CTR_Code	-	Error code (if any)
      * @Param 	-	idx		- 	ID of solenoid (0-7) to return if solenoid is on.
-     * @Param	-	status	-	OK if solenoid enabled, false otherwise
+     * @Param	-	status	-	true if solenoid enabled, false otherwise
      */
     CTR_Code 	GetSolenoid(UINT8 idx, bool &status);
 
+    /* Get state of all solenoids
+     *
+     * @Return	-	CTR_Code	-	Error code (if any)
+     * @Param	-	status	-	bitfield of solenoid states
+     */
+    CTR_Code 	GetAllSolenoids(UINT8 &status);
+
     /* Get pressure switch state
      * @Return	-	CTR_Code	-	Error code (if any)
      * @Param	-	status		-	True if pressure adequate, false if low
@@ -188,6 +195,7 @@ extern "C" {
 	CTR_Code c_SetClosedLoopControl(void * handle,INT8 param);
 	CTR_Code c_ClearStickyFaults(void * handle,INT8 param);
 	CTR_Code c_GetSolenoid(void * handle,UINT8 idx,INT8 * status);
+	CTR_Code c_GetAllSolenoids(void * handle,UINT8 * status);
 	CTR_Code c_GetPressure(void * handle,INT8 * status);
 	CTR_Code c_GetCompressor(void * handle,INT8 * status);
 	CTR_Code c_GetClosedLoopControl(void * handle,INT8 * status);

hal/lib/Athena/Solenoid.cpp

@@ -51,6 +51,15 @@ bool getSolenoid(void* solenoid_port_pointer, int32_t *status) {
 	return value;
 }
 
+uint8_t getAllSolenoids(void* solenoid_port_pointer, int32_t *status) {
+	solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer;
+	uint8_t value;
+
+	*status = port->module->GetAllSolenoids(value);
+
+	return value;
+}
+
 void setSolenoid(void* solenoid_port_pointer, bool value, int32_t *status) {
 	solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer;
 

hal/lib/Athena/ctre/PCM.cpp

@@ -4,7 +4,7 @@
 #include "FRC_NetworkCommunication/CANSessionMux.h"
 #include <string.h> // memset
 #include <unistd.h> // usleep
-/* This can be a constant, as long as nobody needs to updatie solenoids within
+/* This can be a constant, as long as nobody needs to update solenoids within
     1/50 of a second. */
 static const INT32 kCANPeriod = 20;
 
@@ -238,6 +238,17 @@ CTR_Code PCM::GetSolenoid(UINT8 idx, bool &status)
 	return rx.err;
 }
 
+/* Get solenoid state for all solenoids on the PCM
+ *
+ * @Return	-	Bitfield of solenoid states
+ */
+CTR_Code PCM::GetAllSolenoids(UINT8 &status)
+{
+	GET_PCM_STATUS();
+	status = rx->SolenoidBits;
+	return rx.err;
+}
+
 /* Get pressure switch state
  *
  * @Return	-	True/False	-	True if pressure adequate, false if low
@@ -467,6 +478,9 @@ extern "C" {
 		*status = bstatus;
 		return retval;
 	}
+	CTR_Code c_GetAllSolenoids(void * handle, UINT8 * status) {
+		return ((PCM*) handle)->GetAllSolenoids(*status);
+	}
 	CTR_Code c_GetPressure(void * handle, INT8 * status) {
 		bool bstatus;
 		CTR_Code retval = ((PCM*) handle)->GetPressure(bstatus);

wpilibc/Athena/src/SolenoidBase.cpp

@@ -48,9 +48,8 @@ void SolenoidBase::Set(uint8_t value, uint8_t mask, int module) {
 uint8_t SolenoidBase::GetAll(int module) const {
   uint8_t value = 0;
   int32_t status = 0;
-  for (int i = 0; i < m_maxPorts; i++) {
-    value |= getSolenoid(m_ports[module][i], &status) << i;
-  }
+  value = getAllSolenoids(m_ports[module][0], &status);
+  wpi_setErrorWithContext(status, getHALErrorMessage(status));
   return value;
 }
 /**

wpilibj/src/athena/cpp/lib/SolenoidJNI.cpp

@@ -99,6 +99,20 @@ JNIEXPORT jboolean JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getSolenoi
 	return val;
 }
 
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_SolenoidJNI
+ * Method:    getAllSolenoids
+ * Signature: (J)Z
+ */
+JNIEXPORT jbyte JNICALL Java_edu_wpi_first_wpilibj_hal_SolenoidJNI_getAllSolenoids
+  (JNIEnv *env, jclass, jlong solenoid_port)
+{
+	int32_t status = 0;
+	jbyte val = getAllSolenoids((void*)solenoid_port, &status);
+	CheckStatus(env, status);
+	return val;
+}
+
 /*
  * Class:     edu_wpi_first_wpilibj_hal_SolenoidJNI
  * Method:    getPCMSolenoidBlackList

wpilibj/src/athena/java/edu/wpi/first/wpilibj/SolenoidBase.java

@@ -65,11 +65,7 @@ public abstract class SolenoidBase extends SensorBase {
    * @return The current value of all 8 solenoids on this module.
    */
   public byte getAll() {
-    byte value = 0;
-    for (int i = 0; i < SensorBase.kSolenoidChannels; i++) {
-      value |= (SolenoidJNI.getSolenoid(m_ports[i]) ? 1 : 0) << i;
-    }
-    return value;
+    return SolenoidJNI.getAllSolenoids(m_ports[0]);
   }
 
   /**

wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/SolenoidJNI.java

@@ -11,6 +11,8 @@ public class SolenoidJNI extends JNIWrapper {
 
   public static native boolean getSolenoid(long port);
 
+  public static native byte getAllSolenoids(long port);
+
   public static native int getPCMSolenoidBlackList(long pcm_pointer);
 
   public static native boolean getPCMSolenoidVoltageStickyFault(long pcm_pointer);