Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 41cdd3e87217ce13d4167a1fda7baa9c60fefb8c / . / hal / src / main / native / athena / PDP.cpp
blob: ff1eb8149a1919f4b2ada14be5898b48c564c259 [file] [log] [blame]
Brian Silverman41cdd3e2019-01-19 19:48:58 -0800[diff] [blame^]1/*----------------------------------------------------------------------------*/
2/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
3/* Open Source Software - may be modified and shared by FRC teams. The code */
4/* must be accompanied by the FIRST BSD license file in the root directory of */
5/* the project. */
6/*----------------------------------------------------------------------------*/
7
8#include "hal/PDP.h"
9
10#include <memory>
11
12#include "HALInitializer.h"
13#include "PortsInternal.h"
14#include "hal/CANAPI.h"
15#include "hal/Errors.h"
16#include "hal/Ports.h"
17#include "hal/handles/IndexedHandleResource.h"
18
19using namespace hal;
20
21static constexpr HAL_CANManufacturer manufacturer =
22 HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
23
24static constexpr HAL_CANDeviceType deviceType =
25 HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
26
27static constexpr int32_t Status1 = 0x50;
28static constexpr int32_t Status2 = 0x51;
29static constexpr int32_t Status3 = 0x52;
30static constexpr int32_t StatusEnergy = 0x5D;
31
32static constexpr int32_t Control1 = 0x70;
33
34static constexpr int32_t TimeoutMs = 100;
35static constexpr int32_t StatusPeriodMs = 25;
36
37/* encoder/decoders */
38union PdpStatus1 {
39 uint8_t data[8];
40 struct Bits {
41 unsigned chan1_h8 : 8;
42 unsigned chan2_h6 : 6;
43 unsigned chan1_l2 : 2;
44 unsigned chan3_h4 : 4;
45 unsigned chan2_l4 : 4;
46 unsigned chan4_h2 : 2;
47 unsigned chan3_l6 : 6;
48 unsigned chan4_l8 : 8;
49 unsigned chan5_h8 : 8;
50 unsigned chan6_h6 : 6;
51 unsigned chan5_l2 : 2;
52 unsigned reserved4 : 4;
53 unsigned chan6_l4 : 4;
54 } bits;
55};
56
57union PdpStatus2 {
58 uint8_t data[8];
59 struct Bits {
60 unsigned chan7_h8 : 8;
61 unsigned chan8_h6 : 6;
62 unsigned chan7_l2 : 2;
63 unsigned chan9_h4 : 4;
64 unsigned chan8_l4 : 4;
65 unsigned chan10_h2 : 2;
66 unsigned chan9_l6 : 6;
67 unsigned chan10_l8 : 8;
68 unsigned chan11_h8 : 8;
69 unsigned chan12_h6 : 6;
70 unsigned chan11_l2 : 2;
71 unsigned reserved4 : 4;
72 unsigned chan12_l4 : 4;
73 } bits;
74};
75
76union PdpStatus3 {
77 uint8_t data[8];
78 struct Bits {
79 unsigned chan13_h8 : 8;
80 unsigned chan14_h6 : 6;
81 unsigned chan13_l2 : 2;
82 unsigned chan15_h4 : 4;
83 unsigned chan14_l4 : 4;
84 unsigned chan16_h2 : 2;
85 unsigned chan15_l6 : 6;
86 unsigned chan16_l8 : 8;
87 unsigned internalResBattery_mOhms : 8;
88 unsigned busVoltage : 8;
89 unsigned temp : 8;
90 } bits;
91};
92
93union PdpStatusEnergy {
94 uint8_t data[8];
95 struct Bits {
96 unsigned TmeasMs_likelywillbe20ms_ : 8;
97 unsigned TotalCurrent_125mAperunit_h8 : 8;
98 unsigned Power_125mWperunit_h4 : 4;
99 unsigned TotalCurrent_125mAperunit_l4 : 4;
100 unsigned Power_125mWperunit_m8 : 8;
101 unsigned Energy_125mWPerUnitXTmeas_h4 : 4;
102 unsigned Power_125mWperunit_l4 : 4;
103 unsigned Energy_125mWPerUnitXTmeas_mh8 : 8;
104 unsigned Energy_125mWPerUnitXTmeas_ml8 : 8;
105 unsigned Energy_125mWPerUnitXTmeas_l8 : 8;
106 } bits;
107};
108
109namespace hal {
110namespace init {
111void InitializePDP() {}
112} // namespace init
113} // namespace hal
114
115extern "C" {
116
117HAL_PDPHandle HAL_InitializePDP(int32_t module, int32_t* status) {
118 hal::init::CheckInit();
119 if (!HAL_CheckPDPModule(module)) {
120 *status = PARAMETER_OUT_OF_RANGE;
121 return HAL_kInvalidHandle;
122 }
123
124 auto handle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
125
126 if (*status != 0) {
127 HAL_CleanCAN(handle);
128 return HAL_kInvalidHandle;
129 }
130
131 return handle;
132}
133
134void HAL_CleanPDP(HAL_PDPHandle handle) { HAL_CleanCAN(handle); }
135
136HAL_Bool HAL_CheckPDPModule(int32_t module) {
137 return module < kNumPDPModules && module >= 0;
138}
139
140HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
141 return channel < kNumPDPChannels && channel >= 0;
142}
143
144double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
145 PdpStatus3 pdpStatus;
146 int32_t length = 0;
147 uint64_t receivedTimestamp = 0;
148
149 HAL_ReadCANPeriodicPacket(handle, Status3, pdpStatus.data, &length,
150 &receivedTimestamp, TimeoutMs, StatusPeriodMs,
151 status);
152
153 return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
154}
155
156double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
157 PdpStatus3 pdpStatus;
158 int32_t length = 0;
159 uint64_t receivedTimestamp = 0;
160
161 HAL_ReadCANPeriodicPacket(handle, Status3, pdpStatus.data, &length,
162 &receivedTimestamp, TimeoutMs, StatusPeriodMs,
163 status);
164
165 return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
166}
167
168double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
169 int32_t* status) {
170 if (!HAL_CheckPDPChannel(channel)) {
171 *status = PARAMETER_OUT_OF_RANGE;
172 return 0;
173 }
174
175 int32_t length = 0;
176 uint64_t receivedTimestamp = 0;
177
178 double raw = 0;
179
180 if (channel <= 5) {
181 PdpStatus1 pdpStatus;
182 HAL_ReadCANPeriodicPacket(handle, Status1, pdpStatus.data, &length,
183 &receivedTimestamp, TimeoutMs, StatusPeriodMs,
184 status);
185 switch (channel) {
186 case 0:
187 raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
188 pdpStatus.bits.chan1_l2;
189 break;
190 case 1:
191 raw = (static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
192 pdpStatus.bits.chan2_l4;
193 break;
194 case 2:
195 raw = (static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
196 pdpStatus.bits.chan3_l6;
197 break;
198 case 3:
199 raw = (static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
200 pdpStatus.bits.chan4_l8;
201 break;
202 case 4:
203 raw = (static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
204 pdpStatus.bits.chan5_l2;
205 break;
206 case 5:
207 raw = (static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
208 pdpStatus.bits.chan6_l4;
209 break;
210 }
211 } else if (channel <= 11) {
212 PdpStatus2 pdpStatus;
213 HAL_ReadCANPeriodicPacket(handle, Status2, pdpStatus.data, &length,
214 &receivedTimestamp, TimeoutMs, StatusPeriodMs,
215 status);
216 switch (channel) {
217 case 6:
218 raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
219 pdpStatus.bits.chan7_l2;
220 break;
221 case 7:
222 raw = (static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
223 pdpStatus.bits.chan8_l4;
224 break;
225 case 8:
226 raw = (static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
227 pdpStatus.bits.chan9_l6;
228 break;
229 case 9:
230 raw = (static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
231 pdpStatus.bits.chan10_l8;
232 break;
233 case 10:
234 raw = (static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
235 pdpStatus.bits.chan11_l2;
236 break;
237 case 11:
238 raw = (static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
239 pdpStatus.bits.chan12_l4;
240 break;
241 }
242 } else {
243 PdpStatus3 pdpStatus;
244 HAL_ReadCANPeriodicPacket(handle, Status3, pdpStatus.data, &length,
245 &receivedTimestamp, TimeoutMs, StatusPeriodMs,
246 status);
247 switch (channel) {
248 case 12:
249 raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
250 pdpStatus.bits.chan13_l2;
251 break;
252 case 13:
253 raw = (static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
254 pdpStatus.bits.chan14_l4;
255 break;
256 case 14:
257 raw = (static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
258 pdpStatus.bits.chan15_l6;
259 break;
260 case 15:
261 raw = (static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
262 pdpStatus.bits.chan16_l8;
263 break;
264 }
265 }
266
267 /* convert to amps */
268 return raw * 0.125; /* 7.3 fixed pt value in Amps */
269}
270
271double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
272 PdpStatusEnergy pdpStatus;
273 int32_t length = 0;
274 uint64_t receivedTimestamp = 0;
275
276 HAL_ReadCANPeriodicPacket(handle, StatusEnergy, pdpStatus.data, &length,
277 &receivedTimestamp, TimeoutMs, StatusPeriodMs,
278 status);
279
280 uint32_t raw;
281 raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
282 raw <<= 4;
283 raw |= pdpStatus.bits.TotalCurrent_125mAperunit_l4;
284 return 0.125 * raw; /* 7.3 fixed pt value in Amps */
285}
286
287double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
288 PdpStatusEnergy pdpStatus;
289 int32_t length = 0;
290 uint64_t receivedTimestamp = 0;
291
292 HAL_ReadCANPeriodicPacket(handle, StatusEnergy, pdpStatus.data, &length,
293 &receivedTimestamp, TimeoutMs, StatusPeriodMs,
294 status);
295
296 uint32_t raw;
297 raw = pdpStatus.bits.Power_125mWperunit_h4;
298 raw <<= 8;
299 raw |= pdpStatus.bits.Power_125mWperunit_m8;
300 raw <<= 4;
301 raw |= pdpStatus.bits.Power_125mWperunit_l4;
302 return 0.125 * raw; /* 7.3 fixed pt value in Watts */
303}
304
305double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
306 PdpStatusEnergy pdpStatus;
307 int32_t length = 0;
308 uint64_t receivedTimestamp = 0;
309
310 HAL_ReadCANPeriodicPacket(handle, StatusEnergy, pdpStatus.data, &length,
311 &receivedTimestamp, TimeoutMs, StatusPeriodMs,
312 status);
313
314 uint32_t raw;
315 raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
316 raw <<= 8;
317 raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_mh8;
318 raw <<= 8;
319 raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_ml8;
320 raw <<= 8;
321 raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_l8;
322
323 double energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
324 energyJoules *= 0.001; /* convert from mW to W */
325 energyJoules *=
326 pdpStatus.bits
327 .TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
328 return 0.125 * raw;
329}
330
331void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
332 uint8_t pdpControl[] = {0x40}; /* only bit set is ResetEnergy */
333 HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
334}
335
336void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
337 uint8_t pdpControl[] = {0x80}; /* only bit set is ClearStickyFaults */
338 HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
339}
340
341} // extern "C"