mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
Use new CAN API for PDP (#1081)
This commit is contained in:
Thad House
committed by
Peter Johnson
Peter Johnson
parent
f6e4df6a18
commit
056e68f2ae
@@ -9,51 +9,130 @@
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "HAL/CANAPI.h"
|
||||
#include "HAL/Errors.h"
|
||||
#include "HAL/Ports.h"
|
||||
#include "HAL/handles/IndexedHandleResource.h"
|
||||
#include "HALInitializer.h"
|
||||
#include "PortsInternal.h"
|
||||
#include "ctre/PDP.h"
|
||||
|
||||
using namespace hal;
|
||||
|
||||
static std::unique_ptr<PDP> pdp[kNumPDPModules];
|
||||
static constexpr HAL_CANManufacturer manufacturer =
|
||||
HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
|
||||
|
||||
static inline bool checkPDPInit(int32_t module, int32_t* status) {
|
||||
if (!HAL_CheckPDPModule(module)) {
|
||||
*status = RESOURCE_OUT_OF_RANGE;
|
||||
return false;
|
||||
}
|
||||
if (!pdp[module]) {
|
||||
*status = INCOMPATIBLE_STATE;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
static constexpr HAL_CANDeviceType deviceType =
|
||||
HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
|
||||
|
||||
static constexpr int32_t Status1 = 0x50;
|
||||
static constexpr int32_t Status2 = 0x51;
|
||||
static constexpr int32_t Status3 = 0x52;
|
||||
static constexpr int32_t StatusEnergy = 0x5D;
|
||||
|
||||
static constexpr int32_t Control1 = 0x70;
|
||||
|
||||
static constexpr int32_t TimeoutMs = 50;
|
||||
static constexpr int32_t StatusPeriodMs = 25;
|
||||
|
||||
/* encoder/decoders */
|
||||
union PdpStatus1 {
|
||||
uint8_t data[8];
|
||||
struct Bits {
|
||||
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;
|
||||
} bits;
|
||||
};
|
||||
|
||||
union PdpStatus2 {
|
||||
uint8_t data[8];
|
||||
struct Bits {
|
||||
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;
|
||||
} bits;
|
||||
};
|
||||
|
||||
union PdpStatus3 {
|
||||
uint8_t data[8];
|
||||
struct Bits {
|
||||
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;
|
||||
} bits;
|
||||
};
|
||||
|
||||
union PdpStatusEnergy {
|
||||
uint8_t data[8];
|
||||
struct Bits {
|
||||
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;
|
||||
} bits;
|
||||
};
|
||||
|
||||
namespace hal {
|
||||
namespace init {
|
||||
void InitializePDP() {
|
||||
for (int i = 0; i < kNumPDPModules; i++) {
|
||||
pdp[i] = nullptr;
|
||||
}
|
||||
}
|
||||
void InitializePDP() {}
|
||||
} // namespace init
|
||||
} // namespace hal
|
||||
|
||||
extern "C" {
|
||||
|
||||
void HAL_InitializePDP(int32_t module, int32_t* status) {
|
||||
HAL_PDPHandle HAL_InitializePDP(int32_t module, int32_t* status) {
|
||||
hal::init::CheckInit();
|
||||
if (!HAL_CheckPDPModule(module)) {
|
||||
*status = RESOURCE_OUT_OF_RANGE;
|
||||
return;
|
||||
*status = PARAMETER_OUT_OF_RANGE;
|
||||
return HAL_kInvalidHandle;
|
||||
}
|
||||
if (!pdp[module]) {
|
||||
pdp[module] = std::make_unique<PDP>(module);
|
||||
|
||||
auto handle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
|
||||
|
||||
if (*status != 0) {
|
||||
HAL_CleanCAN(handle);
|
||||
return HAL_kInvalidHandle;
|
||||
}
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
void HAL_CleanPDP(HAL_PDPHandle handle) { HAL_CleanCAN(handle); }
|
||||
|
||||
HAL_Bool HAL_CheckPDPModule(int32_t module) {
|
||||
return module < kNumPDPModules && module >= 0;
|
||||
}
|
||||
@@ -62,77 +141,201 @@ HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
|
||||
return channel < kNumPDPChannels && channel >= 0;
|
||||
}
|
||||
|
||||
double HAL_GetPDPTemperature(int32_t module, int32_t* status) {
|
||||
if (!checkPDPInit(module, status)) return 0;
|
||||
double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
|
||||
PdpStatus3 pdpStatus;
|
||||
int32_t length = 0;
|
||||
uint64_t receivedTimestamp = 0;
|
||||
|
||||
double temperature;
|
||||
HAL_ReadCANPeriodicPacket(handle, Status3, pdpStatus.data, &length,
|
||||
&receivedTimestamp, TimeoutMs, StatusPeriodMs,
|
||||
status);
|
||||
|
||||
*status = pdp[module]->GetTemperature(temperature);
|
||||
|
||||
return temperature;
|
||||
return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
|
||||
}
|
||||
|
||||
double HAL_GetPDPVoltage(int32_t module, int32_t* status) {
|
||||
if (!checkPDPInit(module, status)) return 0;
|
||||
double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
|
||||
PdpStatus3 pdpStatus;
|
||||
int32_t length = 0;
|
||||
uint64_t receivedTimestamp = 0;
|
||||
|
||||
double voltage;
|
||||
HAL_ReadCANPeriodicPacket(handle, Status3, pdpStatus.data, &length,
|
||||
&receivedTimestamp, TimeoutMs, StatusPeriodMs,
|
||||
status);
|
||||
|
||||
*status = pdp[module]->GetVoltage(voltage);
|
||||
|
||||
return voltage;
|
||||
return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
|
||||
}
|
||||
|
||||
double HAL_GetPDPChannelCurrent(int32_t module, int32_t channel,
|
||||
double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
|
||||
int32_t* status) {
|
||||
if (!checkPDPInit(module, status)) return 0;
|
||||
if (!HAL_CheckPDPChannel(channel)) {
|
||||
*status = PARAMETER_OUT_OF_RANGE;
|
||||
return 0;
|
||||
}
|
||||
|
||||
double current;
|
||||
int32_t length = 0;
|
||||
uint64_t receivedTimestamp = 0;
|
||||
|
||||
*status = pdp[module]->GetChannelCurrent(channel, current);
|
||||
double raw = 0;
|
||||
|
||||
return current;
|
||||
if (channel <= 5) {
|
||||
PdpStatus1 pdpStatus;
|
||||
HAL_ReadCANPeriodicPacket(handle, Status1, pdpStatus.data, &length,
|
||||
&receivedTimestamp, TimeoutMs, StatusPeriodMs,
|
||||
status);
|
||||
switch (channel) {
|
||||
case 0:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
|
||||
pdpStatus.bits.chan1_l2;
|
||||
break;
|
||||
case 1:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
|
||||
pdpStatus.bits.chan2_l4;
|
||||
break;
|
||||
case 2:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
|
||||
pdpStatus.bits.chan3_l6;
|
||||
break;
|
||||
case 3:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
|
||||
pdpStatus.bits.chan4_l8;
|
||||
break;
|
||||
case 4:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
|
||||
pdpStatus.bits.chan5_l2;
|
||||
break;
|
||||
case 5:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
|
||||
pdpStatus.bits.chan6_l4;
|
||||
break;
|
||||
}
|
||||
} else if (channel <= 11) {
|
||||
PdpStatus2 pdpStatus;
|
||||
HAL_ReadCANPeriodicPacket(handle, Status2, pdpStatus.data, &length,
|
||||
&receivedTimestamp, TimeoutMs, StatusPeriodMs,
|
||||
status);
|
||||
switch (channel) {
|
||||
case 6:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
|
||||
pdpStatus.bits.chan7_l2;
|
||||
break;
|
||||
case 7:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
|
||||
pdpStatus.bits.chan8_l4;
|
||||
break;
|
||||
case 8:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
|
||||
pdpStatus.bits.chan9_l6;
|
||||
break;
|
||||
case 9:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
|
||||
pdpStatus.bits.chan10_l8;
|
||||
break;
|
||||
case 10:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
|
||||
pdpStatus.bits.chan11_l2;
|
||||
break;
|
||||
case 11:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
|
||||
pdpStatus.bits.chan12_l4;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
PdpStatus3 pdpStatus;
|
||||
HAL_ReadCANPeriodicPacket(handle, Status3, pdpStatus.data, &length,
|
||||
&receivedTimestamp, TimeoutMs, StatusPeriodMs,
|
||||
status);
|
||||
switch (channel) {
|
||||
case 12:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
|
||||
pdpStatus.bits.chan13_l2;
|
||||
break;
|
||||
case 13:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
|
||||
pdpStatus.bits.chan14_l4;
|
||||
break;
|
||||
case 14:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
|
||||
pdpStatus.bits.chan15_l6;
|
||||
break;
|
||||
case 15:
|
||||
raw = (static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
|
||||
pdpStatus.bits.chan16_l8;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* convert to amps */
|
||||
return raw * 0.125; /* 7.3 fixed pt value in Amps */
|
||||
}
|
||||
|
||||
double HAL_GetPDPTotalCurrent(int32_t module, int32_t* status) {
|
||||
if (!checkPDPInit(module, status)) return 0;
|
||||
double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
|
||||
PdpStatusEnergy pdpStatus;
|
||||
int32_t length = 0;
|
||||
uint64_t receivedTimestamp = 0;
|
||||
|
||||
double current;
|
||||
HAL_ReadCANPeriodicPacket(handle, StatusEnergy, pdpStatus.data, &length,
|
||||
&receivedTimestamp, TimeoutMs, StatusPeriodMs,
|
||||
status);
|
||||
|
||||
*status = pdp[module]->GetTotalCurrent(current);
|
||||
|
||||
return current;
|
||||
uint32_t raw;
|
||||
raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
|
||||
raw <<= 4;
|
||||
raw |= pdpStatus.bits.TotalCurrent_125mAperunit_l4;
|
||||
return 0.125 * raw; /* 7.3 fixed pt value in Amps */
|
||||
}
|
||||
|
||||
double HAL_GetPDPTotalPower(int32_t module, int32_t* status) {
|
||||
if (!checkPDPInit(module, status)) return 0;
|
||||
double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
|
||||
PdpStatusEnergy pdpStatus;
|
||||
int32_t length = 0;
|
||||
uint64_t receivedTimestamp = 0;
|
||||
|
||||
double power;
|
||||
HAL_ReadCANPeriodicPacket(handle, StatusEnergy, pdpStatus.data, &length,
|
||||
&receivedTimestamp, TimeoutMs, StatusPeriodMs,
|
||||
status);
|
||||
|
||||
*status = pdp[module]->GetTotalPower(power);
|
||||
|
||||
return power;
|
||||
uint32_t raw;
|
||||
raw = pdpStatus.bits.Power_125mWperunit_h4;
|
||||
raw <<= 8;
|
||||
raw |= pdpStatus.bits.Power_125mWperunit_m8;
|
||||
raw <<= 4;
|
||||
raw |= pdpStatus.bits.Power_125mWperunit_l4;
|
||||
return 0.125 * raw; /* 7.3 fixed pt value in Watts */
|
||||
}
|
||||
|
||||
double HAL_GetPDPTotalEnergy(int32_t module, int32_t* status) {
|
||||
if (!checkPDPInit(module, status)) return 0;
|
||||
double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
|
||||
PdpStatusEnergy pdpStatus;
|
||||
int32_t length = 0;
|
||||
uint64_t receivedTimestamp = 0;
|
||||
|
||||
double energy;
|
||||
HAL_ReadCANPeriodicPacket(handle, StatusEnergy, pdpStatus.data, &length,
|
||||
&receivedTimestamp, TimeoutMs, StatusPeriodMs,
|
||||
status);
|
||||
|
||||
*status = pdp[module]->GetTotalEnergy(energy);
|
||||
uint32_t raw;
|
||||
raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
|
||||
raw <<= 8;
|
||||
raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_mh8;
|
||||
raw <<= 8;
|
||||
raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_ml8;
|
||||
raw <<= 8;
|
||||
raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_l8;
|
||||
|
||||
return energy;
|
||||
double energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
|
||||
energyJoules *= 0.001; /* convert from mW to W */
|
||||
energyJoules *=
|
||||
pdpStatus.bits
|
||||
.TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
|
||||
return 0.125 * raw;
|
||||
}
|
||||
|
||||
void HAL_ResetPDPTotalEnergy(int32_t module, int32_t* status) {
|
||||
if (!checkPDPInit(module, status)) return;
|
||||
|
||||
*status = pdp[module]->ResetEnergy();
|
||||
void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
|
||||
uint8_t pdpControl[] = {0x40}; /* only bit set is ResetEnergy */
|
||||
HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
|
||||
}
|
||||
|
||||
void HAL_ClearPDPStickyFaults(int32_t module, int32_t* status) {
|
||||
if (!checkPDPInit(module, status)) return;
|
||||
|
||||
*status = pdp[module]->ClearStickyFaults();
|
||||
void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
|
||||
uint8_t pdpControl[] = {0x80}; /* only bit set is ClearStickyFaults */
|
||||
HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
|
||||
@@ -1,230 +0,0 @@
|
||||
#include "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)
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -1,62 +0,0 @@
|
||||
#ifndef PDP_H_
|
||||
#define PDP_H_
|
||||
#include "ctre.h" //BIT Defines + Typedefs
|
||||
#include "CtreCanNode.h"
|
||||
class PDP : public CtreCanNode
|
||||
{
|
||||
public:
|
||||
/* Get PDP Channel Current
|
||||
*
|
||||
* @Param - deviceNumber - Device ID for PDP. Factory default is 60. Function defaults to 60.
|
||||
*/
|
||||
PDP(UINT8 deviceNumber=0);
|
||||
~PDP();
|
||||
/* Get PDP Channel Current
|
||||
*
|
||||
* @Return - CTR_Code - Error code (if any)
|
||||
*
|
||||
* @Param - idx - ID of channel to return current for (channels 1-16)
|
||||
*
|
||||
* @Param - status - Current of channel 'idx' in Amps (A)
|
||||
*/
|
||||
CTR_Code GetChannelCurrent(UINT8 idx, double &status);
|
||||
|
||||
/* Get Bus Voltage of PDP
|
||||
*
|
||||
* @Return - CTR_Code - Error code (if any)
|
||||
*
|
||||
* @Param - status - Voltage (V) across PDP
|
||||
*/
|
||||
CTR_Code GetVoltage(double &status);
|
||||
|
||||
/* Get Temperature of PDP
|
||||
*
|
||||
* @Return - CTR_Code - Error code (if any)
|
||||
*
|
||||
* @Param - status - Temperature of PDP in Centigrade / Celcius (C)
|
||||
*/
|
||||
CTR_Code GetTemperature(double &status);
|
||||
|
||||
CTR_Code GetTotalCurrent(double ¤tAmps);
|
||||
CTR_Code GetTotalPower(double &powerWatts);
|
||||
CTR_Code GetTotalEnergy(double &energyJoules);
|
||||
/* Clear sticky faults.
|
||||
* @Return - CTR_Code - Error code (if any)
|
||||
*/
|
||||
CTR_Code ClearStickyFaults();
|
||||
|
||||
/* Reset Energy Signals
|
||||
* @Return - CTR_Code - Error code (if any)
|
||||
*/
|
||||
CTR_Code ResetEnergy();
|
||||
private:
|
||||
uint64_t ReadCurrents(uint8_t api);
|
||||
};
|
||||
extern "C" {
|
||||
void * c_PDP_Init();
|
||||
CTR_Code c_GetChannelCurrent(void * handle,UINT8 idx, double *status);
|
||||
CTR_Code c_GetVoltage(void * handle,double *status);
|
||||
CTR_Code c_GetTemperature(void * handle,double *status);
|
||||
void c_SetDeviceNumber_PDP(void * handle,UINT8 deviceNumber);
|
||||
}
|
||||
#endif /* PDP_H_ */
|
||||
Reference in New Issue
Block a user