added micro ros tansport canfd

boards/nucleo_h563zi.overlay

@@ -40,3 +40,10 @@
         pinctrl-names = "default";
     };
 };
+
+&fdcan1 {
+    pinctrl-0 = <&fdcan1_rx_pd0 &fdcan1_tx_pd1>;
+    bitrate = <500000>;
+    bitrate-data = <4000000>;
+    status = "okay";
+};

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_

prj.conf

@@ -8,10 +8,9 @@ CONFIG_MINIMAL_LIBC=n
 CONFIG_POSIX_API=y
 
 # logging
-CONFIG_LOG=y
+CONFIG_LOG=n
 CONFIG_STDOUT_CONSOLE=y
 CONFIG_PRINTK=y
-CONFIG_LOG_DEFAULT_LEVEL=3
 
 # gpio support
 CONFIG_GPIO=y
@@ -26,4 +25,9 @@ CONFIG_PWM_STM32=y
 
 # micro ros
 CONFIG_MICROROS=y
-CONFIG_MICROROS_TRANSPORT_SERIAL=y
+CONFIG_MICROROS_TRANSPORT_FDCAN=y
+
+# can FD
+CONFIG_CAN=y
+CONFIG_CAN_FD_MODE=y
+CONFIG_CAN_STM32_FDCAN=y

src/devices.cpp

@@ -1,5 +1,7 @@
 #include "devices.hpp"
 
+const struct device *fdcan1 = DEVICE_DT_GET(DT_NODELABEL(fdcan1));
+
 bool verify_are_devices_available(void) {
     bool devices_ready = true;
 
@@ -22,6 +24,10 @@ bool verify_are_devices_available(void) {
         printk("Servo0 not ready\n");
         devices_ready = false;
     }
+    if (!device_is_ready(fdcan1)) {
+        printk("CAN FD 1 not ready\n");
+        devices_ready = false;
+    }
 
     return devices_ready;
 }

src/devices.hpp

@@ -19,4 +19,6 @@ static const struct gpio_dt_spec led2 = GPIO_DT_SPEC_GET(DT_ALIAS(led2), gpios);
 
 static const struct pwm_dt_spec servo0 = PWM_DT_SPEC_GET(DT_ALIAS(servo0));
 
+extern const struct device *fdcan1;
+
 bool verify_are_devices_available(void);

src/main.cpp

@@ -36,8 +36,11 @@ int main(void)
 
     MicrorosExecutor node0 = MicrorosExecutor("nucleo_node0");
 
+    printk("Connecting to Agent....\n");
+
     while (!node0.connect()) {
         printk("Waiting for connection ...\n");
+        k_msleep(1000);
     };
 
     printk("Connection to Agent node established!\n");

src/microros/microros_node.cpp

@@ -35,14 +35,33 @@ MicrorosExecutor::~MicrorosExecutor() {
 }
 
 bool MicrorosExecutor::connect() {
-    if (rclc_support_init(&this->support, 0, NULL, &this->allocator) != RCL_RET_OK)
-        return false;
+    static bool support_init = false;
+    static bool defaults_init = false;
+    static bool executor_init = false;
 
-    if (rclc_node_init_default(&this->node, this->node_name, "", &this->support) != RCL_RET_OK)
-        return false;
+    if (!support_init){
+        if (rclc_support_init(&this->support, 0, NULL, &this->allocator) != RCL_RET_OK) {
+            printk("Can't init support!\r\n");
+            return false;
+        }
+        support_init = true;
+    }
         
-    if (rclc_executor_init(&this->executor, &this->support.context, 32, &this->allocator) != RCL_RET_OK)
-        return false;
+    if (!defaults_init){
+        if (rclc_node_init_default(&this->node, this->node_name, "", &this->support) != RCL_RET_OK){
+            printk("Can't init defaults!\r\n");
+            return false;
+        }
+        defaults_init = true;
+    }
+
+    if (!executor_init){
+        if (rclc_executor_init(&this->executor, &this->support.context, 32, &this->allocator) != RCL_RET_OK){
+            printk("Can't init executor!\r\n");
+            return false;
+        }
+        executor_init = true;
+    }
 
     return true;
 }