Squashed 'third_party/allwpilib_2017/' content from commit 35ac87d
Change-Id: I7bb6f5556c30d3f5a092e68de0be9c710c60c9f4
git-subtree-dir: third_party/allwpilib_2017
git-subtree-split: 35ac87d6ff8b7f061c4f18c9ea316e5dccd4888a
diff --git a/hal/lib/athena/ctre/CtreCanNode.cpp b/hal/lib/athena/ctre/CtreCanNode.cpp
new file mode 100644
index 0000000..2eb42ab
--- /dev/null
+++ b/hal/lib/athena/ctre/CtreCanNode.cpp
@@ -0,0 +1,163 @@
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+
+#include "ctre/CtreCanNode.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+#include <string.h> // memset
+
+static const UINT32 kFullMessageIDMask = 0x1fffffff;
+
+CtreCanNode::CtreCanNode(UINT8 deviceNumber)
+{
+ _deviceNumber = deviceNumber;
+}
+CtreCanNode::~CtreCanNode()
+{
+}
+void CtreCanNode::RegisterRx(uint32_t arbId)
+{
+ /* no need to do anything, we just use new API to poll last received message */
+}
+/**
+ * Schedule a CAN Frame for periodic transmit.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ * @param dlc Number of bytes to transmit (0 to 8).
+ * @param initialFrame Ptr to the frame data to schedule for transmitting. Passing null will result
+ * in defaulting to zero data value.
+ */
+void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs, uint32_t dlc, const uint8_t * initialFrame)
+{
+ int32_t status = 0;
+ if(dlc > 8)
+ dlc = 8;
+ txJob_t job = {0};
+ job.arbId = arbId;
+ job.periodMs = periodMs;
+ job.dlc = dlc;
+ if(initialFrame){
+ /* caller wants to specify original data */
+ memcpy(job.toSend, initialFrame, dlc);
+ }
+ _txJobs[arbId] = job;
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( job.arbId,
+ job.toSend,
+ job.dlc,
+ job.periodMs,
+ &status);
+}
+/**
+ * Schedule a CAN Frame for periodic transmit. Assume eight byte DLC and zero value for initial transmission.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ */
+void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs)
+{
+ RegisterTx(arbId,periodMs, 8, 0);
+}
+/**
+ * Remove a CAN frame Arbid to stop transmission.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ */
+void CtreCanNode::UnregisterTx(uint32_t arbId)
+{
+ /* set period to zero */
+ ChangeTxPeriod(arbId, 0);
+ /* look and remove */
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end()) {
+ _txJobs.erase(iter);
+ }
+}
+timespec diff(const timespec & start, const timespec & end)
+{
+ timespec temp;
+ if ((end.tv_nsec-start.tv_nsec)<0) {
+ temp.tv_sec = end.tv_sec-start.tv_sec-1;
+ temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
+ } else {
+ temp.tv_sec = end.tv_sec-start.tv_sec;
+ temp.tv_nsec = end.tv_nsec-start.tv_nsec;
+ }
+ return temp;
+}
+CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs)
+{
+ CTR_Code retval = CTR_OKAY;
+ int32_t status = 0;
+ uint8_t len = 0;
+ uint32_t timeStamp;
+ /* cap timeout at 999ms */
+ if(timeoutMs > 999)
+ timeoutMs = 999;
+ FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status);
+ if(status == 0){
+ /* fresh update */
+ rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */
+ clock_gettime(2,&r.time); /* fill in time */
+ memcpy(r.bytes, dataBytes, 8); /* fill in databytes */
+ }else{
+ /* did not get the message */
+ rxRxEvents_t::iterator i = _rxRxEvents.find(arbId);
+ if(i == _rxRxEvents.end()){
+ /* we've never gotten this mesage */
+ retval = CTR_RxTimeout;
+ /* fill caller's buffer with zeros */
+ memset(dataBytes,0,8);
+ }else{
+ /* we've gotten this message before but not recently */
+ memcpy(dataBytes,i->second.bytes,8);
+ /* get the time now */
+ struct timespec temp;
+ clock_gettime(2,&temp); /* get now */
+ /* how long has it been? */
+ temp = diff(i->second.time,temp); /* temp = now - last */
+ if(temp.tv_sec > 0){
+ retval = CTR_RxTimeout;
+ }else if(temp.tv_nsec > ((int32_t)timeoutMs*1000*1000)){
+ retval = CTR_RxTimeout;
+ }else {
+ /* our last update was recent enough */
+ }
+ }
+ }
+
+ return retval;
+}
+void CtreCanNode::FlushTx(uint32_t arbId)
+{
+ int32_t status = 0;
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end())
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId,
+ iter->second.toSend,
+ iter->second.dlc,
+ iter->second.periodMs,
+ &status);
+}
+/**
+ * Change the transmit period of an already scheduled CAN frame.
+ * This keeps the frame payload contents the same without caller having to perform
+ * a read-modify-write.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ * @return true if scheduled job was found and updated, false if there was no preceding job for the specified arbID.
+ */
+bool CtreCanNode::ChangeTxPeriod(uint32_t arbId, uint32_t periodMs)
+{
+ int32_t status = 0;
+ /* lookup the data bytes and period for this message */
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end()) {
+ /* modify th periodMs */
+ iter->second.periodMs = periodMs;
+ /* reinsert into scheduler with the same data bytes, only the period changed. */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId,
+ iter->second.toSend,
+ iter->second.dlc,
+ iter->second.periodMs,
+ &status);
+ return true;
+ }
+ return false;
+}
+
diff --git a/hal/lib/athena/ctre/PCM.cpp b/hal/lib/athena/ctre/PCM.cpp
new file mode 100644
index 0000000..2676743
--- /dev/null
+++ b/hal/lib/athena/ctre/PCM.cpp
@@ -0,0 +1,572 @@
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+
+#include "ctre/PCM.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+#include <string.h> // memset
+/* This can be a constant, as long as nobody needs to update solenoids within
+ 1/50 of a second. */
+static const INT32 kCANPeriod = 20;
+
+#define STATUS_1 0x9041400
+#define STATUS_SOL_FAULTS 0x9041440
+#define STATUS_DEBUG 0x9041480
+
+#define EXPECTED_RESPONSE_TIMEOUT_MS (50)
+#define GET_PCM_STATUS() CtreCanNode::recMsg<PcmStatus_t> rx = GetRx<PcmStatus_t> (STATUS_1|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_PCM_SOL_FAULTS() CtreCanNode::recMsg<PcmStatusFault_t> rx = GetRx<PcmStatusFault_t> (STATUS_SOL_FAULTS|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_PCM_DEBUG() CtreCanNode::recMsg<PcmDebug_t> rx = GetRx<PcmDebug_t> (STATUS_DEBUG|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+
+#define CONTROL_1 0x09041C00 /* PCM_Control */
+#define CONTROL_2 0x09041C40 /* PCM_SupplemControl */
+#define CONTROL_3 0x09041C80 /* PcmControlSetOneShotDur_t */
+
+/* encoder/decoders */
+typedef struct _PcmStatus_t{
+ /* Byte 0 */
+ unsigned SolenoidBits:8;
+ /* Byte 1 */
+ unsigned compressorOn:1;
+ unsigned stickyFaultFuseTripped:1;
+ unsigned stickyFaultCompCurrentTooHigh:1;
+ unsigned faultFuseTripped:1;
+ unsigned faultCompCurrentTooHigh:1;
+ unsigned faultHardwareFailure:1;
+ unsigned isCloseloopEnabled:1;
+ unsigned pressureSwitchEn:1;
+ /* Byte 2*/
+ unsigned battVoltage:8;
+ /* Byte 3 */
+ unsigned solenoidVoltageTop8:8;
+ /* Byte 4 */
+ unsigned compressorCurrentTop6:6;
+ unsigned solenoidVoltageBtm2:2;
+ /* Byte 5 */
+ unsigned StickyFault_dItooHigh :1;
+ unsigned Fault_dItooHigh :1;
+ unsigned moduleEnabled:1;
+ unsigned closedLoopOutput:1;
+ unsigned compressorCurrentBtm4:4;
+ /* Byte 6 */
+ unsigned tokenSeedTop8:8;
+ /* Byte 7 */
+ unsigned tokenSeedBtm8:8;
+}PcmStatus_t;
+
+typedef struct _PcmControl_t{
+ /* Byte 0 */
+ unsigned tokenTop8:8;
+ /* Byte 1 */
+ unsigned tokenBtm8:8;
+ /* Byte 2 */
+ unsigned solenoidBits:8;
+ /* Byte 3*/
+ unsigned reserved:4;
+ unsigned closeLoopOutput:1;
+ unsigned compressorOn:1;
+ unsigned closedLoopEnable:1;
+ unsigned clearStickyFaults:1;
+ /* Byte 4 */
+ unsigned OneShotField_h8:8;
+ /* Byte 5 */
+ unsigned OneShotField_l8:8;
+}PcmControl_t;
+
+typedef struct _PcmControlSetOneShotDur_t{
+ uint8_t sol10MsPerUnit[8];
+}PcmControlSetOneShotDur_t;
+
+typedef struct _PcmStatusFault_t{
+ /* Byte 0 */
+ unsigned SolenoidBlacklist:8;
+ /* Byte 1 */
+ unsigned reserved_bit0 :1;
+ unsigned reserved_bit1 :1;
+ unsigned reserved_bit2 :1;
+ unsigned reserved_bit3 :1;
+ unsigned StickyFault_CompNoCurrent :1;
+ unsigned Fault_CompNoCurrent :1;
+ unsigned StickyFault_SolenoidJumper :1;
+ unsigned Fault_SolenoidJumper :1;
+}PcmStatusFault_t;
+
+typedef struct _PcmDebug_t{
+ unsigned tokFailsTop8:8;
+ unsigned tokFailsBtm8:8;
+ unsigned lastFailedTokTop8:8;
+ unsigned lastFailedTokBtm8:8;
+ unsigned tokSuccessTop8:8;
+ unsigned tokSuccessBtm8:8;
+}PcmDebug_t;
+
+
+/* PCM Constructor - Clears all vars, establishes default settings, starts PCM background process
+ *
+ * @Return - void
+ *
+ * @Param - deviceNumber - Device ID of PCM to be controlled
+ */
+PCM::PCM(UINT8 deviceNumber): CtreCanNode(deviceNumber)
+{
+ RegisterRx(STATUS_1 | deviceNumber );
+ RegisterRx(STATUS_SOL_FAULTS | deviceNumber );
+ RegisterRx(STATUS_DEBUG | deviceNumber );
+ RegisterTx(CONTROL_1 | deviceNumber, kCANPeriod);
+ /* enable close loop */
+ SetClosedLoopControl(1);
+}
+/* PCM D'tor
+ */
+PCM::~PCM()
+{
+
+}
+
+/* Set PCM solenoid state
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ *
+ * @Param - idx - ID of solenoid (0-7)
+ * @Param - en - Enable / Disable identified solenoid
+ */
+CTR_Code PCM::SetSolenoid(unsigned char idx, bool en)
+{
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ if (en)
+ toFill->solenoidBits |= (1ul << (idx));
+ else
+ toFill->solenoidBits &= ~(1ul << (idx));
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+
+/* Set all PCM solenoid states
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoids
+ * @Param - state Bitfield to set all solenoids to
+ */
+CTR_Code PCM::SetAllSolenoids(UINT8 state) {
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ toFill->solenoidBits = state;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+
+/* Clears PCM sticky faults (indicators of past faults
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ *
+ * @Param - clr - Clear / do not clear faults
+ */
+CTR_Code PCM::ClearStickyFaults()
+{
+ int32_t status = 0;
+ uint8_t pcmSupplemControl[] = { 0, 0, 0, 0x80 }; /* only bit set is ClearStickyFaults */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_2 | GetDeviceNumber(), pcmSupplemControl, sizeof(pcmSupplemControl), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+
+/* Enables PCM Closed Loop Control of Compressor via pressure switch
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ *
+ * @Param - en - Enable / Disable Closed Loop Control
+ */
+CTR_Code PCM::SetClosedLoopControl(bool en)
+{
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ toFill->closedLoopEnable = en;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+/* Get solenoid Blacklist status
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid [0,7] to fire one shot pulse.
+ */
+CTR_Code PCM::FireOneShotSolenoid(UINT8 idx)
+{
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ /* grab field as it is now */
+ uint16_t oneShotField;
+ oneShotField = toFill->OneShotField_h8;
+ oneShotField <<= 8;
+ oneShotField |= toFill->OneShotField_l8;
+ /* get the caller's channel */
+ uint16_t shift = 2*idx;
+ uint16_t mask = 3; /* two bits wide */
+ uint8_t chBits = (oneShotField >> shift) & mask;
+ /* flip it */
+ chBits = (chBits)%3 + 1;
+ /* clear out 2bits for this channel*/
+ oneShotField &= ~(mask << shift);
+ /* put new field in */
+ oneShotField |= chBits << shift;
+ /* apply field as it is now */
+ toFill->OneShotField_h8 = oneShotField >> 8;
+ toFill->OneShotField_l8 = oneShotField;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+/* Configure the pulse width of a solenoid channel for one-shot pulse.
+ * Preprogrammed pulsewidth is 10ms resolute and can be between 20ms and 5.1s.
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid [0,7] to configure.
+ * @Param - durMs - pulse width in ms.
+ */
+CTR_Code PCM::SetOneShotDurationMs(UINT8 idx,uint32_t durMs)
+{
+ /* sanity check caller's param */
+ if(idx > 7)
+ return CTR_InvalidParamValue;
+ /* get latest tx frame */
+ CtreCanNode::txTask<PcmControlSetOneShotDur_t> toFill = GetTx<PcmControlSetOneShotDur_t>(CONTROL_3 | GetDeviceNumber());
+ if(toFill.IsEmpty()){
+ /* only send this out if caller wants to do one-shots */
+ RegisterTx(CONTROL_3 | _deviceNumber, kCANPeriod);
+ /* grab it */
+ toFill = GetTx<PcmControlSetOneShotDur_t>(CONTROL_3 | GetDeviceNumber());
+ }
+ toFill->sol10MsPerUnit[idx] = std::min(durMs/10,(uint32_t)0xFF);
+ /* apply the new data bytes */
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+
+/* Get solenoid state
+ *
+ * @Return - True/False - True if solenoid enabled, false otherwise
+ *
+ * @Param - idx - ID of solenoid (0-7) to return status of
+ */
+CTR_Code PCM::GetSolenoid(UINT8 idx, bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->SolenoidBits & (1ul<<(idx)) ) ? 1 : 0;
+ 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
+ */
+CTR_Code PCM::GetPressure(bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->pressureSwitchEn ) ? 1 : 0;
+ return rx.err;
+}
+
+/* Get compressor state
+ *
+ * @Return - True/False - True if enabled, false if otherwise
+ */
+CTR_Code PCM::GetCompressor(bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->compressorOn);
+ return rx.err;
+}
+
+/* Get closed loop control state
+ *
+ * @Return - True/False - True if closed loop enabled, false if otherwise
+ */
+CTR_Code PCM::GetClosedLoopControl(bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->isCloseloopEnabled);
+ return rx.err;
+}
+
+/* Get compressor current draw
+ *
+ * @Return - Amperes - Compressor current
+ */
+CTR_Code PCM::GetCompressorCurrent(float &status)
+{
+ GET_PCM_STATUS();
+ uint32_t temp =(rx->compressorCurrentTop6);
+ temp <<= 4;
+ temp |= rx->compressorCurrentBtm4;
+ status = temp * 0.03125; /* 5.5 fixed pt value in Amps */
+ return rx.err;
+}
+
+/* Get voltage across solenoid rail
+ *
+ * @Return - Volts - Voltage across solenoid rail
+ */
+CTR_Code PCM::GetSolenoidVoltage(float &status)
+{
+ GET_PCM_STATUS();
+ uint32_t raw =(rx->solenoidVoltageTop8);
+ raw <<= 2;
+ raw |= rx->solenoidVoltageBtm2;
+ status = (double) raw * 0.03125; /* 5.5 fixed pt value in Volts */
+ return rx.err;
+}
+
+/* Get hardware fault value
+ *
+ * @Return - True/False - True if hardware failure detected, false if otherwise
+ */
+CTR_Code PCM::GetHardwareFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->faultHardwareFailure;
+ return rx.err;
+}
+
+/* Get compressor fault value
+ *
+ * @Return - True/False - True if shorted compressor detected, false if otherwise
+ */
+CTR_Code PCM::GetCompressorCurrentTooHighFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->faultCompCurrentTooHigh;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorShortedStickyFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->StickyFault_dItooHigh;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorShortedFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->Fault_dItooHigh;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorNotConnectedStickyFault(bool &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = rx->StickyFault_CompNoCurrent;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorNotConnectedFault(bool &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = rx->Fault_CompNoCurrent;
+ return rx.err;
+}
+
+/* Get solenoid fault value
+ *
+ * @Return - True/False - True if shorted solenoid detected, false if otherwise
+ */
+CTR_Code PCM::GetSolenoidFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->faultFuseTripped;
+ return rx.err;
+}
+
+/* Get compressor sticky fault value
+ *
+ * @Return - True/False - True if solenoid had previously been shorted
+ * (and sticky fault was not cleared), false if otherwise
+ */
+CTR_Code PCM::GetCompressorCurrentTooHighStickyFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->stickyFaultCompCurrentTooHigh;
+ return rx.err;
+}
+
+/* Get solenoid sticky fault value
+ *
+ * @Return - True/False - True if compressor had previously been shorted
+ * (and sticky fault was not cleared), false if otherwise
+ */
+CTR_Code PCM::GetSolenoidStickyFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->stickyFaultFuseTripped;
+ return rx.err;
+}
+/* Get battery voltage
+ *
+ * @Return - Volts - Voltage across PCM power ports
+ */
+CTR_Code PCM::GetBatteryVoltage(float &status)
+{
+ GET_PCM_STATUS();
+ status = (float)rx->battVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
+ return rx.err;
+}
+/* Return status of module enable/disable
+ *
+ * @Return - bool - Returns TRUE if PCM is enabled, FALSE if disabled
+ */
+CTR_Code PCM::isModuleEnabled(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->moduleEnabled;
+ return rx.err;
+}
+/* Get number of total failed PCM Control Frame
+ *
+ * @Return - Failed Control Frames - Number of failed control frames (tokenization fails)
+ *
+ * @WARNING - Return only valid if [SeekDebugFrames] is enabled
+ * See function SeekDebugFrames
+ * See function EnableSeekDebugFrames
+ */
+CTR_Code PCM::GetNumberOfFailedControlFrames(UINT16 &status)
+{
+ GET_PCM_DEBUG();
+ status = rx->tokFailsTop8;
+ status <<= 8;
+ status |= rx->tokFailsBtm8;
+ return rx.err;
+}
+/* Get raw Solenoid Blacklist
+ *
+ * @Return - BINARY - Raw binary breakdown of Solenoid Blacklist
+ * BIT7 = Solenoid 1, BIT6 = Solenoid 2, etc.
+ *
+ * @WARNING - Return only valid if [SeekStatusFaultFrames] is enabled
+ * See function SeekStatusFaultFrames
+ * See function EnableSeekStatusFaultFrames
+ */
+CTR_Code PCM::GetSolenoidBlackList(UINT8 &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = rx->SolenoidBlacklist;
+ return rx.err;
+}
+/* Get solenoid Blacklist status
+ * - Blacklisted solenoids cannot be enabled until PCM is power cycled
+ *
+ * @Return - True/False - True if Solenoid is blacklisted, false if otherwise
+ *
+ * @Param - idx - ID of solenoid [0,7]
+ *
+ * @WARNING - Return only valid if [SeekStatusFaultFrames] is enabled
+ * See function SeekStatusFaultFrames
+ * See function EnableSeekStatusFaultFrames
+ */
+CTR_Code PCM::IsSolenoidBlacklisted(UINT8 idx, bool &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = (rx->SolenoidBlacklist & (1ul<<(idx)) )? 1 : 0;
+ return rx.err;
+}
+//------------------ C interface --------------------------------------------//
+extern "C" {
+ void * c_PCM_Init(void) {
+ return new PCM();
+ }
+ CTR_Code c_SetSolenoid(void * handle, unsigned char idx, INT8 param) {
+ return ((PCM*) handle)->SetSolenoid(idx, param);
+ }
+ CTR_Code c_SetAllSolenoids(void * handle, UINT8 state) {
+ return ((PCM*) handle)->SetAllSolenoids(state);
+ }
+ CTR_Code c_SetClosedLoopControl(void * handle, INT8 param) {
+ return ((PCM*) handle)->SetClosedLoopControl(param);
+ }
+ CTR_Code c_ClearStickyFaults(void * handle, INT8 param) {
+ return ((PCM*) handle)->ClearStickyFaults();
+ }
+ CTR_Code c_GetSolenoid(void * handle, UINT8 idx, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetSolenoid(idx, bstatus);
+ *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);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressor(void * handle, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetCompressor(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetClosedLoopControl(void * handle, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetClosedLoopControl(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressorCurrent(void * handle, float * status) {
+ CTR_Code retval = ((PCM*) handle)->GetCompressorCurrent(*status);
+ return retval;
+ }
+ CTR_Code c_GetSolenoidVoltage(void * handle, float*status) {
+ return ((PCM*) handle)->GetSolenoidVoltage(*status);
+ }
+ CTR_Code c_GetHardwareFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetHardwareFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressorFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetCompressorCurrentTooHighFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetSolenoidFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetSolenoidFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressorStickyFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetCompressorCurrentTooHighStickyFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetSolenoidStickyFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetSolenoidStickyFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetBatteryVoltage(void * handle, float*status) {
+ CTR_Code retval = ((PCM*) handle)->GetBatteryVoltage(*status);
+ return retval;
+ }
+ void c_SetDeviceNumber_PCM(void * handle, UINT8 deviceNumber) {
+ }
+ CTR_Code c_GetNumberOfFailedControlFrames(void * handle, UINT16*status) {
+ return ((PCM*) handle)->GetNumberOfFailedControlFrames(*status);
+ }
+ CTR_Code c_GetSolenoidBlackList(void * handle, UINT8 *status) {
+ return ((PCM*) handle)->GetSolenoidBlackList(*status);
+ }
+ CTR_Code c_IsSolenoidBlacklisted(void * handle, UINT8 idx, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->IsSolenoidBlacklisted(idx, bstatus);
+ *status = bstatus;
+ return retval;
+ }
+}
diff --git a/hal/lib/athena/ctre/PDP.cpp b/hal/lib/athena/ctre/PDP.cpp
new file mode 100644
index 0000000..127b1dc
--- /dev/null
+++ b/hal/lib/athena/ctre/PDP.cpp
@@ -0,0 +1,230 @@
+#include "ctre/PDP.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h" //CAN Comm
+#include <string.h> // memset
+
+#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|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS2() CtreCanNode::recMsg<PdpStatus2_t> rx = GetRx<PdpStatus2_t>(STATUS_2|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS3() CtreCanNode::recMsg<PdpStatus3_t> rx = GetRx<PdpStatus3_t>(STATUS_3|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS_ENERGY() CtreCanNode::recMsg<PDP_Status_Energy_t> rx = GetRx<PDP_Status_Energy_t>(STATUS_ENERGY|GetDeviceNumber(),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)
+ {
+ }
+}