mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-02 02:51:42 +00:00
PDP and PCM updated to rely on CtreCanNode parent class, which uses new CAN API.
CtreCanNode registers the periodic tx messages and provides an rx function to child classes for easy getters and setters. Some template magic to make the PDP and PCM getters/setters easy to stamp out. Change-Id: Ibdd0745af070756a282df5074504491fadfde336
This commit is contained in:
Omar Zrien
@@ -3,145 +3,130 @@
|
||||
#include <string.h> // memset
|
||||
#include <unistd.h> // usleep
|
||||
|
||||
#define kFullMessageIDMask 0x1fffffff
|
||||
#define STATUS_1 0x8041400
|
||||
#define STATUS_2 0x8041440
|
||||
#define STATUS_3 0x8041480
|
||||
|
||||
PDP::PDP(UINT8 deviceNumber)
|
||||
#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)
|
||||
|
||||
/* 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;
|
||||
|
||||
PDP::PDP(UINT8 deviceNumber): CtreCanNode(deviceNumber)
|
||||
{
|
||||
memset(&_status1, 0, sizeof(_status1));
|
||||
memset(&_status2, 0, sizeof(_status2));
|
||||
memset(&_status3, 0, sizeof(_status3));
|
||||
memset(&PDP_Settings, 0, sizeof PDP_Settings);
|
||||
/* setup arbids */
|
||||
SetDeviceNumber(deviceNumber);
|
||||
/* clear error info */
|
||||
_timeSinceLastRx = 0;
|
||||
_numFailedRxs = 0;
|
||||
/* start thread */
|
||||
_threadIsRunning = 1;
|
||||
_threadErr = pthread_create( &_thread, NULL, ThreadFunc, (void*) this);
|
||||
RegisterRx(STATUS_1 | deviceNumber );
|
||||
RegisterRx(STATUS_2 | deviceNumber );
|
||||
RegisterRx(STATUS_3 | deviceNumber );
|
||||
}
|
||||
/* PDP D'tor
|
||||
*/
|
||||
PDP::~PDP() {
|
||||
/* wait for thread to finish */
|
||||
_threadIsRunning = 0;
|
||||
pthread_join( _thread, NULL);
|
||||
_thread = 0;
|
||||
PDP::~PDP()
|
||||
{
|
||||
}
|
||||
CTR_Code PDP::GetChannelCurrent(UINT8 idx, double ¤t){
|
||||
/* atomically copy out our data */
|
||||
PdpStatus1_t status1 = _status1;
|
||||
PdpStatus2_t status2 = _status2;
|
||||
PdpStatus3_t status3 = _status3;
|
||||
|
||||
CTR_Code PDP::GetChannelCurrent(UINT8 idx, double ¤t)
|
||||
{
|
||||
CTR_Code retval = CTR_InvalidParamValue;
|
||||
uint32_t raw = 0;
|
||||
if(idx <= 6)
|
||||
{
|
||||
switch(idx)
|
||||
{
|
||||
case 0: raw = 0; break;
|
||||
case 1: raw = ((uint32_t)status1.chan1_h8 << 2) | status1.chan1_l2; break;
|
||||
case 2: raw = ((uint32_t)status1.chan2_h6 << 4) | status1.chan2_l4; break;
|
||||
case 3: raw = ((uint32_t)status1.chan3_h4 << 6) | status1.chan3_l6; break;
|
||||
case 4: raw = ((uint32_t)status1.chan4_h2 << 8) | status1.chan4_l8; break;
|
||||
case 5: raw = ((uint32_t)status1.chan5_h8 << 2) | status1.chan5_l2; break;
|
||||
case 6: raw = ((uint32_t)status1.chan6_h6 << 4) | status1.chan6_l4; break;
|
||||
|
||||
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 <= 12)
|
||||
{
|
||||
switch(idx)
|
||||
{
|
||||
case 7: raw = ((uint32_t)status2.chan7_h8 << 2) | status2.chan7_l2; break;
|
||||
case 8: raw = ((uint32_t)status2.chan8_h6 << 4) | status2.chan8_l4; break;
|
||||
case 9: raw = ((uint32_t)status2.chan9_h4 << 6) | status2.chan9_l6; break;
|
||||
case 10: raw = ((uint32_t)status2.chan10_h2 << 8) | status2.chan10_l8; break;
|
||||
case 11: raw = ((uint32_t)status2.chan11_h8 << 2) | status2.chan11_l2; break;
|
||||
case 12: raw = ((uint32_t)status2.chan12_h6 << 4) | status2.chan12_l4; 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 <= 16)
|
||||
{
|
||||
switch(idx)
|
||||
{
|
||||
case 13: raw = ((uint32_t)status3.chan13_h8 << 2) | status3.chan13_l2; break;
|
||||
case 14: raw = ((uint32_t)status3.chan14_h6 << 4) | status3.chan14_l4; break;
|
||||
case 15: raw = ((uint32_t)status3.chan15_h4 << 6) | status3.chan15_l6; break;
|
||||
case 16: raw = ((uint32_t)status3.chan16_h2 << 8) | status3.chan16_l8; 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 = 0.06724511900000001*raw + 1.527114967;
|
||||
/* signal caller with success */
|
||||
if(GetTimeSinceLastRx()>=50)
|
||||
return CTR_RxTimeout;
|
||||
return CTR_OKAY;
|
||||
return retval;
|
||||
}
|
||||
CTR_Code PDP::GetVoltage(double &voltage){
|
||||
uint32_t raw = _status3.busVoltage;
|
||||
CTR_Code PDP::GetVoltage(double &voltage)
|
||||
{
|
||||
GET_STATUS3();
|
||||
uint32_t raw = rx->busVoltage;
|
||||
voltage = 0.0554413328606877 * raw;
|
||||
if(GetTimeSinceLastRx()>=50)
|
||||
return CTR_RxTimeout;
|
||||
return CTR_OKAY;
|
||||
return rx.err;
|
||||
}
|
||||
|
||||
CTR_Code PDP::GetTemperature(double &tempC){
|
||||
uint32_t raw = _status3.temp;
|
||||
CTR_Code PDP::GetTemperature(double &tempC)
|
||||
{
|
||||
GET_STATUS3();
|
||||
uint32_t raw = rx->temp;
|
||||
tempC = ((double)raw-67.8564500484966)*1.03250836957542;
|
||||
if(GetTimeSinceLastRx()>=50)
|
||||
return CTR_RxTimeout;
|
||||
return CTR_OKAY;
|
||||
}
|
||||
void PDP::SetDeviceNumber(UINT8 deviceNumber){
|
||||
PDP_Settings.deviceNumber = deviceNumber;
|
||||
PDP_Settings.frameIDs[0] = 0x8041400 + (deviceNumber) + (UINT32) (0 * BIT6);
|
||||
PDP_Settings.frameIDs[1] = 0x8041400 + (deviceNumber) + (UINT32) (1 * BIT6);
|
||||
PDP_Settings.frameIDs[2] = 0x8041400 + (deviceNumber) + (UINT32) (2 * BIT6);
|
||||
}
|
||||
void PDP::GetErrorInfo( uint32_t * timeSinceLastRx,
|
||||
uint32_t * numFailedRxs)
|
||||
{
|
||||
if(timeSinceLastRx) *timeSinceLastRx = _timeSinceLastRx;
|
||||
if(numFailedRxs) *numFailedRxs = _numFailedRxs;
|
||||
}
|
||||
//------------------ CAN interface and thread --------------------------------------------//
|
||||
/* Search for PCM Status Frame on CAN bus */
|
||||
uint64_t PDP::ReadCurrents(uint8_t api)
|
||||
{
|
||||
uint64_t frame = 0;
|
||||
UINT8 size = 0;
|
||||
INT32 status = 0;
|
||||
UINT32 timeStamp = 0;
|
||||
|
||||
FRC_NetworkCommunication_CANSessionMux_receiveMessage(&PDP_Settings.frameIDs[api], kFullMessageIDMask, (uint8_t *)&frame, &size, &timeStamp, &status);
|
||||
if (status == 0) {
|
||||
_timeSinceLastRx = 0;
|
||||
} else {
|
||||
++_numFailedRxs;
|
||||
}
|
||||
return frame;
|
||||
}
|
||||
void * PDP::ThreadFunc()
|
||||
{
|
||||
while(_threadIsRunning){
|
||||
/* reads */
|
||||
uint64_t frame1 = ReadCurrents(0);
|
||||
uint64_t frame2 = ReadCurrents(1);
|
||||
uint64_t frame3 = ReadCurrents(2);
|
||||
/* update stats */
|
||||
memcpy(&_status1,&frame1,8);
|
||||
memcpy(&_status2,&frame2,8);
|
||||
memcpy(&_status3,&frame3,8);
|
||||
/* yield for 25ms */
|
||||
usleep(25e3);
|
||||
/* timeouts */
|
||||
if(_timeSinceLastRx < 60000)
|
||||
_timeSinceLastRx += 20;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
void * PDP::ThreadFunc( void *ptr )
|
||||
{
|
||||
return ((PDP*)ptr)->ThreadFunc();
|
||||
return rx.err;
|
||||
}
|
||||
//------------------ C interface --------------------------------------------//
|
||||
extern "C" {
|
||||
@@ -163,6 +148,5 @@ extern "C" {
|
||||
}
|
||||
void c_SetDeviceNumber_PDP(void * handle,UINT8 deviceNumber)
|
||||
{
|
||||
return ((PDP*)handle)-> SetDeviceNumber(deviceNumber);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user