diff --git a/modules/libmicroros/CMakeLists.txt b/modules/libmicroros/CMakeLists.txt index 193524d..b3f916c 100644 --- a/modules/libmicroros/CMakeLists.txt +++ b/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() diff --git a/modules/libmicroros/Kconfig b/modules/libmicroros/Kconfig index d918025..d24eabf 100644 --- a/modules/libmicroros/Kconfig +++ b/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 diff --git a/modules/libmicroros/microros_transports/canfd/microros_transports.c b/modules/libmicroros/microros_transports/canfd/microros_transports.c new file mode 100644 index 0000000..eae2546 --- /dev/null +++ b/modules/libmicroros/microros_transports/canfd/microros_transports.c @@ -0,0 +1,137 @@ +#include + +#include +#include + +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) + #include + #include + #include + #include + #include +#else + #include + #include + #include + #include + #include +#endif + +#include +#include +#include + + + +// --- 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; + +} \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/canfd/microros_transports.h b/modules/libmicroros/microros_transports/canfd/microros_transports.h new file mode 100644 index 0000000..0ed8fec --- /dev/null +++ b/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 +#include + +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#else +#include +#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_ \ No newline at end of file