Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal, wpilib] Rewrite CAN APIs (#7798)

Browse Source
This commit is contained in:
Thad House
2025-02-25 19:07:01 -08:00
committed by GitHub
parent b39744b562
commit baa20fa239
107 changed files with 1447 additions and 1379 deletions
Download Patch File Download Diff File Expand all files Collapse all files

135
hal/src/main/native/systemcore/CAN.cpp
View File

@@ -21,14 +21,13 @@
#include <wpi/mutex.h>
#include <wpi/timestamp.h>
#include "PortsInternal.h"
#include "hal/Errors.h"
#include "hal/Threads.h"
#include "wpinet/EventLoopRunner.h"
#include "wpinet/uv/Poll.h"
#include "wpinet/uv/Timer.h"
#define NUM_CAN_BUSES 1
namespace {
static constexpr uint32_t MatchingBitMask = CAN_EFF_MASK | CAN_RTR_FLAG;
@@ -68,20 +67,21 @@ struct FrameStore {
struct SocketCanState {
wpi::EventLoopRunner readLoopRunner;
wpi::EventLoopRunner writeLoopRunner;
wpi::mutex writeMutex[NUM_CAN_BUSES];
int socketHandle[NUM_CAN_BUSES];
wpi::mutex writeMutex[hal::kNumCanBuses];
int socketHandle[hal::kNumCanBuses];
// ms to count/timer map
wpi::DenseMap<uint16_t, std::pair<size_t, std::weak_ptr<wpi::uv::Timer>>>
timers;
// ms to bus mask/packet
wpi::DenseMap<uint16_t, std::array<std::optional<canfd_frame>, NUM_CAN_BUSES>>
wpi::DenseMap<uint16_t,
std::array<std::optional<canfd_frame>, hal::kNumCanBuses>>
timedFrames;
// packet to time
wpi::DenseMap<uint32_t, std::array<uint16_t, NUM_CAN_BUSES>> packetToTime;
wpi::DenseMap<uint32_t, std::array<uint16_t, hal::kNumCanBuses>> packetToTime;
wpi::mutex readMutex[NUM_CAN_BUSES];
wpi::mutex readMutex[hal::kNumCanBuses];
// TODO(thadhouse) we need a MUCH better way of doing this masking
wpi::DenseMap<uint32_t, FrameStore> readFrames[NUM_CAN_BUSES];
wpi::DenseMap<uint32_t, FrameStore> readFrames[hal::kNumCanBuses];
bool InitializeBuses();
@@ -111,7 +111,7 @@ bool SocketCanState::InitializeBuses() {
std::printf("Failed to set CAN thread priority\n");
}
for (int i = 0; i < NUM_CAN_BUSES; i++) {
for (int i = 0; i < hal::kNumCanBuses; i++) {
std::scoped_lock lock{writeMutex[i]};
socketHandle[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (socketHandle[i] == -1) {
@@ -240,23 +240,21 @@ namespace {} // namespace
extern "C" {
void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
uint8_t dataSize, int32_t periodMs, int32_t* status) {
// TODO(thadhouse) this will become a parameter
// isFd will also be a part of this parameter
uint8_t busId = 0;
if (busId >= NUM_CAN_BUSES) {
void HAL_CAN_SendMessage(int32_t busId, uint32_t messageId,
const struct HAL_CANMessage* message, int32_t periodMs,
int32_t* status) {
if (busId < 0 || busId >= hal::kNumCanBuses) {
*status = PARAMETER_OUT_OF_RANGE;
return;
}
bool isFd = false;
messageID = MapMessageIdToSocketCan(messageID);
messageId = MapMessageIdToSocketCan(messageId);
// TODO determine on the real roborio is a non periodic send removes any
// periodic send.
if (periodMs == HAL_CAN_SEND_PERIOD_STOP_REPEATING) {
canState->writeLoopRunner.ExecSync([messageID, busId](wpi::uv::Loop&) {
canState->RemovePeriodic(busId, messageID);
canState->writeLoopRunner.ExecSync([messageId, busId](wpi::uv::Loop&) {
canState->RemovePeriodic(busId, messageId);
});
*status = 0;
@@ -265,15 +263,20 @@ void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
canfd_frame frame;
std::memset(&frame, 0, sizeof(frame));
frame.can_id = messageID;
frame.flags = isFd ? CANFD_FDF | CANFD_BRS : 0;
if (dataSize) {
auto size = (std::min)(dataSize, static_cast<uint8_t>(sizeof(frame.data)));
std::memcpy(frame.data, data, size);
frame.can_id = messageId;
frame.flags |=
(message->flags & HAL_CANFlags::HAL_CAN_FD_DATALENGTH) ? CANFD_FDF : 0;
frame.flags |=
(message->flags & HAL_CANFlags::HAL_CAN_FD_BITRATESWITCH) ? CANFD_BRS : 0;
if (message->dataSize) {
auto size =
(std::min)(message->dataSize, static_cast<uint8_t>(sizeof(frame.data)));
std::memcpy(frame.data, message->data, size);
frame.len = size;
}
int mtu = isFd ? CANFD_MTU : CAN_MTU;
int mtu = (message->flags & HAL_CANFlags::HAL_CAN_FD_DATALENGTH) ? CANFD_MTU
: CAN_MTU;
{
std::scoped_lock lock{canState->writeMutex[busId]};
int result = send(canState->socketHandle[busId], &frame, mtu, 0);
@@ -291,70 +294,62 @@ void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
});
}
}
void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
uint8_t* data, uint8_t* dataSize,
uint32_t* timeStamp, int32_t* status) {
uint8_t busId = 0;
if (busId >= NUM_CAN_BUSES) {
void HAL_CAN_ReceiveMessage(int32_t busId, uint32_t messageId,
struct HAL_CANReceiveMessage* message,
int32_t* status) {
if (busId < 0 || busId >= hal::kNumCanBuses) {
message->message.dataSize = 0;
message->timeStamp = 0;
*status = PARAMETER_OUT_OF_RANGE;
return;
}
std::scoped_lock lock{canState->readMutex[busId]};
// TODO(thadhouse) this is going to be wrong, but we're going to assume that
// any lookup without the 11 bit mask set wants to look for a 29 bit frame.
// Also, only do fast lookups for 29 bit frames
// Fast case is the following.
// Mask doesn't include 11 bit flag
// Mask doesn't include RTR flag
// Mask is full
if (messageIDMask == CAN_EFF_MASK) {
// We're doing a fast lookup
auto& msg = canState->readFrames[busId][*messageID];
if (msg.timestamp == 0) {
*status = HAL_ERR_CANSessionMux_MessageNotFound;
return;
}
if ((msg.frame.flags & CANFD_FDF) || msg.frame.len > 8) {
std::printf("FD frames not supported for read right now\n");
*status = HAL_ERR_CANSessionMux_InvalidBuffer;
return;
}
// TODO(thadhouse) this time needs to be fixed up.
*timeStamp = msg.timestamp / 1000;
std::memcpy(data, msg.frame.data, msg.frame.len);
*dataSize = msg.frame.len;
*status = 0;
msg.timestamp = 0;
auto& msg = canState->readFrames[busId][messageId];
if (msg.timestamp == 0) {
message->message.dataSize = 0;
message->timeStamp = 0;
*status = HAL_ERR_CANSessionMux_MessageNotFound;
return;
}
std::printf("Slow lookup not supported yet\n");
message->message.flags = HAL_CANFlags::HAL_CAN_NO_FLAGS;
message->message.flags |= (msg.frame.flags & CANFD_FDF)
? HAL_CANFlags::HAL_CAN_FD_DATALENGTH
: HAL_CANFlags::HAL_CAN_NO_FLAGS;
message->message.flags |= (msg.frame.flags & CANFD_BRS)
? HAL_CANFlags::HAL_CAN_FD_BITRATESWITCH
: HAL_CANFlags::HAL_CAN_NO_FLAGS;
// Add support for slow lookup later
*status = HAL_ERR_CANSessionMux_NotAllowed;
message->message.dataSize = msg.frame.len;
if (msg.frame.len > 0) {
std::memcpy(message->message.data, msg.frame.data, msg.frame.len);
}
message->timeStamp = msg.timestamp;
*status = 0;
msg.timestamp = 0;
return;
}
void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
uint32_t messageIDMask, uint32_t maxMessages,
int32_t* status) {
*sessionHandle = 0;
HAL_CANStreamHandle HAL_CAN_OpenStreamSession(int32_t busId, uint32_t messageId,
uint32_t messageIDMask,
uint32_t maxMessages,
int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
return 0;
}
void HAL_CAN_CloseStreamSession(uint32_t sessionHandle) {}
void HAL_CAN_ReadStreamSession(uint32_t sessionHandle,
void HAL_CAN_CloseStreamSession(HAL_CANStreamHandle sessionHandle) {}
void HAL_CAN_ReadStreamSession(HAL_CANStreamHandle sessionHandle,
struct HAL_CANStreamMessage* messages,
uint32_t messagesToRead, uint32_t* messagesRead,
int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_CAN_GetCANStatus(float* percentBusUtilization, uint32_t* busOffCount,
uint32_t* txFullCount, uint32_t* receiveErrorCount,
void HAL_CAN_GetCANStatus(int32_t busId, float* percentBusUtilization,
uint32_t* busOffCount, uint32_t* txFullCount,
uint32_t* receiveErrorCount,
uint32_t* transmitErrorCount, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;

125
hal/src/main/native/systemcore/CANAPI.cpp
View File

@@ -12,6 +12,7 @@
#include <wpi/timestamp.h>
#include "HALInitializer.h"
#include "PortsInternal.h"
#include "hal/CAN.h"
#include "hal/Errors.h"
#include "hal/handles/UnlimitedHandleResource.h"
@@ -19,20 +20,15 @@
using namespace hal;
namespace {
struct Receives {
uint32_t lastTimeStamp;
uint8_t data[8];
uint8_t length;
};
struct CANStorage {
HAL_CANManufacturer manufacturer;
HAL_CANDeviceType deviceType;
int32_t busId;
uint8_t deviceId;
wpi::mutex periodicSendsMutex;
wpi::SmallDenseMap<int32_t, int32_t> periodicSends;
wpi::mutex receivesMutex;
wpi::SmallDenseMap<int32_t, Receives> receives;
wpi::SmallDenseMap<int32_t, HAL_CANReceiveMessage> receives;
};
} // namespace
@@ -58,14 +54,16 @@ static int32_t CreateCANId(CANStorage* storage, int32_t apiId) {
extern "C" {
uint32_t HAL_GetCANPacketBaseTime(void) {
return wpi::Now() / 1000;
}
HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
HAL_CANHandle HAL_InitializeCAN(int32_t busId, HAL_CANManufacturer manufacturer,
int32_t deviceId, HAL_CANDeviceType deviceType,
int32_t* status) {
hal::init::CheckInit();
if (busId < 0 || busId > hal::kNumCanBuses) {
*status = PARAMETER_OUT_OF_RANGE;
return HAL_kInvalidHandle;
}
auto can = std::make_shared<CANStorage>();
auto handle = canHandles->Allocate(can);
@@ -75,6 +73,7 @@ HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
return HAL_kInvalidHandle;
}
can->busId = busId;
can->deviceId = deviceId;
can->deviceType = deviceType;
can->manufacturer = manufacturer;
@@ -93,13 +92,16 @@ void HAL_CleanCAN(HAL_CANHandle handle) {
for (auto&& i : data->periodicSends) {
int32_t s = 0;
auto id = CreateCANId(data.get(), i.first);
HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, &s);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
HAL_CAN_SendMessage(data->busId, id, &message,
HAL_CAN_SEND_PERIOD_STOP_REPEATING, &s);
i.second = -1;
}
}
void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
int32_t length, int32_t apiId, int32_t* status) {
void HAL_WriteCANPacket(HAL_CANHandle handle, int32_t apiId,
const struct HAL_CANMessage* message, int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
@@ -108,12 +110,13 @@ void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
auto id = CreateCANId(can.get(), apiId);
std::scoped_lock lock(can->periodicSendsMutex);
HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
HAL_CAN_SendMessage(can->busId, id, message, HAL_CAN_SEND_PERIOD_NO_REPEAT,
status);
can->periodicSends[apiId] = -1;
}
void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
int32_t length, int32_t apiId,
void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
const struct HAL_CANMessage* message,
int32_t repeatMs, int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
@@ -123,11 +126,12 @@ void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
auto id = CreateCANId(can.get(), apiId);
std::scoped_lock lock(can->periodicSendsMutex);
HAL_CAN_SendMessage(id, data, length, repeatMs, status);
HAL_CAN_SendMessage(can->busId, id, message, repeatMs, status);
can->periodicSends[apiId] = repeatMs;
}
void HAL_WriteCANRTRFrame(HAL_CANHandle handle, int32_t length, int32_t apiId,
void HAL_WriteCANRTRFrame(HAL_CANHandle handle, int32_t apiId,
const struct HAL_CANMessage* message,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
@@ -136,11 +140,10 @@ void HAL_WriteCANRTRFrame(HAL_CANHandle handle, int32_t length, int32_t apiId,
}
auto id = CreateCANId(can.get(), apiId);
id |= HAL_CAN_IS_FRAME_REMOTE;
uint8_t data[8];
std::memset(data, 0, sizeof(data));
std::scoped_lock lock(can->periodicSendsMutex);
HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
HAL_CAN_SendMessage(can->busId, id, message, HAL_CAN_SEND_PERIOD_NO_REPEAT,
status);
can->periodicSends[apiId] = -1;
}
@@ -153,14 +156,17 @@ void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
}
auto id = CreateCANId(can.get(), apiId);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
std::scoped_lock lock(can->periodicSendsMutex);
HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING,
status);
HAL_CAN_SendMessage(can->busId, id, &message,
HAL_CAN_SEND_PERIOD_STOP_REPEATING, status);
can->periodicSends[apiId] = -1;
}
void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
int32_t* length, uint64_t* receivedTimestamp,
void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId,
struct HAL_CANReceiveMessage* message,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
@@ -169,24 +175,17 @@ void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
}
uint32_t messageId = CreateCANId(can.get(), apiId);
uint8_t dataSize = 0;
uint32_t ts = 0;
HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
HAL_CAN_ReceiveMessage(can->busId, messageId, message, status);
if (*status == 0) {
std::scoped_lock lock(can->receivesMutex);
auto& msg = can->receives[messageId];
msg.length = dataSize;
msg.lastTimeStamp = ts;
// The NetComm call placed in data, copy into the msg
std::memcpy(msg.data, data, dataSize);
can->receives[messageId] = *message;
}
*length = dataSize;
*receivedTimestamp = ts;
}
void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
int32_t* length, uint64_t* receivedTimestamp,
void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId,
struct HAL_CANReceiveMessage* message,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
@@ -195,36 +194,26 @@ void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
}
uint32_t messageId = CreateCANId(can.get(), apiId);
uint8_t dataSize = 0;
uint32_t ts = 0;
HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
HAL_CAN_ReceiveMessage(can->busId, messageId, message, status);
std::scoped_lock lock(can->receivesMutex);
if (*status == 0) {
// fresh update
auto& msg = can->receives[messageId];
msg.length = dataSize;
*length = dataSize;
msg.lastTimeStamp = ts;
*receivedTimestamp = ts;
// The NetComm call placed in data, copy into the msg
std::memcpy(msg.data, data, dataSize);
can->receives[messageId] = *message;
} else {
auto i = can->receives.find(messageId);
if (i != can->receives.end()) {
// Read the data from the stored message into the output
std::memcpy(data, i->second.data, i->second.length);
*length = i->second.length;
*receivedTimestamp = i->second.lastTimeStamp;
*message = i->second;
*status = 0;
}
}
}
void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
uint8_t* data, int32_t* length,
uint64_t* receivedTimestamp, int32_t timeoutMs,
int32_t* status) {
struct HAL_CANReceiveMessage* message,
int32_t timeoutMs, int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
@@ -232,34 +221,26 @@ void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
}
uint32_t messageId = CreateCANId(can.get(), apiId);
uint8_t dataSize = 0;
uint32_t ts = 0;
HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);
HAL_CAN_ReceiveMessage(can->busId, messageId, message, status);
std::scoped_lock lock(can->receivesMutex);
if (*status == 0) {
// fresh update
auto& msg = can->receives[messageId];
msg.length = dataSize;
*length = dataSize;
msg.lastTimeStamp = ts;
*receivedTimestamp = ts;
// The NetComm call placed in data, copy into the msg
std::memcpy(msg.data, data, dataSize);
can->receives[messageId] = *message;
} else {
auto i = can->receives.find(messageId);
if (i != can->receives.end()) {
// Found, check if new enough
uint32_t now = HAL_GetCANPacketBaseTime();
if (now - i->second.lastTimeStamp > static_cast<uint32_t>(timeoutMs)) {
uint64_t now = wpi::Now();
if (now - i->second.timeStamp >
(static_cast<uint64_t>(timeoutMs) * 1000)) {
// Timeout, return bad status
*status = HAL_CAN_TIMEOUT;
return;
}
// Read the data from the stored message into the output
std::memcpy(data, i->second.data, i->second.length);
*length = i->second.length;
*receivedTimestamp = i->second.lastTimeStamp;
*message = i->second;
*status = 0;
}
}
@@ -275,8 +256,8 @@ uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
uint32_t messageId = CreateCANId(can.get(), apiId);
uint32_t session = 0;
HAL_CAN_OpenStreamSession(&session, messageId, 0x1FFFFFFF, depth, status);
uint32_t session = HAL_CAN_OpenStreamSession(can->busId, messageId,
0x1FFFFFFF, depth, status);
return session;
}
} // extern "C"

58
hal/src/main/native/systemcore/CTREPCM.cpp
View File

@@ -145,33 +145,37 @@ void InitializeCTREPCM() {
}
} // namespace hal::init
#define READ_PACKET(type, frame, failureValue) \
auto pcm = pcmHandles->Get(handle); \
if (pcm == nullptr) { \
*status = HAL_HANDLE_ERROR; \
return failureValue; \
} \
type pcmStatus; \
int32_t length = 0; \
uint64_t receivedTimestamp = 0; \
HAL_ReadCANPacketTimeout(pcm->canHandle, frame, pcmStatus.data, &length, \
&receivedTimestamp, TimeoutMs, status); \
if (*status != 0) { \
return failureValue; \
}
#define READ_PACKET(type, frame, failureValue) \
auto pcm = pcmHandles->Get(handle); \
if (pcm == nullptr) { \
*status = HAL_HANDLE_ERROR; \
return failureValue; \
} \
HAL_CANReceiveMessage message; \
type pcmStatus; \
HAL_ReadCANPacketTimeout(pcm->canHandle, frame, &message, TimeoutMs, \
status); \
if (*status != 0) { \
return failureValue; \
} \
std::memcpy(pcmStatus.data, message.message.data, sizeof(pcmStatus.data));
#define READ_STATUS(failureValue) READ_PACKET(PcmStatus, Status1, failureValue)
#define READ_SOL_FAULTS(failureValue) \
READ_PACKET(PcmStatusFault, StatusSolFaults, failureValue)
static void SendControl(PCM* pcm, int32_t* status) {
HAL_WriteCANPacketRepeating(pcm->canHandle, pcm->control.data, 8, Control1,
SendPeriod, status);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = 8;
std::memcpy(message.data, pcm->control.data, 8);
HAL_WriteCANPacketRepeating(pcm->canHandle, Control1, &message, SendPeriod,
status);
}
extern "C" {
HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module,
HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t busId, int32_t module,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
@@ -190,7 +194,8 @@ HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module,
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
pcm->canHandle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
pcm->canHandle =
HAL_InitializeCAN(busId, manufacturer, module, deviceType, status);
if (*status != 0) {
pcmHandles->Free(handle);
return HAL_kInvalidHandle;
@@ -344,9 +349,11 @@ void HAL_ClearAllCTREPCMStickyFaults(HAL_CTREPCMHandle handle,
*status = HAL_HANDLE_ERROR;
return;
}
uint8_t controlData[] = {0, 0, 0, 0x80};
HAL_WriteCANPacket(pcm->canHandle, controlData, sizeof(controlData), Control2,
status);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = 4;
message.data[3] = 0x80;
HAL_WriteCANPacket(pcm->canHandle, Control2, &message, status);
}
void HAL_FireCTREPCMOneShot(HAL_CTREPCMHandle handle, int32_t index,
@@ -397,11 +404,16 @@ void HAL_SetCTREPCMOneShotDuration(HAL_CTREPCMHandle handle, int32_t index,
return;
}
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = 8;
std::scoped_lock lock{pcm->lock};
pcm->oneShot.sol10MsPerUnit[index] = (std::min)(
static_cast<uint32_t>(durMs) / 10, static_cast<uint32_t>(0xFF));
HAL_WriteCANPacketRepeating(pcm->canHandle, pcm->oneShot.sol10MsPerUnit, 8,
Control3, SendPeriod, status);
std::memcpy(message.data, pcm->oneShot.sol10MsPerUnit, 8);
HAL_WriteCANPacketRepeating(pcm->canHandle, Control3, &message, SendPeriod,
status);
}
} // extern "C"

113
hal/src/main/native/systemcore/CTREPDP.cpp
View File

@@ -129,7 +129,8 @@ void InitializeCTREPDP() {
extern "C" {
HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation,
HAL_PDPHandle HAL_InitializePDP(int32_t busId, int32_t module,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
if (!HAL_CheckPDPModule(module)) {
@@ -153,7 +154,8 @@ HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation,
return HAL_kInvalidHandle; // failed to allocate. Pass error back.
}
pdp->canHandle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
pdp->canHandle =
HAL_InitializeCAN(busId, manufacturer, module, deviceType, status);
if (*status != 0) {
pdpHandles->Free(handle);
return HAL_kInvalidHandle;
@@ -191,16 +193,16 @@ double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
return 0;
}
HAL_CANReceiveMessage message;
PdpStatus3 pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, &message, TimeoutMs,
status);
if (*status != 0) {
return 0;
} else {
std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
}
}
@@ -212,16 +214,16 @@ double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
return 0;
}
HAL_CANReceiveMessage message;
PdpStatus3 pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, &message, TimeoutMs,
status);
if (*status != 0) {
return 0;
} else {
std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
}
}
@@ -240,18 +242,18 @@ double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
return 0;
}
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_CANReceiveMessage message;
double raw = 0;
if (channel <= 5) {
PdpStatus1 pdpStatus;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, &message, TimeoutMs,
status);
if (*status != 0) {
return 0;
}
std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
switch (channel) {
case 0:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
@@ -280,11 +282,12 @@ double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
}
} else if (channel <= 11) {
PdpStatus2 pdpStatus;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, &message, TimeoutMs,
status);
if (*status != 0) {
return 0;
}
std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
switch (channel) {
case 6:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
@@ -313,11 +316,12 @@ double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
}
} else {
PdpStatus3 pdpStatus;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, &message, TimeoutMs,
status);
if (*status != 0) {
return 0;
}
std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
switch (channel) {
case 12:
raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
@@ -350,26 +354,28 @@ void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
return;
}
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_CANReceiveMessage message;
PdpStatus1 pdpStatus;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, pdpStatus.data, &length,
&receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, &message, TimeoutMs,
status);
if (*status != 0) {
return;
}
std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
PdpStatus2 pdpStatus2;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, pdpStatus2.data, &length,
&receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, &message, TimeoutMs,
status);
if (*status != 0) {
return;
}
std::memcpy(pdpStatus2.data, message.message.data, sizeof(pdpStatus2.data));
PdpStatus3 pdpStatus3;
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus3.data, &length,
&receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, &message, TimeoutMs,
status);
if (*status != 0) {
return;
}
std::memcpy(pdpStatus3.data, message.message.data, sizeof(pdpStatus3.data));
currents[0] = ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
pdpStatus.bits.chan1_l2) *
@@ -430,15 +436,15 @@ double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
return 0;
}
HAL_CANReceiveMessage message;
PdpStatusEnergy pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data,
&length, &receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, &message, TimeoutMs,
status);
if (*status != 0) {
return 0;
}
std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
uint32_t raw;
raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
@@ -454,15 +460,15 @@ double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
return 0;
}
HAL_CANReceiveMessage message;
PdpStatusEnergy pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data,
&length, &receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, &message, TimeoutMs,
status);
if (*status != 0) {
return 0;
}
std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
uint32_t raw;
raw = pdpStatus.bits.Power_125mWperunit_h4;
@@ -480,15 +486,15 @@ double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
return 0;
}
HAL_CANReceiveMessage message;
PdpStatusEnergy pdpStatus;
int32_t length = 0;
uint64_t receivedTimestamp = 0;
HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data,
&length, &receivedTimestamp, TimeoutMs, status);
HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, &message, TimeoutMs,
status);
if (*status != 0) {
return 0;
}
std::memcpy(pdpStatus.data, message.message.data, sizeof(pdpStatus.data));
uint32_t raw;
raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
@@ -514,8 +520,12 @@ void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
return;
}
uint8_t pdpControl[] = {0x40}; /* only bit set is ResetEnergy */
HAL_WriteCANPacket(pdp->canHandle, pdpControl, 1, Control1, status);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = 1;
message.data[0] = 0x40; /* only bit set is ResetEnergy */
HAL_WriteCANPacket(pdp->canHandle, Control1, &message, status);
}
void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
@@ -525,8 +535,12 @@ void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
return;
}
uint8_t pdpControl[] = {0x80}; /* only bit set is ClearStickyFaults */
HAL_WriteCANPacket(pdp->canHandle, pdpControl, 1, Control1, status);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = 1;
message.data[0] = 0x80; /* only bit set is ClearStickyFaults */
HAL_WriteCANPacket(pdp->canHandle, Control1, &message, status);
}
uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
@@ -594,8 +608,9 @@ HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle,
for (uint32_t i = 0; i < messagesRead; i++) {
PdpStatus1 pdpStatus;
std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data));
uint32_t timestamp = messages[i].timeStamp;
std::memcpy(pdpStatus.data, messages[i].message.message.data,
sizeof(pdpStatus));
uint64_t timestamp = messages[i].message.timeStamp;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
@@ -650,8 +665,9 @@ HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle,
for (uint32_t i = 0; i < messagesRead; i++) {
PdpStatus2 pdpStatus;
std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data));
uint32_t timestamp = messages[i].timeStamp;
std::memcpy(pdpStatus.data, messages[i].message.message.data,
sizeof(pdpStatus));
uint64_t timestamp = messages[i].message.timeStamp;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
@@ -706,8 +722,9 @@ HAL_PowerDistributionChannelData* HAL_GetPDPStreamData(HAL_PDPHandle handle,
for (uint32_t i = 0; i < messagesRead; i++) {
PdpStatus3 pdpStatus;
std::memcpy(pdpStatus.data, messages[i].data, sizeof(messages[i].data));
uint32_t timestamp = messages[i].timeStamp;
std::memcpy(pdpStatus.data, messages[i].message.message.data,
sizeof(pdpStatus));
uint64_t timestamp = messages[i].message.timeStamp;
retData[*count].current =
((static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |

4
hal/src/main/native/systemcore/CTREPDP.h
View File

@@ -23,10 +23,12 @@ extern "C" {
/**
* Initializes a Power Distribution Panel.
*
* @param busID the can bus id
* @param module the module number to initialize
* @return the created PDP
*/
HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation,
HAL_PDPHandle HAL_InitializePDP(int32_t busId, int32_t module,
const char* allocationLocation,
int32_t* status);
/**

3
hal/src/main/native/systemcore/Ports.cpp
View File

@@ -13,6 +13,9 @@ void InitializePorts() {}
} // namespace hal::init
extern "C" {
int32_t HAL_GetNumCanBuses(void) {
return kNumCanBuses;
}
int32_t HAL_GetNumAnalogInputs(void) {
return kNumAnalogInputs;
}

1
hal/src/main/native/systemcore/PortsInternal.h
View File

@@ -8,6 +8,7 @@
namespace hal {
constexpr int32_t kNumCanBuses = 2;
constexpr int32_t kNumSmartIo = 5;
constexpr int32_t kNumAccumulators = 0;
constexpr int32_t kNumAnalogInputs = 8;

10
hal/src/main/native/systemcore/PowerDistribution.cpp
View File

@@ -20,7 +20,7 @@ using namespace hal;
extern "C" {
HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
int32_t moduleNumber, HAL_PowerDistributionType type,
int32_t busId, int32_t moduleNumber, HAL_PowerDistributionType type,
const char* allocationLocation, int32_t* status) {
if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic) {
if (moduleNumber != HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
@@ -46,7 +46,7 @@ HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
} while (true);
// Try PDP first
auto pdpHandle = HAL_InitializePDP(0, allocationLocation, status);
auto pdpHandle = HAL_InitializePDP(busId, 0, allocationLocation, status);
if (pdpHandle != HAL_kInvalidHandle) {
*status = 0;
HAL_GetPDPVoltage(pdpHandle, status);
@@ -56,7 +56,7 @@ HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
HAL_CleanPDP(pdpHandle);
}
*status = 0;
auto pdhHandle = HAL_InitializeREVPDH(1, allocationLocation, status);
auto pdhHandle = HAL_InitializeREVPDH(busId, 1, allocationLocation, status);
return static_cast<HAL_PowerDistributionHandle>(pdhHandle);
}
@@ -65,13 +65,13 @@ HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
moduleNumber = 0;
}
return static_cast<HAL_PowerDistributionHandle>(
HAL_InitializePDP(moduleNumber, allocationLocation, status));
HAL_InitializePDP(busId, moduleNumber, allocationLocation, status));
} else {
if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
moduleNumber = 1;
}
return static_cast<HAL_PowerDistributionHandle>(
HAL_InitializeREVPDH(moduleNumber, allocationLocation, status));
HAL_InitializeREVPDH(busId, moduleNumber, allocationLocation, status));
}
}

108
hal/src/main/native/systemcore/REVPDH.cpp
View File

@@ -91,95 +91,85 @@ extern "C" {
static PDH_status_0_t HAL_ReadREVPDHStatus0(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
HAL_CANReceiveMessage message;
PDH_status_0_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_0_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus0Timeout * 2, status);
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_0_FRAME_API, &message,
kPDHFrameStatus0Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_0_unpack(&result, packedData, PDH_STATUS_0_LENGTH);
PDH_status_0_unpack(&result, message.message.data, PDH_STATUS_0_LENGTH);
return result;
}
static PDH_status_1_t HAL_ReadREVPDHStatus1(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
HAL_CANReceiveMessage message;
PDH_status_1_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_1_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus1Timeout * 2, status);
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_1_FRAME_API, &message,
kPDHFrameStatus1Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_1_unpack(&result, packedData, PDH_STATUS_1_LENGTH);
PDH_status_1_unpack(&result, message.message.data, PDH_STATUS_1_LENGTH);
return result;
}
static PDH_status_2_t HAL_ReadREVPDHStatus2(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
HAL_CANReceiveMessage message;
PDH_status_2_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_2_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus2Timeout * 2, status);
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_2_FRAME_API, &message,
kPDHFrameStatus2Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_2_unpack(&result, packedData, PDH_STATUS_2_LENGTH);
PDH_status_2_unpack(&result, message.message.data, PDH_STATUS_2_LENGTH);
return result;
}
static PDH_status_3_t HAL_ReadREVPDHStatus3(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
HAL_CANReceiveMessage message;
PDH_status_3_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_3_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus3Timeout * 2, status);
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_3_FRAME_API, &message,
kPDHFrameStatus3Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_3_unpack(&result, packedData, PDH_STATUS_3_LENGTH);
PDH_status_3_unpack(&result, message.message.data, PDH_STATUS_3_LENGTH);
return result;
}
static PDH_status_4_t HAL_ReadREVPDHStatus4(HAL_CANHandle hcan,
int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
HAL_CANReceiveMessage message;
PDH_status_4_t result = {};
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_4_FRAME_API, packedData, &length,
&timestamp, kPDHFrameStatus4Timeout * 2, status);
HAL_ReadCANPacketTimeout(hcan, PDH_STATUS_4_FRAME_API, &message,
kPDHFrameStatus4Timeout * 2, status);
if (*status != 0) {
return result;
}
PDH_status_4_unpack(&result, packedData, PDH_STATUS_4_LENGTH);
PDH_status_4_unpack(&result, message.message.data, PDH_STATUS_4_LENGTH);
return result;
}
@@ -199,7 +189,7 @@ PDH_status_4_t HAL_GetREVPDHStatus4(HAL_REVPDHHandle handle, int32_t* status) {
return statusFrame;
}
HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t module,
HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t busId, int32_t module,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
@@ -225,7 +215,7 @@ HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t module,
}
HAL_CANHandle hcan =
HAL_InitializeCAN(manufacturer, module, deviceType, status);
HAL_InitializeCAN(busId, manufacturer, module, deviceType, status);
if (*status != 0) {
REVPDHHandles->Free(handle);
@@ -454,14 +444,16 @@ void HAL_SetREVPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled,
return;
}
uint8_t packedData[8] = {0};
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
PDH_set_switch_channel_t frame;
frame.output_set_value = enabled;
PDH_set_switch_channel_pack(packedData, &frame,
PDH_set_switch_channel_pack(message.data, &frame,
PDH_SET_SWITCH_CHANNEL_LENGTH);
message.dataSize = PDH_SET_SWITCH_CHANNEL_LENGTH;
HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_SET_SWITCH_CHANNEL_LENGTH,
PDH_SET_SWITCH_CHANNEL_FRAME_API, status);
HAL_WriteCANPacket(hpdh->hcan, PDH_SET_SWITCH_CHANNEL_FRAME_API, &message,
status);
}
HAL_Bool HAL_GetREVPDHSwitchableChannelState(HAL_REVPDHHandle handle,
@@ -490,9 +482,6 @@ void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle,
HAL_PowerDistributionVersion* version,
int32_t* status) {
std::memset(version, 0, sizeof(*version));
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PDH_version_t result = {};
auto hpdh = REVPDHHandles->Get(handle);
if (hpdh == nullptr) {
@@ -512,17 +501,20 @@ void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle,
return;
}
HAL_WriteCANRTRFrame(hpdh->hcan, PDH_VERSION_LENGTH, PDH_VERSION_FRAME_API,
status);
HAL_CANMessage rtrmessage;
std::memset(&rtrmessage, 0, sizeof(rtrmessage));
rtrmessage.dataSize = PDH_VERSION_LENGTH;
HAL_WriteCANRTRFrame(hpdh->hcan, PDH_VERSION_FRAME_API, &rtrmessage, status);
if (*status != 0) {
return;
}
uint32_t timeoutMs = 100;
HAL_CANReceiveMessage message;
for (uint32_t i = 0; i <= timeoutMs; i++) {
HAL_ReadCANPacketNew(hpdh->hcan, PDH_VERSION_FRAME_API, packedData, &length,
&timestamp, status);
HAL_ReadCANPacketNew(hpdh->hcan, PDH_VERSION_FRAME_API, &message, status);
if (*status == 0) {
break;
}
@@ -533,7 +525,7 @@ void HAL_GetREVPDHVersion(HAL_REVPDHHandle handle,
return;
}
PDH_version_unpack(&result, packedData, PDH_VERSION_LENGTH);
PDH_version_unpack(&result, message.message.data, PDH_VERSION_LENGTH);
version->firmwareMajor = result.firmware_year;
version->firmwareMinor = result.firmware_minor;
@@ -640,9 +632,11 @@ void HAL_ClearREVPDHStickyFaults(HAL_REVPDHHandle handle, int32_t* status) {
return;
}
uint8_t packedData[8] = {0};
HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_CLEAR_FAULTS_LENGTH,
PDH_CLEAR_FAULTS_FRAME_API, status);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = PDH_CLEAR_FAULTS_LENGTH;
HAL_WriteCANPacket(hpdh->hcan, PDH_CLEAR_FAULTS_FRAME_API, &message, status);
}
uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
@@ -717,8 +711,9 @@ HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData(
for (uint32_t i = 0; i < messagesRead; i++) {
PDH_status_0_t statusFrame0;
PDH_status_0_unpack(&statusFrame0, messages[i].data, PDH_STATUS_0_LENGTH);
uint32_t timestamp = messages[i].timeStamp;
PDH_status_0_unpack(&statusFrame0, messages[i].message.message.data,
PDH_STATUS_0_LENGTH);
uint32_t timestamp = messages[i].message.timeStamp;
retData[*count].current =
PDH_status_0_channel_0_current_decode(statusFrame0.channel_0_current);
@@ -761,8 +756,9 @@ HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData(
for (uint32_t i = 0; i < messagesRead; i++) {
PDH_status_1_t statusFrame1;
PDH_status_1_unpack(&statusFrame1, messages[i].data, PDH_STATUS_1_LENGTH);
uint32_t timestamp = messages[i].timeStamp;
PDH_status_1_unpack(&statusFrame1, messages[i].message.message.data,
PDH_STATUS_1_LENGTH);
uint32_t timestamp = messages[i].message.timeStamp;
retData[*count].current =
PDH_status_1_channel_6_current_decode(statusFrame1.channel_6_current);
@@ -805,8 +801,9 @@ HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData(
for (uint32_t i = 0; i < messagesRead; i++) {
PDH_status_2_t statusFrame2;
PDH_status_2_unpack(&statusFrame2, messages[i].data, PDH_STATUS_2_LENGTH);
uint32_t timestamp = messages[i].timeStamp;
PDH_status_2_unpack(&statusFrame2, messages[i].message.message.data,
PDH_STATUS_2_LENGTH);
uint32_t timestamp = messages[i].message.timeStamp;
retData[*count].current =
PDH_status_2_channel_12_current_decode(statusFrame2.channel_12_current);
@@ -849,8 +846,9 @@ HAL_PowerDistributionChannelData* HAL_GetREVPDHStreamData(
for (uint32_t i = 0; i < messagesRead; i++) {
PDH_status_3_t statusFrame3;
PDH_status_3_unpack(&statusFrame3, messages[i].data, PDH_STATUS_3_LENGTH);
uint32_t timestamp = messages[i].timeStamp;
PDH_status_3_unpack(&statusFrame3, messages[i].message.message.data,
PDH_STATUS_3_LENGTH);
uint32_t timestamp = messages[i].message.timeStamp;
retData[*count].current =
PDH_status_3_channel_18_current_decode(statusFrame3.channel_18_current);

2
hal/src/main/native/systemcore/REVPDH.h
View File

@@ -25,7 +25,7 @@ extern "C" {
* @param module the device CAN ID (1 .. 63)
* @return the created PDH handle
*/
HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t module,
HAL_REVPDHHandle HAL_InitializeREVPDH(int32_t busId, int32_t module,
const char* allocationLocation,
int32_t* status);

79
hal/src/main/native/systemcore/REVPH.cpp
View File

@@ -84,37 +84,33 @@ void InitializeREVPH() {
} // namespace hal::init
static PH_status_0_t HAL_ReadREVPHStatus0(HAL_CANHandle hcan, int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
HAL_CANReceiveMessage message;
PH_status_0_t result = {};
HAL_ReadCANPacketTimeout(hcan, PH_STATUS_0_FRAME_API, packedData, &length,
&timestamp, kPHFrameStatus0Timeout * 2, status);
HAL_ReadCANPacketTimeout(hcan, PH_STATUS_0_FRAME_API, &message,
kPHFrameStatus0Timeout * 2, status);
if (*status != 0) {
return result;
}
PH_status_0_unpack(&result, packedData, PH_STATUS_0_LENGTH);
PH_status_0_unpack(&result, message.message.data, PH_STATUS_0_LENGTH);
return result;
}
static PH_status_1_t HAL_ReadREVPHStatus1(HAL_CANHandle hcan, int32_t* status) {
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
HAL_CANReceiveMessage message;
PH_status_1_t result = {};
HAL_ReadCANPacketTimeout(hcan, PH_STATUS_1_FRAME_API, packedData, &length,
&timestamp, kPHFrameStatus1Timeout * 2, status);
HAL_ReadCANPacketTimeout(hcan, PH_STATUS_1_FRAME_API, &message,
kPHFrameStatus1Timeout * 2, status);
if (*status != 0) {
return result;
}
PH_status_1_unpack(&result, packedData, PH_STATUS_1_LENGTH);
PH_status_1_unpack(&result, message.message.data, PH_STATUS_1_LENGTH);
return result;
}
@@ -181,17 +177,21 @@ static void HAL_UpdateDesiredREVPHSolenoidState(REV_PHObj* hph,
}
static void HAL_SendREVPHSolenoidsState(REV_PHObj* hph, int32_t* status) {
uint8_t packedData[PH_SET_ALL_LENGTH] = {0};
PH_set_all_pack(packedData, &(hph->desiredSolenoidsState), PH_SET_ALL_LENGTH);
HAL_WriteCANPacketRepeating(hph->hcan, packedData, PH_SET_ALL_LENGTH,
PH_SET_ALL_FRAME_API, hph->controlPeriod, status);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = PH_SET_ALL_LENGTH;
PH_set_all_pack(message.data, &(hph->desiredSolenoidsState),
PH_SET_ALL_LENGTH);
HAL_WriteCANPacketRepeating(hph->hcan, PH_SET_ALL_FRAME_API, &message,
hph->controlPeriod, status);
}
static HAL_Bool HAL_CheckREVPHPulseTime(int32_t time) {
return ((time > 0) && (time <= HAL_REVPH_MAX_PULSE_TIME)) ? 1 : 0;
}
HAL_REVPHHandle HAL_InitializeREVPH(int32_t module,
HAL_REVPHHandle HAL_InitializeREVPH(int32_t busId, int32_t module,
const char* allocationLocation,
int32_t* status) {
hal::init::CheckInit();
@@ -217,7 +217,7 @@ HAL_REVPHHandle HAL_InitializeREVPH(int32_t module,
}
HAL_CANHandle hcan =
HAL_InitializeCAN(manufacturer, module, deviceType, status);
HAL_InitializeCAN(busId, manufacturer, module, deviceType, status);
if (*status != 0) {
REVPHHandles->Free(handle);
@@ -291,11 +291,13 @@ void HAL_SetREVPHCompressorConfig(HAL_REVPHHandle handle,
frameData.force_disable = config->forceDisable;
frameData.use_digital = config->useDigital;
uint8_t packedData[PH_COMPRESSOR_CONFIG_LENGTH] = {0};
PH_compressor_config_pack(packedData, &frameData,
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = PH_COMPRESSOR_CONFIG_LENGTH;
PH_compressor_config_pack(message.data, &frameData,
PH_COMPRESSOR_CONFIG_LENGTH);
HAL_WriteCANPacket(ph->hcan, packedData, PH_COMPRESSOR_CONFIG_LENGTH,
PH_COMPRESSOR_CONFIG_API, status);
HAL_WriteCANPacket(ph->hcan, PH_COMPRESSOR_CONFIG_API, &message, status);
}
void HAL_SetREVPHClosedLoopControlDisabled(HAL_REVPHHandle handle,
@@ -480,9 +482,6 @@ double HAL_GetREVPHSolenoidVoltage(HAL_REVPHHandle handle, int32_t* status) {
void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version,
int32_t* status) {
std::memset(version, 0, sizeof(*version));
uint8_t packedData[8] = {0};
int32_t length = 0;
uint64_t timestamp = 0;
PH_version_t result = {};
auto ph = REVPHHandles->Get(handle);
if (ph == nullptr) {
@@ -502,17 +501,20 @@ void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version,
return;
}
HAL_WriteCANRTRFrame(ph->hcan, PH_VERSION_LENGTH, PH_VERSION_FRAME_API,
status);
HAL_CANMessage rtrmessage;
std::memset(&rtrmessage, 0, sizeof(rtrmessage));
rtrmessage.dataSize = PH_VERSION_LENGTH;
HAL_WriteCANRTRFrame(ph->hcan, PH_VERSION_FRAME_API, &rtrmessage, status);
if (*status != 0) {
return;
}
uint32_t timeoutMs = 100;
HAL_CANReceiveMessage message;
for (uint32_t i = 0; i <= timeoutMs; i++) {
HAL_ReadCANPacketNew(ph->hcan, PH_VERSION_FRAME_API, packedData, &length,
&timestamp, status);
HAL_ReadCANPacketNew(ph->hcan, PH_VERSION_FRAME_API, &message, status);
if (*status == 0) {
break;
}
@@ -523,7 +525,7 @@ void HAL_GetREVPHVersion(HAL_REVPHHandle handle, HAL_REVPHVersion* version,
return;
}
PH_version_unpack(&result, packedData, PH_VERSION_LENGTH);
PH_version_unpack(&result, message.message.data, PH_VERSION_LENGTH);
version->firmwareMajor = result.firmware_year;
version->firmwareMinor = result.firmware_minor;
@@ -683,10 +685,11 @@ void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs,
}
// Send pulse command
uint8_t packedData[PH_PULSE_ONCE_LENGTH] = {0};
PH_pulse_once_pack(packedData, &pulse, PH_PULSE_ONCE_LENGTH);
HAL_WriteCANPacket(ph->hcan, packedData, PH_PULSE_ONCE_LENGTH,
PH_PULSE_ONCE_FRAME_API, status);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = PH_PULSE_ONCE_LENGTH;
PH_pulse_once_pack(message.data, &pulse, PH_PULSE_ONCE_LENGTH);
HAL_WriteCANPacket(ph->hcan, PH_PULSE_ONCE_FRAME_API, &message, status);
}
void HAL_GetREVPHFaults(HAL_REVPHHandle handle, HAL_REVPHFaults* faults,
@@ -752,9 +755,11 @@ void HAL_ClearREVPHStickyFaults(HAL_REVPHHandle handle, int32_t* status) {
return;
}
uint8_t packedData[8] = {0};
HAL_WriteCANPacket(ph->hcan, packedData, PH_CLEAR_FAULTS_LENGTH,
PH_CLEAR_FAULTS_FRAME_API, status);
HAL_CANMessage message;
std::memset(&message, 0, sizeof(message));
message.dataSize = PH_CLEAR_FAULTS_LENGTH;
HAL_WriteCANPacket(ph->hcan, PH_CLEAR_FAULTS_FRAME_API, &message, status);
}
int32_t HAL_GetREVPHSolenoidDisabledList(HAL_REVPHHandle handle,