Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +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

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"