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