diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay index 8553ef2..8a459ba 100644 --- a/boards/nucleo_h563zi.overlay +++ b/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"; +}; \ No newline at end of file diff --git a/modules/libmicroros/CMakeLists.txt b/modules/libmicroros/CMakeLists.txt index 4e8c213..25bc17b 100644 --- a/modules/libmicroros/CMakeLists.txt +++ b/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) diff --git a/modules/libmicroros/Kconfig b/modules/libmicroros/Kconfig index b18c21c..18abf59 100644 --- a/modules/libmicroros/Kconfig +++ b/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" diff --git a/modules/libmicroros/microros_transports/fdcan/microros_transports.c b/modules/libmicroros/microros_transports/fdcan/microros_transports.c new file mode 100644 index 0000000..7620023 --- /dev/null +++ b/modules/libmicroros/microros_transports/fdcan/microros_transports.c @@ -0,0 +1,138 @@ +#include + +#include +#include + +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#include +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + +#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; +} \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/fdcan/microros_transports.h b/modules/libmicroros/microros_transports/fdcan/microros_transports.h new file mode 100644 index 0000000..8889d47 --- /dev/null +++ b/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 +#include + +#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0) +#include +#else +#include +#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_ \ No newline at end of file diff --git a/prj.conf b/prj.conf index c6e6401..f1f0b50 100644 --- a/prj.conf +++ b/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 \ No newline at end of file +CONFIG_MICROROS_TRANSPORT_FDCAN=y + +# can FD +CONFIG_CAN=y +CONFIG_CAN_FD_MODE=y +CONFIG_CAN_STM32_FDCAN=y \ No newline at end of file diff --git a/src/devices.cpp b/src/devices.cpp index f6233f1..7ce8dc8 100644 --- a/src/devices.cpp +++ b/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; } \ No newline at end of file diff --git a/src/devices.hpp b/src/devices.hpp index 355c54e..5dfddee 100644 --- a/src/devices.hpp +++ b/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); \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 4f0c32b..fbb916a 100644 --- a/src/main.cpp +++ b/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"); diff --git a/src/microros/microros_node.cpp b/src/microros/microros_node.cpp index 9db7821..d36b437 100644 --- a/src/microros/microros_node.cpp +++ b/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 (!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 (rclc_executor_init(&this->executor, &this->support.context, 32, &this->allocator) != RCL_RET_OK) - return false; + 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; }