Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 26e4e5289bfe3b1691be9c14bedfb70bdcd6dc56 / . / hal / lib / Athena / ctre / PDP.cpp
blob: cae2264452009b246de250c53ba28ca0597d5c78 [file] [log] [blame]
Brian Silverman26e4e522015-12-17 01:56:40 -0500[diff] [blame^]1#include "ctre/PDP.h"
2#include "FRC_NetworkCommunication/CANSessionMux.h" //CAN Comm
3#include <string.h> // memset
4#include <unistd.h> // usleep
5
6#define STATUS_1 0x8041400
7#define STATUS_2 0x8041440
8#define STATUS_3 0x8041480
9#define STATUS_ENERGY 0x8041740
10
11#define CONTROL_1 0x08041C00 /* PDP_Control_ClearStats */
12
13#define EXPECTED_RESPONSE_TIMEOUT_MS (50)
14#define GET_STATUS1() CtreCanNode::recMsg<PdpStatus1_t> rx = GetRx<PdpStatus1_t>(STATUS_1,EXPECTED_RESPONSE_TIMEOUT_MS)
15#define GET_STATUS2() CtreCanNode::recMsg<PdpStatus2_t> rx = GetRx<PdpStatus2_t>(STATUS_2,EXPECTED_RESPONSE_TIMEOUT_MS)
16#define GET_STATUS3() CtreCanNode::recMsg<PdpStatus3_t> rx = GetRx<PdpStatus3_t>(STATUS_3,EXPECTED_RESPONSE_TIMEOUT_MS)
17#define GET_STATUS_ENERGY() CtreCanNode::recMsg<PDP_Status_Energy_t> rx = GetRx<PDP_Status_Energy_t>(STATUS_ENERGY,EXPECTED_RESPONSE_TIMEOUT_MS)
18
19/* encoder/decoders */
20typedef struct _PdpStatus1_t{
21 unsigned chan1_h8:8;
22 unsigned chan2_h6:6;
23 unsigned chan1_l2:2;
24 unsigned chan3_h4:4;
25 unsigned chan2_l4:4;
26 unsigned chan4_h2:2;
27 unsigned chan3_l6:6;
28 unsigned chan4_l8:8;
29 unsigned chan5_h8:8;
30 unsigned chan6_h6:6;
31 unsigned chan5_l2:2;
32 unsigned reserved4:4;
33 unsigned chan6_l4:4;
34}PdpStatus1_t;
35typedef struct _PdpStatus2_t{
36 unsigned chan7_h8:8;
37 unsigned chan8_h6:6;
38 unsigned chan7_l2:2;
39 unsigned chan9_h4:4;
40 unsigned chan8_l4:4;
41 unsigned chan10_h2:2;
42 unsigned chan9_l6:6;
43 unsigned chan10_l8:8;
44 unsigned chan11_h8:8;
45 unsigned chan12_h6:6;
46 unsigned chan11_l2:2;
47 unsigned reserved4:4;
48 unsigned chan12_l4:4;
49}PdpStatus2_t;
50typedef struct _PdpStatus3_t{
51 unsigned chan13_h8:8;
52 unsigned chan14_h6:6;
53 unsigned chan13_l2:2;
54 unsigned chan15_h4:4;
55 unsigned chan14_l4:4;
56 unsigned chan16_h2:2;
57 unsigned chan15_l6:6;
58 unsigned chan16_l8:8;
59 unsigned internalResBattery_mOhms:8;
60 unsigned busVoltage:8;
61 unsigned temp:8;
62}PdpStatus3_t;
63typedef struct _PDP_Status_Energy_t {
64 unsigned TmeasMs_likelywillbe20ms_:8;
65 unsigned TotalCurrent_125mAperunit_h8:8;
66 unsigned Power_125mWperunit_h4:4;
67 unsigned TotalCurrent_125mAperunit_l4:4;
68 unsigned Power_125mWperunit_m8:8;
69 unsigned Energy_125mWPerUnitXTmeas_h4:4;
70 unsigned Power_125mWperunit_l4:4;
71 unsigned Energy_125mWPerUnitXTmeas_mh8:8;
72 unsigned Energy_125mWPerUnitXTmeas_ml8:8;
73 unsigned Energy_125mWPerUnitXTmeas_l8:8;
74} PDP_Status_Energy_t ;
75
76PDP::PDP(UINT8 deviceNumber): CtreCanNode(deviceNumber)
77{
78 RegisterRx(STATUS_1 | deviceNumber );
79 RegisterRx(STATUS_2 | deviceNumber );
80 RegisterRx(STATUS_3 | deviceNumber );
81}
82/* PDP D'tor
83 */
84PDP::~PDP()
85{
86}
87
88CTR_Code PDP::GetChannelCurrent(UINT8 idx, double &current)
89{
90 CTR_Code retval = CTR_InvalidParamValue;
91 uint32_t raw = 0;
92
93 if(idx <= 5){
94 GET_STATUS1();
95 retval = rx.err;
96 switch(idx){
97 case 0: raw = ((uint32_t)rx->chan1_h8 << 2) | rx->chan1_l2; break;
98 case 1: raw = ((uint32_t)rx->chan2_h6 << 4) | rx->chan2_l4; break;
99 case 2: raw = ((uint32_t)rx->chan3_h4 << 6) | rx->chan3_l6; break;
100 case 3: raw = ((uint32_t)rx->chan4_h2 << 8) | rx->chan4_l8; break;
101 case 4: raw = ((uint32_t)rx->chan5_h8 << 2) | rx->chan5_l2; break;
102 case 5: raw = ((uint32_t)rx->chan6_h6 << 4) | rx->chan6_l4; break;
103 default: retval = CTR_InvalidParamValue; break;
104 }
105 }else if(idx <= 11){
106 GET_STATUS2();
107 retval = rx.err;
108 switch(idx){
109 case 6: raw = ((uint32_t)rx->chan7_h8 << 2) | rx->chan7_l2; break;
110 case 7: raw = ((uint32_t)rx->chan8_h6 << 4) | rx->chan8_l4; break;
111 case 8: raw = ((uint32_t)rx->chan9_h4 << 6) | rx->chan9_l6; break;
112 case 9: raw = ((uint32_t)rx->chan10_h2 << 8) | rx->chan10_l8; break;
113 case 10: raw = ((uint32_t)rx->chan11_h8 << 2) | rx->chan11_l2; break;
114 case 11: raw = ((uint32_t)rx->chan12_h6 << 4) | rx->chan12_l4; break;
115 default: retval = CTR_InvalidParamValue; break;
116 }
117 }else if(idx <= 15){
118 GET_STATUS3();
119 retval = rx.err;
120 switch(idx){
121 case 12: raw = ((uint32_t)rx->chan13_h8 << 2) | rx->chan13_l2; break;
122 case 13: raw = ((uint32_t)rx->chan14_h6 << 4) | rx->chan14_l4; break;
123 case 14: raw = ((uint32_t)rx->chan15_h4 << 6) | rx->chan15_l6; break;
124 case 15: raw = ((uint32_t)rx->chan16_h2 << 8) | rx->chan16_l8; break;
125 default: retval = CTR_InvalidParamValue; break;
126 }
127 }
128 /* convert to amps */
129 current = (double)raw * 0.125; /* 7.3 fixed pt value in Amps */
130 /* signal caller with success */
131 return retval;
132}
133CTR_Code PDP::GetVoltage(double &voltage)
134{
135 GET_STATUS3();
136 uint32_t raw = rx->busVoltage;
137 voltage = (double)raw * 0.05 + 4.0; /* 50mV per unit plus 4V. */;
138 return rx.err;
139}
140CTR_Code PDP::GetTemperature(double &tempC)
141{
142 GET_STATUS3();
143 uint32_t raw = rx->temp;
144 tempC = (double)raw * 1.03250836957542 - 67.8564500484966;
145 return rx.err;
146}
147CTR_Code PDP::GetTotalCurrent(double &currentAmps)
148{
149 GET_STATUS_ENERGY();
150 uint32_t raw;
151 raw = rx->TotalCurrent_125mAperunit_h8;
152 raw <<= 4;
153 raw |= rx->TotalCurrent_125mAperunit_l4;
154 currentAmps = 0.125 * raw;
155 return rx.err;
156}
157CTR_Code PDP::GetTotalPower(double &powerWatts)
158{
159 GET_STATUS_ENERGY();
160 uint32_t raw;
161 raw = rx->Power_125mWperunit_h4;
162 raw <<= 8;
163 raw |= rx->Power_125mWperunit_m8;
164 raw <<= 4;
165 raw |= rx->Power_125mWperunit_l4;
166 powerWatts = 0.125 * raw;
167 return rx.err;
168}
169CTR_Code PDP::GetTotalEnergy(double &energyJoules)
170{
171 GET_STATUS_ENERGY();
172 uint32_t raw;
173 raw = rx->Energy_125mWPerUnitXTmeas_h4;
174 raw <<= 8;
175 raw |= rx->Energy_125mWPerUnitXTmeas_mh8;
176 raw <<= 8;
177 raw |= rx->Energy_125mWPerUnitXTmeas_ml8;
178 raw <<= 8;
179 raw |= rx->Energy_125mWPerUnitXTmeas_l8;
180 energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
181 energyJoules *= 0.001; /* convert from mW to W */
182 energyJoules *= rx->TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
183 return rx.err;
184}
185/* Clear sticky faults.
186 * @Return - CTR_Code - Error code (if any)
187 */
188CTR_Code PDP::ClearStickyFaults()
189{
190 int32_t status = 0;
191 uint8_t pdpControl[] = { 0x80 }; /* only bit set is ClearStickyFaults */
192 FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_1 | GetDeviceNumber(), pdpControl, sizeof(pdpControl), 0, &status);
193 if(status)
194 return CTR_TxFailed;
195 return CTR_OKAY;
196}
197
198/* Reset Energy Signals
199 * @Return - CTR_Code - Error code (if any)
200 */
201CTR_Code PDP::ResetEnergy()
202{
203 int32_t status = 0;
204 uint8_t pdpControl[] = { 0x40 }; /* only bit set is ResetEnergy */
205 FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_1 | GetDeviceNumber(), pdpControl, sizeof(pdpControl), 0, &status);
206 if(status)
207 return CTR_TxFailed;
208 return CTR_OKAY;
209}
210//------------------ C interface --------------------------------------------//
211extern "C" {
212 void * c_PDP_Init(void)
213 {
214 return new PDP();
215 }
216 CTR_Code c_GetChannelCurrent(void * handle,UINT8 idx, double *status)
217 {
218 return ((PDP*)handle)-> GetChannelCurrent(idx,*status);
219 }
220 CTR_Code c_GetVoltage(void * handle,double *status)
221 {
222 return ((PDP*)handle)-> GetVoltage(*status);
223 }
224 CTR_Code c_GetTemperature(void * handle,double *status)
225 {
226 return ((PDP*)handle)-> GetTemperature(*status);
227 }
228 void c_SetDeviceNumber_PDP(void * handle,UINT8 deviceNumber)
229 {
230 }
231}