Support for CAN FD transport for Zephyr 3.5.0, 3.6.0, and 3.7.0

modules/libmicroros/CMakeLists.txt

@@ -82,6 +82,8 @@ elseif(CONFIG_MICROROS_TRANSPORT_SERIAL_USB)
     set(MICROROS_TRANSPORT_DIR ${MICROROS_DIR}/microros_transports/serial-usb)
 elseif(CONFIG_MICROROS_TRANSPORT_UDP)
     set(MICROROS_TRANSPORT_DIR ${MICROROS_DIR}/microros_transports/udp)
+elseif(CONFIG_MICROROS_TRANSPORT_CAN_FD)
+    set(MICROROS_TRANSPORT_DIR ${MICROROS_DIR}/microros_transports/canfd)
 else()
     message(FATAL_ERROR Please set a micro-ROS transport)
 endif()

modules/libmicroros/Kconfig

@@ -25,6 +25,8 @@ if MICROROS
              select USB_DEVICE_STACK
         config MICROROS_TRANSPORT_UDP
             bool "micro-ROS UDP network transport"
+        config MICROROS_TRANSPORT_CAN_FD
+            bool "micro-ROS CAN-FD transport"
 
     endchoice
 
@@ -103,11 +105,13 @@ if MICROROS
 
     config MICROROS_XRCE_DDS_MTU
         string "micro-ROS transport MTU"
+        default "64" if MICROROS_TRANSPORT_CAN_FD
         default "512"
-        
+
     config MICROROS_XRCE_DDS_HISTORIC
         string "micro-ROS middleware memory slots"
-        default "4"  
+        default "16" if MICROROS_TRANSPORT_CAN_FD
+        default "4"
 
 endif
 

modules/libmicroros/microros_transports/canfd/microros_transports.c

@@ -0,0 +1,137 @@
+#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/posix/unistd.h>
+#else
+    #include <zephyr.h>
+    #include <device.h>
+    #include <sys/printk.h>
+    #include <drivers/can.h>
+    #include <posix/unistd.h>
+#endif
+
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+
+
+
+// --- micro-ROS CAN Transport for Zephyr ---
+
+
+CAN_MSGQ_DEFINE(can_rx_msgq, 64);
+int filter_id;
+
+
+
+
+
+
+bool zephyr_transport_open(struct uxrCustomTransport * transport)
+{
+    zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args;
+    const struct device * can_dev = (const struct device *) params->can_dev;
+    if (!device_is_ready(can_dev)) {
+		printf("CAN: Device %s not ready.\n", can_dev->name);
+		return false;
+	}
+
+    const struct can_filter can_rx_filter = {
+        //Support for Zephyr 3.5.0 (already End of Life) , and Zephyr 3.6.0, and 3.7.0
+        #if ZEPHYR_VERSION_CODE == ZEPHYR_VERSION(3,5,0)
+        .flags =  CAN_FILTER_DATA | CAN_FILTER_FDF | CAN_FILTER_IDE ,
+        #elif (ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,6,0)) && ZEPHYR_VERSION_CODE <= ZEPHYR_VERSION(3,7,0)
+            .flags = CAN_FILTER_DATA | CAN_FILTER_IDE,
+        #endif
+        .id = (uint32_t) params->can_id,
+
+        /*Have to look more into what range of CAN IDs to filter for micro-ROS messages. 
+        For now we will assume that the whole bus will be used exlusively for micro-ROS */
+        .mask =  0x1FFFF000U
+    };
+
+
+    int ret = can_set_mode(can_dev,  CAN_MODE_FD  );
+    ret = can_start(can_dev);
+
+    int filter_id;
+    filter_id = can_add_rx_filter_msgq(can_dev, &can_rx_msgq, &can_rx_filter);
+
+    if (filter_id < 0) 
+    {
+     printf("Unable to add rx filter [%d]\n", filter_id);
+    }
+    else
+    {
+        printf("Filter id: %d\n", filter_id);
+    }
+
+    return true;
+}
+
+bool zephyr_transport_close(struct uxrCustomTransport * transport)
+{
+    zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args;
+    const struct device * can_dev = (const struct device *) params->can_dev;
+    return can_stop(can_dev);
+
+}
+
+void tx_irq_callback(const struct device *dev, int error, void *user_data)
+{
+        char *sender = (char *)user_data;
+
+        if (error != 0) {
+                printf("Sending failed [%d]\nSender: %s\n", error, sender);
+        }
+}
+
+
+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;
+    const struct device * can_dev = (const struct device *) params->can_dev;
+    struct can_frame frame = {
+        .flags = CAN_FRAME_FDF | CAN_FRAME_IDE ,
+        .id = (uint32_t) params->can_id, 
+        .dlc = can_bytes_to_dlc(len+1),
+    };
+
+
+    memcpy(&(frame.data[1]),buf,len);
+    frame.data[0]= (uint8_t) len;
+    can_send(can_dev, &frame,  K_FOREVER, tx_irq_callback, "Sender 1");
+
+    return len;
+}
+
+size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err){
+    struct can_frame rx_frame;
+    rx_frame.data[0] = 0;
+
+    uint32_t queue_count =  k_msgq_num_used_get(&can_rx_msgq);
+
+    k_msgq_get(&can_rx_msgq, &rx_frame, K_MSEC(timeout));
+    queue_count =  k_msgq_num_used_get(&can_rx_msgq);
+
+    uint8_t received_bytes = rx_frame.data_32[0];
+
+    if(received_bytes > len)
+    {
+        printf("Error Received bytes > len \n");
+        *err=1;
+        return 0;
+    }
+
+    memcpy(buf,&(rx_frame.data[1]),received_bytes);
+
+
+    return received_bytes;
+
+}

modules/libmicroros/microros_transports/canfd/microros_transports.h

@@ -0,0 +1,53 @@
+// 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
+
+#define MICRO_ROS_FRAMING_REQUIRED false
+
+typedef struct {
+     const struct device * can_dev;
+      uint32_t can_id;
+
+} zephyr_transport_params_t;
+
+static zephyr_transport_params_t default_params = {
+    .can_dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_canbus)),
+    .can_id = 0x00000001
+};
+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_