Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 8fce748cf331a21a94eca51b0c81c5dedb8fbdc5 / . / hal / src / main / native / athena / PDP.cpp
blob: f5cf92b6870fdc1ae2b1fcd6fab16299f65f86d8 [file] [log] [blame]
Brian Silverman8fce7482020-01-05 13:18:21 -0800[diff] [blame^]1/*----------------------------------------------------------------------------*/
2/* Copyright (c) 2016-2019 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 <wpi/mutex.h>
13
14#include "HALInitializer.h"
15#include "PortsInternal.h"
16#include "hal/CANAPI.h"
17#include "hal/Errors.h"
18#include "hal/Ports.h"
19#include "hal/handles/IndexedHandleResource.h"
20
21using namespace hal;
22
23static constexpr HAL_CANManufacturer manufacturer =
24 HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
25
26static constexpr HAL_CANDeviceType deviceType =
27 HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
28
29static constexpr int32_t Status1 = 0x50;
30static constexpr int32_t Status2 = 0x51;
31static constexpr int32_t Status3 = 0x52;
32static constexpr int32_t StatusEnergy = 0x5D;
33
34static constexpr int32_t Control1 = 0x70;
35
36static constexpr int32_t TimeoutMs = 100;
37
38/* encoder/decoders */
39union PdpStatus1 {
40 uint8_t data[8];
41 struct Bits {
42 unsigned chan1_h8 : 8;
43 unsigned chan2_h6 : 6;
44 unsigned chan1_l2 : 2;
45 unsigned chan3_h4 : 4;
46 unsigned chan2_l4 : 4;
47 unsigned chan4_h2 : 2;
48 unsigned chan3_l6 : 6;
49 unsigned chan4_l8 : 8;
50 unsigned chan5_h8 : 8;
51 unsigned chan6_h6 : 6;
52 unsigned chan5_l2 : 2;
53 unsigned reserved4 : 4;
54 unsigned chan6_l4 : 4;
55 } bits;
56};
57
58union PdpStatus2 {
59 uint8_t data[8];
60 struct Bits {
61 unsigned chan7_h8 : 8;
62 unsigned chan8_h6 : 6;
63 unsigned chan7_l2 : 2;
64 unsigned chan9_h4 : 4;
65 unsigned chan8_l4 : 4;
66 unsigned chan10_h2 : 2;
67 unsigned chan9_l6 : 6;
68 unsigned chan10_l8 : 8;
69 unsigned chan11_h8 : 8;
70 unsigned chan12_h6 : 6;
71 unsigned chan11_l2 : 2;
72 unsigned reserved4 : 4;
73 unsigned chan12_l4 : 4;
74 } bits;
75};
76
77union PdpStatus3 {
78 uint8_t data[8];
79 struct Bits {
80 unsigned chan13_h8 : 8;
81 unsigned chan14_h6 : 6;
82 unsigned chan13_l2 : 2;
83 unsigned chan15_h4 : 4;
84 unsigned chan14_l4 : 4;
85 unsigned chan16_h2 : 2;
86 unsigned chan15_l6 : 6;
87 unsigned chan16_l8 : 8;
88 unsigned internalResBattery_mOhms : 8;
89 unsigned busVoltage : 8;
90 unsigned temp : 8;
91 } bits;
92};
93
94union PdpStatusEnergy {
95 uint8_t data[8];
96 struct Bits {
97 unsigned TmeasMs_likelywillbe20ms_ : 8;
98 unsigned TotalCurrent_125mAperunit_h8 : 8;
99 unsigned Power_125mWperunit_h4 : 4;
100 unsigned TotalCurrent_125mAperunit_l4 : 4;
101 unsigned Power_125mWperunit_m8 : 8;
102 unsigned Energy_125mWPerUnitXTmeas_h4 : 4;
103 unsigned Power_125mWperunit_l4 : 4;
104 unsigned Energy_125mWPerUnitXTmeas_mh8 : 8;
105 unsigned Energy_125mWPerUnitXTmeas_ml8 : 8;
106 unsigned Energy_125mWPerUnitXTmeas_l8 : 8;
107 } bits;
108};
109
110static wpi::mutex pdpHandleMutex;
111static HAL_PDPHandle pdpHandles[kNumPDPModules];
112
113namespace hal {
114namespace init {
115void InitializePDP() {
116 for (int i = 0; i < kNumPDPModules; i++) {
117 pdpHandles[i] = HAL_kInvalidHandle;
118 }
119}
120} // namespace init
121} // namespace hal
122
123extern "C" {
124
125HAL_PDPHandle HAL_InitializePDP(int32_t module, int32_t* status) {
126 hal::init::CheckInit();
127 if (!HAL_CheckPDPModule(module)) {
128 *status = PARAMETER_OUT_OF_RANGE;
129 return HAL_kInvalidHandle;
130 }
131
132 std::scoped_lock lock(pdpHandleMutex);
133
134 if (pdpHandles[module] != HAL_kInvalidHandle) {
135 *status = 0;
136 return pdpHandles[module];
137 }
138
139 auto handle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
140
141 if (*status != 0) {
142 HAL_CleanCAN(handle);
143 return HAL_kInvalidHandle;
144 }
145
146 pdpHandles[module] = handle;
147
148 return handle;
149}
150
151void HAL_CleanPDP(HAL_PDPHandle handle) {
152 HAL_CleanCAN(handle);
153
154 for (int i = 0; i < kNumPDPModules; i++) {
155 if (pdpHandles[i] == handle) {
156 pdpHandles[i] = HAL_kInvalidHandle;
157 return;
158 }
159 }
160}
161
162HAL_Bool HAL_CheckPDPModule(int32_t module) {
163 return module < kNumPDPModules && module >= 0;
164}
165
166HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
167 return channel < kNumPDPChannels && channel >= 0;
168}
169
170double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
171 PdpStatus3 pdpStatus;
172 int32_t length = 0;
173 uint64_t receivedTimestamp = 0;
174
175 HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
176 &receivedTimestamp, TimeoutMs, status);
177
178 if (*status != 0) {
179 return 0;
180 } else {
181 return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
182 }
183}
184
185double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
186 PdpStatus3 pdpStatus;
187 int32_t length = 0;
188 uint64_t receivedTimestamp = 0;
189
190 HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
191 &receivedTimestamp, TimeoutMs, status);
192
193 if (*status != 0) {
194 return 0;
195 } else {
196 return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
197 }
198}
199
200double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
201 int32_t* status) {
202 if (!HAL_CheckPDPChannel(channel)) {
203 *status = PARAMETER_OUT_OF_RANGE;
204 return 0;
205 }
206
207 int32_t length = 0;
208 uint64_t receivedTimestamp = 0;
209
210 double raw = 0;
211
212 if (channel <= 5) {
213 PdpStatus1 pdpStatus;
214 HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length,
215 &receivedTimestamp, TimeoutMs, status);
216 if (*status != 0) {
217 return 0;
218 }
219 switch (channel) {
220 case 0:
221 raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
222 pdpStatus.bits.chan1_l2;
223 break;
224 case 1:
225 raw = (static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
226 pdpStatus.bits.chan2_l4;
227 break;
228 case 2:
229 raw = (static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
230 pdpStatus.bits.chan3_l6;
231 break;
232 case 3:
233 raw = (static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
234 pdpStatus.bits.chan4_l8;
235 break;
236 case 4:
237 raw = (static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
238 pdpStatus.bits.chan5_l2;
239 break;
240 case 5:
241 raw = (static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
242 pdpStatus.bits.chan6_l4;
243 break;
244 }
245 } else if (channel <= 11) {
246 PdpStatus2 pdpStatus;
247 HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus.data, &length,
248 &receivedTimestamp, TimeoutMs, status);
249 if (*status != 0) {
250 return 0;
251 }
252 switch (channel) {
253 case 6:
254 raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
255 pdpStatus.bits.chan7_l2;
256 break;
257 case 7:
258 raw = (static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
259 pdpStatus.bits.chan8_l4;
260 break;
261 case 8:
262 raw = (static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
263 pdpStatus.bits.chan9_l6;
264 break;
265 case 9:
266 raw = (static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
267 pdpStatus.bits.chan10_l8;
268 break;
269 case 10:
270 raw = (static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
271 pdpStatus.bits.chan11_l2;
272 break;
273 case 11:
274 raw = (static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
275 pdpStatus.bits.chan12_l4;
276 break;
277 }
278 } else {
279 PdpStatus3 pdpStatus;
280 HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
281 &receivedTimestamp, TimeoutMs, status);
282 if (*status != 0) {
283 return 0;
284 }
285 switch (channel) {
286 case 12:
287 raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
288 pdpStatus.bits.chan13_l2;
289 break;
290 case 13:
291 raw = (static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
292 pdpStatus.bits.chan14_l4;
293 break;
294 case 14:
295 raw = (static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
296 pdpStatus.bits.chan15_l6;
297 break;
298 case 15:
299 raw = (static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
300 pdpStatus.bits.chan16_l8;
301 break;
302 }
303 }
304
305 /* convert to amps */
306 return raw * 0.125; /* 7.3 fixed pt value in Amps */
307}
308
309void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
310 int32_t* status) {
311 int32_t length = 0;
312 uint64_t receivedTimestamp = 0;
313 PdpStatus1 pdpStatus;
314 HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length,
315 &receivedTimestamp, TimeoutMs, status);
316 if (*status != 0) return;
317 PdpStatus2 pdpStatus2;
318 HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus2.data, &length,
319 &receivedTimestamp, TimeoutMs, status);
320 if (*status != 0) return;
321 PdpStatus3 pdpStatus3;
322 HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus3.data, &length,
323 &receivedTimestamp, TimeoutMs, status);
324 if (*status != 0) return;
325
326 currents[0] = ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
327 pdpStatus.bits.chan1_l2) *
328 0.125;
329 currents[1] = ((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
330 pdpStatus.bits.chan2_l4) *
331 0.125;
332 currents[2] = ((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
333 pdpStatus.bits.chan3_l6) *
334 0.125;
335 currents[3] = ((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
336 pdpStatus.bits.chan4_l8) *
337 0.125;
338 currents[4] = ((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
339 pdpStatus.bits.chan5_l2) *
340 0.125;
341 currents[5] = ((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
342 pdpStatus.bits.chan6_l4) *
343 0.125;
344
345 currents[6] = ((static_cast<uint32_t>(pdpStatus2.bits.chan7_h8) << 2) |
346 pdpStatus2.bits.chan7_l2) *
347 0.125;
348 currents[7] = ((static_cast<uint32_t>(pdpStatus2.bits.chan8_h6) << 4) |
349 pdpStatus2.bits.chan8_l4) *
350 0.125;
351 currents[8] = ((static_cast<uint32_t>(pdpStatus2.bits.chan9_h4) << 6) |
352 pdpStatus2.bits.chan9_l6) *
353 0.125;
354 currents[9] = ((static_cast<uint32_t>(pdpStatus2.bits.chan10_h2) << 8) |
355 pdpStatus2.bits.chan10_l8) *
356 0.125;
357 currents[10] = ((static_cast<uint32_t>(pdpStatus2.bits.chan11_h8) << 2) |
358 pdpStatus2.bits.chan11_l2) *
359 0.125;
360 currents[11] = ((static_cast<uint32_t>(pdpStatus2.bits.chan12_h6) << 4) |
361 pdpStatus2.bits.chan12_l4) *
362 0.125;
363
364 currents[12] = ((static_cast<uint32_t>(pdpStatus3.bits.chan13_h8) << 2) |
365 pdpStatus3.bits.chan13_l2) *
366 0.125;
367 currents[13] = ((static_cast<uint32_t>(pdpStatus3.bits.chan14_h6) << 4) |
368 pdpStatus3.bits.chan14_l4) *
369 0.125;
370 currents[14] = ((static_cast<uint32_t>(pdpStatus3.bits.chan15_h4) << 6) |
371 pdpStatus3.bits.chan15_l6) *
372 0.125;
373 currents[15] = ((static_cast<uint32_t>(pdpStatus3.bits.chan16_h2) << 8) |
374 pdpStatus3.bits.chan16_l8) *
375 0.125;
376}
377
378double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
379 PdpStatusEnergy pdpStatus;
380 int32_t length = 0;
381 uint64_t receivedTimestamp = 0;
382
383 HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
384 &receivedTimestamp, TimeoutMs, status);
385 if (*status != 0) {
386 return 0;
387 }
388
389 uint32_t raw;
390 raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
391 raw <<= 4;
392 raw |= pdpStatus.bits.TotalCurrent_125mAperunit_l4;
393 return 0.125 * raw; /* 7.3 fixed pt value in Amps */
394}
395
396double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
397 PdpStatusEnergy pdpStatus;
398 int32_t length = 0;
399 uint64_t receivedTimestamp = 0;
400
401 HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
402 &receivedTimestamp, TimeoutMs, status);
403 if (*status != 0) {
404 return 0;
405 }
406
407 uint32_t raw;
408 raw = pdpStatus.bits.Power_125mWperunit_h4;
409 raw <<= 8;
410 raw |= pdpStatus.bits.Power_125mWperunit_m8;
411 raw <<= 4;
412 raw |= pdpStatus.bits.Power_125mWperunit_l4;
413 return 0.125 * raw; /* 7.3 fixed pt value in Watts */
414}
415
416double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
417 PdpStatusEnergy pdpStatus;
418 int32_t length = 0;
419 uint64_t receivedTimestamp = 0;
420
421 HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
422 &receivedTimestamp, TimeoutMs, status);
423 if (*status != 0) {
424 return 0;
425 }
426
427 uint32_t raw;
428 raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
429 raw <<= 8;
430 raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_mh8;
431 raw <<= 8;
432 raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_ml8;
433 raw <<= 8;
434 raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_l8;
435
436 double energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
437 energyJoules *= 0.001; /* convert from mW to W */
438 energyJoules *=
439 pdpStatus.bits
440 .TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
441 return 0.125 * raw;
442}
443
444void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
445 uint8_t pdpControl[] = {0x40}; /* only bit set is ResetEnergy */
446 HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
447}
448
449void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
450 uint8_t pdpControl[] = {0x80}; /* only bit set is ClearStickyFaults */
451 HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
452}
453
454} // extern "C"