Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/hal/lib/Athena/ctre/PDP.cpp b/hal/lib/Athena/ctre/PDP.cpp
new file mode 100644
index 0000000..cae2264
--- /dev/null
+++ b/hal/lib/Athena/ctre/PDP.cpp
@@ -0,0 +1,231 @@
+#include "ctre/PDP.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h" //CAN Comm
+#include <string.h> // memset
+#include <unistd.h> // usleep
+
+#define STATUS_1 0x8041400
+#define STATUS_2 0x8041440
+#define STATUS_3 0x8041480
+#define STATUS_ENERGY 0x8041740
+
+#define CONTROL_1 0x08041C00 /* PDP_Control_ClearStats */
+
+#define EXPECTED_RESPONSE_TIMEOUT_MS (50)
+#define GET_STATUS1() CtreCanNode::recMsg<PdpStatus1_t> rx = GetRx<PdpStatus1_t>(STATUS_1,EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS2() CtreCanNode::recMsg<PdpStatus2_t> rx = GetRx<PdpStatus2_t>(STATUS_2,EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS3() CtreCanNode::recMsg<PdpStatus3_t> rx = GetRx<PdpStatus3_t>(STATUS_3,EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS_ENERGY() CtreCanNode::recMsg<PDP_Status_Energy_t> rx = GetRx<PDP_Status_Energy_t>(STATUS_ENERGY,EXPECTED_RESPONSE_TIMEOUT_MS)
+
+/* encoder/decoders */
+typedef struct _PdpStatus1_t{
+ unsigned chan1_h8:8;
+ unsigned chan2_h6:6;
+ unsigned chan1_l2:2;
+ unsigned chan3_h4:4;
+ unsigned chan2_l4:4;
+ unsigned chan4_h2:2;
+ unsigned chan3_l6:6;
+ unsigned chan4_l8:8;
+ unsigned chan5_h8:8;
+ unsigned chan6_h6:6;
+ unsigned chan5_l2:2;
+ unsigned reserved4:4;
+ unsigned chan6_l4:4;
+}PdpStatus1_t;
+typedef struct _PdpStatus2_t{
+ unsigned chan7_h8:8;
+ unsigned chan8_h6:6;
+ unsigned chan7_l2:2;
+ unsigned chan9_h4:4;
+ unsigned chan8_l4:4;
+ unsigned chan10_h2:2;
+ unsigned chan9_l6:6;
+ unsigned chan10_l8:8;
+ unsigned chan11_h8:8;
+ unsigned chan12_h6:6;
+ unsigned chan11_l2:2;
+ unsigned reserved4:4;
+ unsigned chan12_l4:4;
+}PdpStatus2_t;
+typedef struct _PdpStatus3_t{
+ unsigned chan13_h8:8;
+ unsigned chan14_h6:6;
+ unsigned chan13_l2:2;
+ unsigned chan15_h4:4;
+ unsigned chan14_l4:4;
+ unsigned chan16_h2:2;
+ unsigned chan15_l6:6;
+ unsigned chan16_l8:8;
+ unsigned internalResBattery_mOhms:8;
+ unsigned busVoltage:8;
+ unsigned temp:8;
+}PdpStatus3_t;
+typedef struct _PDP_Status_Energy_t {
+ unsigned TmeasMs_likelywillbe20ms_:8;
+ unsigned TotalCurrent_125mAperunit_h8:8;
+ unsigned Power_125mWperunit_h4:4;
+ unsigned TotalCurrent_125mAperunit_l4:4;
+ unsigned Power_125mWperunit_m8:8;
+ unsigned Energy_125mWPerUnitXTmeas_h4:4;
+ unsigned Power_125mWperunit_l4:4;
+ unsigned Energy_125mWPerUnitXTmeas_mh8:8;
+ unsigned Energy_125mWPerUnitXTmeas_ml8:8;
+ unsigned Energy_125mWPerUnitXTmeas_l8:8;
+} PDP_Status_Energy_t ;
+
+PDP::PDP(UINT8 deviceNumber): CtreCanNode(deviceNumber)
+{
+ RegisterRx(STATUS_1 | deviceNumber );
+ RegisterRx(STATUS_2 | deviceNumber );
+ RegisterRx(STATUS_3 | deviceNumber );
+}
+/* PDP D'tor
+ */
+PDP::~PDP()
+{
+}
+
+CTR_Code PDP::GetChannelCurrent(UINT8 idx, double ¤t)
+{
+ CTR_Code retval = CTR_InvalidParamValue;
+ uint32_t raw = 0;
+
+ if(idx <= 5){
+ GET_STATUS1();
+ retval = rx.err;
+ switch(idx){
+ case 0: raw = ((uint32_t)rx->chan1_h8 << 2) | rx->chan1_l2; break;
+ case 1: raw = ((uint32_t)rx->chan2_h6 << 4) | rx->chan2_l4; break;
+ case 2: raw = ((uint32_t)rx->chan3_h4 << 6) | rx->chan3_l6; break;
+ case 3: raw = ((uint32_t)rx->chan4_h2 << 8) | rx->chan4_l8; break;
+ case 4: raw = ((uint32_t)rx->chan5_h8 << 2) | rx->chan5_l2; break;
+ case 5: raw = ((uint32_t)rx->chan6_h6 << 4) | rx->chan6_l4; break;
+ default: retval = CTR_InvalidParamValue; break;
+ }
+ }else if(idx <= 11){
+ GET_STATUS2();
+ retval = rx.err;
+ switch(idx){
+ case 6: raw = ((uint32_t)rx->chan7_h8 << 2) | rx->chan7_l2; break;
+ case 7: raw = ((uint32_t)rx->chan8_h6 << 4) | rx->chan8_l4; break;
+ case 8: raw = ((uint32_t)rx->chan9_h4 << 6) | rx->chan9_l6; break;
+ case 9: raw = ((uint32_t)rx->chan10_h2 << 8) | rx->chan10_l8; break;
+ case 10: raw = ((uint32_t)rx->chan11_h8 << 2) | rx->chan11_l2; break;
+ case 11: raw = ((uint32_t)rx->chan12_h6 << 4) | rx->chan12_l4; break;
+ default: retval = CTR_InvalidParamValue; break;
+ }
+ }else if(idx <= 15){
+ GET_STATUS3();
+ retval = rx.err;
+ switch(idx){
+ case 12: raw = ((uint32_t)rx->chan13_h8 << 2) | rx->chan13_l2; break;
+ case 13: raw = ((uint32_t)rx->chan14_h6 << 4) | rx->chan14_l4; break;
+ case 14: raw = ((uint32_t)rx->chan15_h4 << 6) | rx->chan15_l6; break;
+ case 15: raw = ((uint32_t)rx->chan16_h2 << 8) | rx->chan16_l8; break;
+ default: retval = CTR_InvalidParamValue; break;
+ }
+ }
+ /* convert to amps */
+ current = (double)raw * 0.125; /* 7.3 fixed pt value in Amps */
+ /* signal caller with success */
+ return retval;
+}
+CTR_Code PDP::GetVoltage(double &voltage)
+{
+ GET_STATUS3();
+ uint32_t raw = rx->busVoltage;
+ voltage = (double)raw * 0.05 + 4.0; /* 50mV per unit plus 4V. */;
+ return rx.err;
+}
+CTR_Code PDP::GetTemperature(double &tempC)
+{
+ GET_STATUS3();
+ uint32_t raw = rx->temp;
+ tempC = (double)raw * 1.03250836957542 - 67.8564500484966;
+ return rx.err;
+}
+CTR_Code PDP::GetTotalCurrent(double ¤tAmps)
+{
+ GET_STATUS_ENERGY();
+ uint32_t raw;
+ raw = rx->TotalCurrent_125mAperunit_h8;
+ raw <<= 4;
+ raw |= rx->TotalCurrent_125mAperunit_l4;
+ currentAmps = 0.125 * raw;
+ return rx.err;
+}
+CTR_Code PDP::GetTotalPower(double &powerWatts)
+{
+ GET_STATUS_ENERGY();
+ uint32_t raw;
+ raw = rx->Power_125mWperunit_h4;
+ raw <<= 8;
+ raw |= rx->Power_125mWperunit_m8;
+ raw <<= 4;
+ raw |= rx->Power_125mWperunit_l4;
+ powerWatts = 0.125 * raw;
+ return rx.err;
+}
+CTR_Code PDP::GetTotalEnergy(double &energyJoules)
+{
+ GET_STATUS_ENERGY();
+ uint32_t raw;
+ raw = rx->Energy_125mWPerUnitXTmeas_h4;
+ raw <<= 8;
+ raw |= rx->Energy_125mWPerUnitXTmeas_mh8;
+ raw <<= 8;
+ raw |= rx->Energy_125mWPerUnitXTmeas_ml8;
+ raw <<= 8;
+ raw |= rx->Energy_125mWPerUnitXTmeas_l8;
+ energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
+ energyJoules *= 0.001; /* convert from mW to W */
+ energyJoules *= rx->TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
+ return rx.err;
+}
+/* Clear sticky faults.
+ * @Return - CTR_Code - Error code (if any)
+ */
+CTR_Code PDP::ClearStickyFaults()
+{
+ int32_t status = 0;
+ uint8_t pdpControl[] = { 0x80 }; /* only bit set is ClearStickyFaults */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_1 | GetDeviceNumber(), pdpControl, sizeof(pdpControl), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+
+/* Reset Energy Signals
+ * @Return - CTR_Code - Error code (if any)
+ */
+CTR_Code PDP::ResetEnergy()
+{
+ int32_t status = 0;
+ uint8_t pdpControl[] = { 0x40 }; /* only bit set is ResetEnergy */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_1 | GetDeviceNumber(), pdpControl, sizeof(pdpControl), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+//------------------ C interface --------------------------------------------//
+extern "C" {
+ void * c_PDP_Init(void)
+ {
+ return new PDP();
+ }
+ CTR_Code c_GetChannelCurrent(void * handle,UINT8 idx, double *status)
+ {
+ return ((PDP*)handle)-> GetChannelCurrent(idx,*status);
+ }
+ CTR_Code c_GetVoltage(void * handle,double *status)
+ {
+ return ((PDP*)handle)-> GetVoltage(*status);
+ }
+ CTR_Code c_GetTemperature(void * handle,double *status)
+ {
+ return ((PDP*)handle)-> GetTemperature(*status);
+ }
+ void c_SetDeviceNumber_PDP(void * handle,UINT8 deviceNumber)
+ {
+ }
+}