From 534be573e6c602498cf5c87fdfb0d02381aa6735 Mon Sep 17 00:00:00 2001 From: Timo Date: Sun, 31 Aug 2025 12:01:55 +0200 Subject: [PATCH] added internal can header to remove padding from msgs --- boards/nucleo_h755zi_q_stm32h755xx_m7.overlay | 2 +- prj.conf | 4 +- src/bridges/bridge_usb_cdc_acm_canfd.cpp | 119 +++++++++--------- src/canfd/can_fd.cpp | 2 +- src/devices/devices.cpp | 1 + src/devices/devices.hpp | 1 + 6 files changed, 68 insertions(+), 61 deletions(-) diff --git a/boards/nucleo_h755zi_q_stm32h755xx_m7.overlay b/boards/nucleo_h755zi_q_stm32h755xx_m7.overlay index 1455e38..2ddc730 100644 --- a/boards/nucleo_h755zi_q_stm32h755xx_m7.overlay +++ b/boards/nucleo_h755zi_q_stm32h755xx_m7.overlay @@ -27,7 +27,7 @@ }; &fdcan1 { - bitrate = <1000000>; + bitrate = <500000>; bitrate-data = <4000000>; status = "okay"; }; \ No newline at end of file diff --git a/prj.conf b/prj.conf index fc05873..2266d58 100644 --- a/prj.conf +++ b/prj.conf @@ -19,11 +19,9 @@ CONFIG_DEBUG_THREAD_INFO=y CONFIG_CONSOLE=y # logging -CONFIG_LOG=y -CONFIG_LOG_MODE_IMMEDIATE=y +CONFIG_LOG=n CONFIG_STDOUT_CONSOLE=y CONFIG_PRINTK=y -CONFIG_LOG_DEFAULT_LEVEL=3 # Enable CDC ACM (virtual COM port) CONFIG_SERIAL=y diff --git a/src/bridges/bridge_usb_cdc_acm_canfd.cpp b/src/bridges/bridge_usb_cdc_acm_canfd.cpp index 687fc9a..76f2253 100644 --- a/src/bridges/bridge_usb_cdc_acm_canfd.cpp +++ b/src/bridges/bridge_usb_cdc_acm_canfd.cpp @@ -13,16 +13,19 @@ K_FIFO_DEFINE(usb_fifo_2); K_FIFO_DEFINE(usb_fifo_3); struct uart_msg { + uint16_t len; + uint8_t data[62]; +}; + +struct uart_msg_queue { void *fifo_reserved; - uint8_t len; - uint8_t *data; + uart_msg msg; }; uint8_t get_can_id_from_usb_dev(const struct device *dev) { if (dev == cdc0) { return 0x10; } - if (dev == cdc1) { return 0x20; } @@ -42,7 +45,6 @@ struct k_fifo *get_fifo_from_dev(const struct device *dev) { if (dev == cdc0) { return &usb_fifo_0; } - if (dev == cdc1) { return &usb_fifo_1; } @@ -60,7 +62,7 @@ struct k_fifo *get_fifo_from_dev(const struct device *dev) { const struct device *get_usb_dev_from_can_id(uint8_t can_id) { switch (can_id) { - case 0x10: + case 0x11: return cdc0; case 0x21: return cdc1; @@ -73,62 +75,66 @@ const struct device *get_usb_dev_from_can_id(uint8_t can_id) { } } -uint8_t get_lenght_from_dlc(uint8_t dlc) { - static const uint8_t dlc_to_len[16] = { - 0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64 - }; - - if (dlc < 16) { - return dlc_to_len[dlc]; - } else { - return 0; - } -} - static void uart_rx_handler(const struct device *dev, void *user_data) { ARG_UNUSED(user_data); while (uart_irq_update(dev) && uart_irq_is_pending(dev)) { - printk("tx: %i, rx: %i\r\n", uart_irq_tx_ready(dev), uart_irq_rx_ready(dev)); + //printk("tx: %i, rx: %i\r\n", uart_irq_tx_ready(dev), uart_irq_rx_ready(dev)); if (uart_irq_rx_ready(dev)) { - uint8_t buffer[64] = {0}; + uart_msg msg; - int recv_len = uart_fifo_read(dev, buffer, 64); - if (recv_len > 0) { - struct can_frame counter_frame = { + msg.len = uart_fifo_read(dev, msg.data, 62); + if (msg.len > 0) { + struct can_frame frame = { .id = get_can_id_from_usb_dev(dev), - .dlc = can_bytes_to_dlc(recv_len), + .dlc = can_bytes_to_dlc(msg.len+2), .flags = CAN_FRAME_FDF | CAN_FRAME_BRS, .data = {0}, }; - memcpy(counter_frame.data, buffer, recv_len); + + memcpy(frame.data, &msg, msg.len+2); - can_send(fdcan1, &counter_frame, K_NO_WAIT, NULL, NULL); - } + can_send(fdcan1, &frame, K_NO_WAIT, NULL, NULL); + + printk("CAN TX: "); + for (size_t i = 0; i < msg.len; i++) { + printk("%02X ", msg.data[i]); + } + printk(" / %i\n", msg.len); + } } - if (uart_irq_tx_ready(dev) >= 64) { + if (uart_irq_tx_ready(dev)) { k_fifo* fifo = get_fifo_from_dev(dev); if (fifo == NULL) { uart_irq_tx_disable(dev); return; } - struct uart_msg *msg = (uart_msg*) k_fifo_get(fifo, K_MSEC(10)); - if(msg){ - printk("sending to UART: %s / %i\n\r", msg->data, msg->len); - uart_fifo_fill(dev, msg->data, msg->len); - k_free(msg->data); - k_free(msg); - } - else { - printk("Nothing to do\n\r"); - uart_irq_tx_disable(dev); + struct uart_msg_queue *msg_q = NULL; + while (1) { + msg_q = (uart_msg_queue*) k_fifo_get(fifo, K_MSEC(10)); + if (msg_q == NULL) { + uart_irq_tx_disable(dev); + printk("Processed all messages\n\r"); + return; + } + if (uart_irq_tx_ready(dev) > msg_q->msg.len) { + printk("USB TX: "); + for (size_t i = 0; i < msg_q->msg.len; i++) { + printk("%02X ", msg_q->msg.data[i]); + } + printk(" / %i\n", msg_q->msg.len); + + uart_fifo_fill(dev, msg_q->msg.data, msg_q->msg.len); + } + k_free(msg_q); } } - } + } } + void can_rx_handler(const struct device *dev, struct can_frame *frame, void *user_data) { ARG_UNUSED(dev); @@ -137,20 +143,17 @@ void can_rx_handler(const struct device *dev, struct can_frame *frame, void *use const struct device *uart_dev = get_usb_dev_from_can_id(frame->id); if (!uart_dev) return; - struct uart_msg *msg = (uart_msg*)k_malloc(sizeof(uart_msg)); - if (!msg) return; + struct uart_msg_queue *msg_q = (uart_msg_queue*)k_malloc(sizeof(uart_msg_queue)); + if (!msg_q) return; - msg->len = get_lenght_from_dlc(frame->dlc); - printk("msg size: %i / %i\n\r", msg->len, frame->dlc); + if (frame->dlc < 2) { + k_free(msg_q); + return; + }; - msg->data = (uint8_t*)k_malloc(sizeof(uint8_t) * msg->len); - if (!msg->data) return; - - memset(msg->data, 0, msg->len); - memcpy(msg->data, frame->data, msg->len); - - printk("sending to Queue: %s / %s / %i / %i\n\r", frame->data, msg->data, msg->len, frame->dlc); - k_fifo_put(get_fifo_from_dev(uart_dev), msg); + memcpy(&msg_q->msg, frame->data, sizeof(uart_msg)); + + k_fifo_put(get_fifo_from_dev(uart_dev), msg_q); uart_irq_tx_enable(uart_dev); } @@ -164,12 +167,16 @@ void bridges_init() { uart_irq_rx_enable(cdc2); uart_irq_callback_set(cdc3, uart_rx_handler); uart_irq_rx_enable(cdc3); + - struct can_filter filter = { - .id = 0, // match all IDs - .mask = 0, // accept everything - .flags = 0, - }; + struct can_filter filters[4] = { + { .id = 0x11, .mask = 0, .flags = 0 }, + { .id = 0x21, .mask = 0, .flags = 0 }, + { .id = 0x31, .mask = 0, .flags = 0 }, + { .id = 0x41, .mask = 0, .flags = 0 }, +}; - can_add_rx_filter(fdcan1, can_rx_handler, NULL, &filter); + for (int i = 0; i < 4; i++) { + can_add_rx_filter(fdcan1, can_rx_handler, NULL, &filters[i]); + } } \ No newline at end of file diff --git a/src/canfd/can_fd.cpp b/src/canfd/can_fd.cpp index 25d4346..6960c75 100644 --- a/src/canfd/can_fd.cpp +++ b/src/canfd/can_fd.cpp @@ -5,7 +5,7 @@ void canfd_init() { - if (!can_set_mode(fdcan1, CAN_MODE_LOOPBACK | CAN_MODE_FD)) { + if (!can_set_mode(fdcan1, CAN_MODE_FD)) { printk("CAN0 set to CAN FD\n"); } diff --git a/src/devices/devices.cpp b/src/devices/devices.cpp index ac86ed4..7aec777 100644 --- a/src/devices/devices.cpp +++ b/src/devices/devices.cpp @@ -5,6 +5,7 @@ const struct device *cdc1 = DEVICE_DT_GET(DT_NODELABEL(cdc_acm_uart1)); const struct device *cdc2 = DEVICE_DT_GET(DT_NODELABEL(cdc_acm_uart2)); const struct device *cdc3 = DEVICE_DT_GET(DT_NODELABEL(cdc_acm_uart3)); + const struct device *fdcan1 = DEVICE_DT_GET(DT_NODELABEL(fdcan1)); diff --git a/src/devices/devices.hpp b/src/devices/devices.hpp index d23558d..3a4761e 100644 --- a/src/devices/devices.hpp +++ b/src/devices/devices.hpp @@ -9,6 +9,7 @@ extern const struct device *cdc1; extern const struct device *cdc2; extern const struct device *cdc3; + extern const struct device *fdcan1;