added micro ros tansport canfd

modules/libmicroros/CMakeLists.txt

@@ -74,8 +74,9 @@ foreach(pkg ${INCLUDE_ROS2_PACKAGES})
 endforeach()
 
 # micro-ROS transport library
-
-if(CONFIG_MICROROS_TRANSPORT_SERIAL)
+if(CONFIG_MICROROS_TRANSPORT_FDCAN)
+    set(MICROROS_TRANSPORT_DIR ${MICROROS_DIR}/microros_transports/fdcan)
+elseif(CONFIG_MICROROS_TRANSPORT_SERIAL)
     set(MICROROS_TRANSPORT_DIR ${MICROROS_DIR}/microros_transports/serial)
 elseif(CONFIG_MICROROS_TRANSPORT_SERIAL_USB)
     set(MICROROS_TRANSPORT_DIR ${MICROROS_DIR}/microros_transports/serial-usb)

modules/libmicroros/Kconfig

@@ -16,6 +16,9 @@ if MICROROS
     choice
         prompt "micro-ROS transport"
         
+        config MICROROS_TRANSPORT_FDCAN
+            bool "micro-ROS fdcab transport"
+            select RING_BUFFER
         config MICROROS_TRANSPORT_SERIAL
             bool "micro-ROS serial transport"
             select RING_BUFFER
@@ -103,7 +106,7 @@ if MICROROS
     
     config MICROROS_XRCE_DDS_MTU
         string "micro-ROS transport MTU"
-        default "512"
+        default "62"
         
     config MICROROS_XRCE_DDS_HISTORIC
         string "micro-ROS middleware memory slots"

modules/libmicroros/microros_transports/fdcan/microros_transports.c

@@ -0,0 +1,138 @@
+#include <uxr/client/transport.h>
+
+#include <microros_transports.h>
+#include <version.h>
+
+#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
+#include <zephyr/kernel.h>
+#include <zephyr/device.h>
+#include <zephyr/sys/printk.h>
+#include <zephyr/drivers/can.h>
+#include <zephyr/sys/ring_buffer.h>
+#include <zephyr/posix/unistd.h>
+#else
+#include <zephyr.h>
+#include <device.h>
+#include <sys/printk.h>
+#include <drivers/can.h>
+#include <sys/ring_buffer.h>
+#include <posix/unistd.h>
+#endif
+
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+
+#define RING_BUF_SIZE 2048
+#define CAN_NODE DT_NODELABEL(fdcan1)
+
+char can_in_buffer[RING_BUF_SIZE];
+char can_out_buffer[RING_BUF_SIZE];
+
+struct ring_buf out_ringbuf, in_ringbuf;
+
+typedef struct uart_msg {
+    uint16_t len;
+    uint8_t data[62];
+} uart_msg;
+
+// --- micro-ROS FDCAN Transport for Zephyr ---
+
+static void can_rx_handler(const struct device *dev, struct can_frame *frame, void *user_data){
+    ARG_UNUSED(dev);
+    ARG_UNUSED(user_data);
+
+    if (frame->dlc < 2) {
+        return;
+    }
+
+    uart_msg *msg = (uart_msg*) frame->data;
+    
+    ring_buf_put(&in_ringbuf, msg->data, MIN(ring_buf_space_get(&in_ringbuf), msg->len));
+
+    printf("RX: ");
+    for (size_t i = 0; i < msg->len; i++) {
+        printf("%02X ", msg->data[i]);
+    }
+    printf(" / %i\n", msg->len);
+}
+
+bool zephyr_transport_open(struct uxrCustomTransport * transport){
+    zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args;
+
+    params->uart_dev = DEVICE_DT_GET(CAN_NODE);
+    if (!params->uart_dev) {
+        printk("FDCAN device not found\n");
+        return false;
+    }
+
+    if (!device_is_ready(params->uart_dev)) {
+        printk("FDCAN device not ready\n");
+        return false;
+    }
+
+    printk("FDCAN device connected\n");
+
+    ring_buf_init(&in_ringbuf, sizeof(can_in_buffer), can_in_buffer);
+
+    can_set_mode(params->uart_dev, CAN_MODE_FD);
+    can_start(params->uart_dev);
+
+    /* Enable rx filter */
+    struct can_filter filter = {
+        .id = 0x10,
+        .mask = 0,
+        .flags = 0,
+    };
+
+    can_add_rx_filter(params->uart_dev, can_rx_handler, NULL, &filter);
+
+    return true;
+}
+
+bool zephyr_transport_close(struct uxrCustomTransport * transport){
+    (void) transport;
+    // TODO: close serial transport here
+    return true;
+}
+
+size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err){
+    zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args;   
+
+    struct can_frame frame = {
+        .id = 0x11,
+        .dlc = can_bytes_to_dlc(len+2),
+        .flags = CAN_FRAME_FDF | CAN_FRAME_BRS,
+        .data = {0},
+    };
+
+    uart_msg *msg = (uart_msg*) frame.data;
+    msg->len = len;
+    memcpy(msg->data, buf, len);
+    
+    can_send(params->uart_dev, &frame, K_NO_WAIT, NULL, NULL);
+
+    printf("TX: ");
+    for (size_t i = 0; i < len; i++) {
+        printf("%02X ", msg->data[i]);
+    }
+    printf(" / %i\n", msg->len);
+
+    return len;
+}
+
+size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err){
+    zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args;
+
+    size_t read = 0;
+    int spent_time = 0;
+
+    while(ring_buf_is_empty(&in_ringbuf) && spent_time < timeout){
+        usleep(1000);
+        spent_time++;
+    }
+
+    read = ring_buf_get(&in_ringbuf, buf, len);
+
+    return read;
+}

modules/libmicroros/microros_transports/fdcan/microros_transports.h

@@ -0,0 +1,49 @@
+// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef _MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_
+#define _MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_
+
+#include <unistd.h>
+#include <version.h>
+
+#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
+#include <zephyr/device.h>
+#else
+#include <device.h>
+#endif
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+typedef struct {
+    size_t fd;
+    const struct device * uart_dev;
+} zephyr_transport_params_t;
+
+#define MICRO_ROS_FRAMING_REQUIRED true
+volatile static zephyr_transport_params_t default_params = {.fd = 1};
+
+bool zephyr_transport_open(struct uxrCustomTransport * transport);
+bool zephyr_transport_close(struct uxrCustomTransport * transport);
+size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
+size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //_MICROROS_CLIENT_ZEPHYR_TRANSPORT_H_