Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +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

47
hal/src/main/native/sim/CAN.cpp
View File

@@ -14,45 +14,50 @@ void InitializeCAN() {}
extern "C" {
void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
uint8_t dataSize, int32_t periodMs, int32_t* status) {
SimCanData->sendMessage(messageID, data, dataSize, periodMs, status);
void HAL_CAN_SendMessage(int32_t busId, uint32_t messageId,
const struct HAL_CANMessage* message, int32_t periodMs,
int32_t* status) {
SimCanData->sendMessage(busId, messageId, message, periodMs, status);
}
void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
uint8_t* data, uint8_t* dataSize,
uint32_t* timeStamp, int32_t* status) {
// Use a data size of 42 as call check. Difficult to add check to invoke
void HAL_CAN_ReceiveMessage(int32_t busId, uint32_t messageId,
struct HAL_CANReceiveMessage* message,
int32_t* status) {
// Use a data size of 100 as call check. Difficult to add check to invoke
// handler
*dataSize = 42;
message->message.dataSize = 100;
auto tmpStatus = *status;
SimCanData->receiveMessage(messageID, messageIDMask, data, dataSize,
timeStamp, status);
SimCanData->receiveMessage(busId, messageId, message, status);
// If no handler invoked, return message not found
if (*dataSize == 42 && *status == tmpStatus) {
if (message->message.dataSize == 100 && *status == tmpStatus) {
*status = HAL_ERR_CANSessionMux_MessageNotFound;
}
}
void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
uint32_t messageIDMask, uint32_t maxMessages,
int32_t* status) {
SimCanData->openStreamSession(sessionHandle, messageID, messageIDMask,
HAL_CANStreamHandle HAL_CAN_OpenStreamSession(int32_t busId, uint32_t messageId,
uint32_t messageIDMask,
uint32_t maxMessages,
int32_t* status) {
HAL_CANStreamHandle handle = 0;
SimCanData->openStreamSession(&handle, busId, messageId, messageIDMask,
maxMessages, status);
return handle;
}
void HAL_CAN_CloseStreamSession(uint32_t sessionHandle) {
void HAL_CAN_CloseStreamSession(HAL_CANStreamHandle sessionHandle) {
SimCanData->closeStreamSession(sessionHandle);
}
void HAL_CAN_ReadStreamSession(uint32_t sessionHandle,
void HAL_CAN_ReadStreamSession(HAL_CANStreamHandle sessionHandle,
struct HAL_CANStreamMessage* messages,
uint32_t messagesToRead, uint32_t* messagesRead,
int32_t* status) {
SimCanData->readStreamSession(sessionHandle, messages, messagesToRead,
messagesRead, status);
}
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) {
SimCanData->getCANStatus(percentBusUtilization, busOffCount, txFullCount,
receiveErrorCount, transmitErrorCount, status);
SimCanData->getCANStatus(busId, percentBusUtilization, busOffCount,
txFullCount, receiveErrorCount, transmitErrorCount,
status);
}
} // extern "C"

146
hal/src/main/native/sim/CANAPI.cpp
View File

@@ -4,34 +4,31 @@
#include "hal/CANAPI.h"
#include <ctime>
#include <memory>
#include <wpi/DenseMap.h>
#include <wpi/mutex.h>
#include <wpi/timestamp.h>
#include "CANAPIInternal.h"
#include "HALInitializer.h"
#include "PortsInternal.h"
#include "hal/CAN.h"
#include "hal/Errors.h"
#include "hal/HALBase.h"
#include "hal/handles/UnlimitedHandleResource.h"
using namespace hal;
namespace {
struct Receives {
uint64_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
@@ -66,18 +63,19 @@ static int32_t CreateCANId(CANStorage* storage, int32_t apiId) {
createdId |= (storage->deviceId & 0x3F);
return createdId;
}
extern "C" {
uint32_t HAL_GetCANPacketBaseTime(void) {
int status = 0;
auto basetime = HAL_GetFPGATime(&status);
// us to ms
return (basetime / 1000ull) & 0xFFFFFFFF;
}
HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
extern "C" {
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);
@@ -87,6 +85,7 @@ HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
return HAL_kInvalidHandle;
}
can->busId = busId;
can->deviceId = deviceId;
can->deviceType = deviceType;
can->manufacturer = manufacturer;
@@ -105,13 +104,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;
@@ -120,12 +122,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) {
@@ -135,11 +138,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) {
@@ -148,11 +152,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;
}
@@ -165,14 +168,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) {
@@ -181,24 +187,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) {
@@ -207,36 +206,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;
@@ -244,36 +233,43 @@ 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;
}
}
}
uint32_t HAL_StartCANStream(HAL_CANHandle handle, int32_t apiId, int32_t depth,
int32_t* status) {
auto can = canHandles->Get(handle);
if (!can) {
*status = HAL_HANDLE_ERROR;
return 0;
}
uint32_t messageId = CreateCANId(can.get(), apiId);
uint32_t session = HAL_CAN_OpenStreamSession(can->busId, messageId,
0x1FFFFFFF, depth, status);
return session;
}
} // extern "C"

2
hal/src/main/native/sim/CTREPCM.cpp
View File

@@ -35,7 +35,7 @@ void InitializeCTREPCM() {
}
} // namespace hal::init
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();

3
hal/src/main/native/sim/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/sim/PortsInternal.h
View File

@@ -7,6 +7,7 @@
#include <stdint.h>
namespace hal {
constexpr int32_t kNumCanBuses = 2;
constexpr int32_t kAccelerometers = 1;
constexpr int32_t kNumAccumulators = 2;
constexpr int32_t kNumAnalogInputs = 8;

5
hal/src/main/native/sim/PowerDistribution.cpp
View File

@@ -28,7 +28,7 @@ void InitializePowerDistribution() {}
extern "C" {
HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
int32_t module, HAL_PowerDistributionType type,
int32_t busId, int32_t module, HAL_PowerDistributionType type,
const char* allocationLocation, int32_t* status) {
if (type == HAL_PowerDistributionType_kAutomatic) {
if (module != HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
@@ -56,7 +56,8 @@ HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
}
hal::init::CheckInit();
SimPowerDistributionData[module].initialized = true;
auto handle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
auto handle =
HAL_InitializeCAN(busId, manufacturer, module, deviceType, status);
if (*status != 0) {
HAL_CleanCAN(handle);

2
hal/src/main/native/sim/REVPH.cpp
View File

@@ -35,7 +35,7 @@ void InitializeREVPH() {
}
} // namespace hal::init
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();