Upload files to "/"
This commit is contained in:
291
can_handler.cpp
Normal file
291
can_handler.cpp
Normal file
@@ -0,0 +1,291 @@
|
||||
// can_handler.cpp - CAN FD Handler Implementation
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "can_handler.h"
|
||||
#include "task_config.h"
|
||||
#include "rtc_manager.h"
|
||||
#include "psram_buffer.h"
|
||||
|
||||
ACAN2517FD canController(HSPI_CS_PIN, SPI, CAN_INT_PIN);
|
||||
|
||||
volatile bool canInitialized = false;
|
||||
volatile uint32_t canRxCount = 0;
|
||||
volatile uint32_t canTxCount = 0;
|
||||
volatile uint32_t canErrorCount = 0;
|
||||
volatile uint32_t canOverflowCount = 0;
|
||||
static uint8_t currentCANMode = 0;
|
||||
|
||||
static CANFDMessage rxMessage;
|
||||
static CANFDMessage txMessage;
|
||||
|
||||
bool initCAN() {
|
||||
Serial.println("Initializing CAN FD Controller (MCP2518FD)...");
|
||||
|
||||
SPI.begin(HSPI_SCLK_PIN, HSPI_MISO_PIN, HSPI_MOSI_PIN, HSPI_CS_PIN);
|
||||
|
||||
ACAN2517FDSettings settings(
|
||||
ACAN2517FDSettings::OSC_40MHz,
|
||||
CAN_DEFAULT_ARBITRATION_BAUDRATE,
|
||||
DataBitRateFactor::x4
|
||||
);
|
||||
|
||||
settings.mISOCRCEnabled = true;
|
||||
settings.mDriverReceiveFIFOSize = MCP2518FD_RX_FIFO_SIZE;
|
||||
settings.mDriverTransmitFIFOSize = MCP2518FD_TX_FIFO_SIZE;
|
||||
|
||||
switch (currentCANMode) {
|
||||
case 1:
|
||||
settings.mRequestedMode = ACAN2517FDSettings::ListenOnly;
|
||||
break;
|
||||
case 2:
|
||||
case 3:
|
||||
settings.mRequestedMode = ACAN2517FDSettings::InternalLoopBack;
|
||||
break;
|
||||
default:
|
||||
settings.mRequestedMode = ACAN2517FDSettings::NormalFD;
|
||||
break;
|
||||
}
|
||||
|
||||
const uint32_t errorCode = canController.begin(settings, [] {
|
||||
onCANInterrupt();
|
||||
});
|
||||
|
||||
if (errorCode == 0) {
|
||||
canInitialized = true;
|
||||
Serial.println("CAN FD initialized successfully!");
|
||||
Serial.printf(" Arbitration: %d bps\n", CAN_DEFAULT_ARBITRATION_BAUDRATE);
|
||||
Serial.printf(" Data: %d bps\n", CAN_DEFAULT_DATA_BAUDRATE);
|
||||
Serial.printf(" Mode: %d\n", currentCANMode);
|
||||
return true;
|
||||
} else {
|
||||
Serial.printf("CAN FD initialization failed! Error: 0x%X\n", errorCode);
|
||||
canInitialized = false;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool setCANMode(uint8_t mode) {
|
||||
if (mode > 3) return false;
|
||||
|
||||
currentCANMode = mode;
|
||||
|
||||
if (canInitialized) {
|
||||
canController.end();
|
||||
canInitialized = false;
|
||||
}
|
||||
|
||||
return initCAN();
|
||||
}
|
||||
|
||||
uint8_t getCANMode() {
|
||||
return currentCANMode;
|
||||
}
|
||||
|
||||
void IRAM_ATTR onCANInterrupt() {
|
||||
// Set flag or notify task - actual handling in task
|
||||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||
|
||||
// Notify CAN RX task
|
||||
if (canRxTaskHandle != NULL) {
|
||||
vTaskNotifyGiveFromISR(canRxTaskHandle, &xHigherPriorityTaskWoken);
|
||||
}
|
||||
|
||||
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||
}
|
||||
|
||||
void canRxTask(void *pvParameters) {
|
||||
Serial.println("CAN RX Task started on Core 0");
|
||||
|
||||
uint32_t framesProcessed = 0;
|
||||
uint32_t lastStatusTime = 0;
|
||||
|
||||
while (1) {
|
||||
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
|
||||
|
||||
uint8_t batchCount = 0;
|
||||
while (canController.available() && batchCount < CAN_RX_BATCH_SIZE) {
|
||||
if (canController.receive(rxMessage)) {
|
||||
canRxCount++;
|
||||
batchCount++;
|
||||
|
||||
CanFrame frame;
|
||||
frame.timestamp = getMicrosTimestamp();
|
||||
frame.id = rxMessage.id;
|
||||
frame.len = rxMessage.len;
|
||||
|
||||
frame.flags = 0;
|
||||
if (rxMessage.type == CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH) {
|
||||
frame.flags |= 0x01;
|
||||
}
|
||||
if (rxMessage.type == CANFDMessage::CANFD_NO_BIT_RATE_SWITCH) {
|
||||
frame.flags |= 0x02;
|
||||
}
|
||||
|
||||
memcpy(frame.data, rxMessage.data, rxMessage.len);
|
||||
|
||||
if (xQueueSend(canRxQueue, &frame, 0) != pdTRUE) {
|
||||
if (!canFrameBuffer.push(frame)) {
|
||||
canErrorCount++;
|
||||
} else {
|
||||
canOverflowCount++;
|
||||
}
|
||||
}
|
||||
|
||||
xQueueSend(graphQueue, &frame, 0);
|
||||
}
|
||||
}
|
||||
|
||||
framesProcessed += batchCount;
|
||||
|
||||
uint32_t now = millis();
|
||||
if (now - lastStatusTime > 5000) {
|
||||
Serial.printf("[CAN] RX: %d, Q: %d, PSRAM: %d/%d, Err: %d\n",
|
||||
canRxCount,
|
||||
uxQueueMessagesWaiting(canRxQueue),
|
||||
canFrameBuffer.available(),
|
||||
canFrameBuffer.capacity(),
|
||||
canErrorCount);
|
||||
lastStatusTime = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void canTxTask(void *pvParameters) {
|
||||
Serial.println("CAN TX Task started on Core 0");
|
||||
|
||||
CanTxRequest request;
|
||||
|
||||
while (1) {
|
||||
// Wait for transmit request
|
||||
if (xQueueReceive(canTxQueue, &request, portMAX_DELAY) == pdTRUE) {
|
||||
// Prepare message
|
||||
txMessage.id = request.id;
|
||||
txMessage.len = request.len;
|
||||
memcpy(txMessage.data, request.data, request.len);
|
||||
txMessage.type = CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH;
|
||||
|
||||
// Handle repeat count
|
||||
uint32_t repeat = request.repeat_count;
|
||||
if (repeat == 0) {
|
||||
repeat = 1; // Send at least once
|
||||
}
|
||||
|
||||
for (uint32_t i = 0; i < repeat; i++) {
|
||||
if (canController.tryToSend(txMessage)) {
|
||||
canTxCount++;
|
||||
} else {
|
||||
canErrorCount++;
|
||||
Serial.println("CAN TX failed: buffer full");
|
||||
}
|
||||
|
||||
// Delay between repeats
|
||||
if (request.delay_ms > 0 && i < repeat - 1) {
|
||||
vTaskDelay(pdMS_TO_TICKS(request.delay_ms));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool sendCANFrame(uint32_t id, uint8_t* data, uint8_t len, bool isFD) {
|
||||
if (!canInitialized) {
|
||||
return false;
|
||||
}
|
||||
|
||||
CanTxRequest request;
|
||||
request.id = id;
|
||||
request.len = len;
|
||||
memcpy(request.data, data, len);
|
||||
request.delay_ms = 0;
|
||||
request.repeat_count = 1;
|
||||
|
||||
return xQueueSend(canTxQueue, &request, pdMS_TO_TICKS(100)) == pdTRUE;
|
||||
}
|
||||
|
||||
bool setCANBaudrate(uint32_t arbitrationBaud, uint32_t dataBaud) {
|
||||
return setCANBaudrateAndMode(arbitrationBaud, dataBaud, currentCANMode, true);
|
||||
}
|
||||
|
||||
bool setCANBaudrateAndMode(uint32_t arbitrationBaud, uint32_t dataBaud, uint8_t mode, bool enableFD) {
|
||||
if (canInitialized) {
|
||||
canController.end();
|
||||
canInitialized = false;
|
||||
}
|
||||
|
||||
currentCANMode = mode;
|
||||
|
||||
DataBitRateFactor dataFactor = DataBitRateFactor::x1;
|
||||
|
||||
if (enableFD) {
|
||||
uint32_t factor = dataBaud / arbitrationBaud;
|
||||
switch (factor) {
|
||||
case 1: dataFactor = DataBitRateFactor::x1; break;
|
||||
case 2: dataFactor = DataBitRateFactor::x2; break;
|
||||
case 4: dataFactor = DataBitRateFactor::x4; break;
|
||||
case 5: dataFactor = DataBitRateFactor::x5; break;
|
||||
case 6: dataFactor = DataBitRateFactor::x6; break;
|
||||
case 7: dataFactor = DataBitRateFactor::x7; break;
|
||||
case 8: dataFactor = DataBitRateFactor::x8; break;
|
||||
case 10: dataFactor = DataBitRateFactor::x10; break;
|
||||
default: dataFactor = DataBitRateFactor::x4; break;
|
||||
}
|
||||
}
|
||||
|
||||
ACAN2517FDSettings settings(
|
||||
ACAN2517FDSettings::OSC_40MHz,
|
||||
arbitrationBaud,
|
||||
dataFactor
|
||||
);
|
||||
|
||||
settings.mISOCRCEnabled = enableFD;
|
||||
settings.mDriverReceiveFIFOSize = MCP2518FD_RX_FIFO_SIZE;
|
||||
settings.mDriverTransmitFIFOSize = MCP2518FD_TX_FIFO_SIZE;
|
||||
|
||||
if (enableFD) {
|
||||
switch (mode) {
|
||||
case 1:
|
||||
settings.mRequestedMode = ACAN2517FDSettings::ListenOnly;
|
||||
break;
|
||||
case 2:
|
||||
case 3:
|
||||
settings.mRequestedMode = ACAN2517FDSettings::InternalLoopBack;
|
||||
break;
|
||||
default:
|
||||
settings.mRequestedMode = ACAN2517FDSettings::NormalFD;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
switch (mode) {
|
||||
case 1:
|
||||
settings.mRequestedMode = ACAN2517FDSettings::ListenOnly;
|
||||
break;
|
||||
case 2:
|
||||
case 3:
|
||||
settings.mRequestedMode = ACAN2517FDSettings::InternalLoopBack;
|
||||
break;
|
||||
default:
|
||||
settings.mRequestedMode = ACAN2517FDSettings::Normal20B;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
const uint32_t errorCode = canController.begin(settings, [] {
|
||||
onCANInterrupt();
|
||||
});
|
||||
|
||||
canInitialized = (errorCode == 0);
|
||||
|
||||
if (canInitialized) {
|
||||
Serial.printf("CAN configured: Arb=%d, Data=%d, FD=%s, Mode=%d\n",
|
||||
arbitrationBaud, enableFD ? dataBaud : arbitrationBaud,
|
||||
enableFD ? "enabled" : "disabled", mode);
|
||||
}
|
||||
|
||||
return canInitialized;
|
||||
}
|
||||
|
||||
void getCANStats(uint32_t& rx, uint32_t& tx, uint32_t& errors) {
|
||||
rx = canRxCount;
|
||||
tx = canTxCount;
|
||||
errors = canErrorCount;
|
||||
}
|
||||
Reference in New Issue
Block a user