From fdf14badc3bbaa2152e4c1144d28aa7ef2ddc65d Mon Sep 17 00:00:00 2001 From: Felipe Neves Date: Fri, 26 Aug 2022 03:58:06 -0300 Subject: [PATCH] boards: move board specific configurations (#63) * 4.0.0 * boards: move board specifc details to its own folder Signed-off-by: Felipe * transports: use zephyr DT macros to set the serial port for serial transport. Signed-off-by: Felipe * boards: disco_l475_iot1: add zephyr UDC0 nodelabel replacing the CONFIG_USB symbol dropped in version 2.6 Signed-off-by: Felipe Neves * Update CHANGELOG.rst Signed-off-by: Felipe Signed-off-by: Felipe Neves Co-authored-by: Pablo Garrido (cherry picked from commit 7e07006cad1d8b3170adef5d9a6642f5017021db) --- boards/disco_l475_iot1.conf | 7 ++++ boards/disco_l475_iot1.overlay | 20 +++++++++ .../serial-usb/microros_transports.c | 42 ++++++++++--------- .../serial-usb/microros_transports.h | 6 --- .../serial/microros_transports.c | 25 ++++------- .../serial/microros_transports.h | 6 --- prj.conf | 8 +--- src/main.c | 2 +- 8 files changed, 59 insertions(+), 57 deletions(-) create mode 100644 boards/disco_l475_iot1.conf create mode 100644 boards/disco_l475_iot1.overlay diff --git a/boards/disco_l475_iot1.conf b/boards/disco_l475_iot1.conf new file mode 100644 index 0000000..fbb1020 --- /dev/null +++ b/boards/disco_l475_iot1.conf @@ -0,0 +1,7 @@ +CONFIG_USB_DEVICE_STACK=y +CONFIG_USB_DEVICE_PRODUCT="Zephyr micro-ROS" +CONFIG_LOG=y +CONFIG_USB_CDC_ACM=y +CONFIG_USB_DRIVER_LOG_LEVEL_ERR=y +CONFIG_USB_DEVICE_LOG_LEVEL_ERR=y +CONFIG_USB_CDC_ACM_RINGBUF_SIZE=2048 diff --git a/boards/disco_l475_iot1.overlay b/boards/disco_l475_iot1.overlay new file mode 100644 index 0000000..7bd84c9 --- /dev/null +++ b/boards/disco_l475_iot1.overlay @@ -0,0 +1,20 @@ +/ { + aliases { + uros-serial-port = &usart1; + }; +}; + +&usart1 { + status = "okay"; +}; + +zephyr_udc0: &usbotg_fs { + status = "okay"; +}; + +&zephyr_udc0 { + cdc_acm_uart0: cdc_acm_uart0 { + compatible = "zephyr,cdc-acm-uart"; + label = "CDC_ACM_0"; + }; +}; diff --git a/modules/libmicroros/microros_transports/serial-usb/microros_transports.c b/modules/libmicroros/microros_transports/serial-usb/microros_transports.c index 2b03461..b2cd64e 100644 --- a/modules/libmicroros/microros_transports/serial-usb/microros_transports.c +++ b/modules/libmicroros/microros_transports/serial-usb/microros_transports.c @@ -22,7 +22,7 @@ char uart_out_buffer[RING_BUF_SIZE]; struct ring_buf out_ringbuf, in_ringbuf; -static void uart_fifo_callback(const struct device *dev, void * user_data){ +static void uart_fifo_callback(const struct device *dev, void *user_data){ while (uart_irq_update(dev) && uart_irq_is_pending(dev)) { if (uart_irq_rx_ready(dev)) { int recv_len; @@ -53,14 +53,15 @@ static void uart_fifo_callback(const struct device *dev, void * user_data){ } bool zephyr_transport_open(struct uxrCustomTransport * transport){ - zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; - int ret; uint32_t baudrate, dtr = 0U; + const struct device *uart_dev; - - params->uart_dev = device_get_binding("CDC_ACM_0"); - if (!params->uart_dev) { + /* for serial-usb transport we just override the device pointer + * with USB to use the same interface + */ + transport->args = (void *)device_get_binding("CDC_ACM_0"); + if (!transport->args) { printk("CDC ACM device not found\n"); return false; } @@ -71,13 +72,15 @@ bool zephyr_transport_open(struct uxrCustomTransport * transport){ return false; } + uart_dev = (const struct device *)transport->args; ring_buf_init(&out_ringbuf, sizeof(uart_out_buffer), uart_out_buffer); ring_buf_init(&in_ringbuf, sizeof(uart_in_buffer), uart_out_buffer); printk("Waiting for agent connection\n"); while (true) { - uart_line_ctrl_get(params->uart_dev, UART_LINE_CTRL_DTR, &dtr); + + uart_line_ctrl_get(uart_dev, UART_LINE_CTRL_DTR, &dtr); if (dtr) { break; } else { @@ -89,12 +92,12 @@ bool zephyr_transport_open(struct uxrCustomTransport * transport){ printk("Serial port connected!\n"); /* They are optional, we use them to test the interrupt endpoint */ - ret = uart_line_ctrl_set(params->uart_dev, UART_LINE_CTRL_DCD, 1); + ret = uart_line_ctrl_set(uart_dev, UART_LINE_CTRL_DCD, 1); if (ret) { printk("Failed to set DCD, ret code %d\n", ret); } - ret = uart_line_ctrl_set(params->uart_dev, UART_LINE_CTRL_DSR, 1); + ret = uart_line_ctrl_set(uart_dev, UART_LINE_CTRL_DSR, 1); if (ret) { printk("Failed to set DSR, ret code %d\n", ret); } @@ -102,34 +105,33 @@ bool zephyr_transport_open(struct uxrCustomTransport * transport){ /* Wait 1 sec for the host to do all settings */ k_busy_wait(1000*1000); - ret = uart_line_ctrl_get(params->uart_dev, UART_LINE_CTRL_BAUD_RATE, &baudrate); + ret = uart_line_ctrl_get(uart_dev, UART_LINE_CTRL_BAUD_RATE, &baudrate); if (ret) { printk("Failed to get baudrate, ret code %d\n", ret); } - uart_irq_callback_set(params->uart_dev, uart_fifo_callback); + uart_irq_callback_set(uart_dev, uart_fifo_callback); /* Enable rx interrupts */ - uart_irq_rx_enable(params->uart_dev); + uart_irq_rx_enable(uart_dev); return true; } bool zephyr_transport_close(struct uxrCustomTransport * transport){ - zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; - (void) params; + (void)transport; 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; + const struct device * uart_dev = (const struct device *) transport->args; size_t wrote; wrote = ring_buf_put(&out_ringbuf, buf, len); - - uart_irq_tx_enable(params->uart_dev); + + uart_irq_tx_enable(uart_dev); while (!ring_buf_is_empty(&out_ringbuf)){ k_sleep(K_MSEC(5)); @@ -139,7 +141,7 @@ size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_ } 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; + const struct device * uart_dev = (const struct device *) transport->args; size_t read = 0; int spent_time = 0; @@ -149,9 +151,9 @@ size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, spent_time++; } - uart_irq_rx_disable(params->uart_dev); + uart_irq_rx_disable(uart_dev); read = ring_buf_get(&in_ringbuf, buf, len); - uart_irq_rx_enable(params->uart_dev); + uart_irq_rx_enable(uart_dev); return read; } \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/serial-usb/microros_transports.h b/modules/libmicroros/microros_transports/serial-usb/microros_transports.h index 281ad5b..1188037 100644 --- a/modules/libmicroros/microros_transports/serial-usb/microros_transports.h +++ b/modules/libmicroros/microros_transports/serial-usb/microros_transports.h @@ -24,13 +24,7 @@ extern "C" { #endif -typedef struct { - size_t fd; - const struct device *uart_dev; -} zephyr_transport_params_t; - #define MICRO_ROS_FRAMING_REQUIRED true -static zephyr_transport_params_t default_params; bool zephyr_transport_open(struct uxrCustomTransport * transport); bool zephyr_transport_close(struct uxrCustomTransport * transport); diff --git a/modules/libmicroros/microros_transports/serial/microros_transports.c b/modules/libmicroros/microros_transports/serial/microros_transports.c index 5a5ab3c..b43b3fd 100644 --- a/modules/libmicroros/microros_transports/serial/microros_transports.c +++ b/modules/libmicroros/microros_transports/serial/microros_transports.c @@ -40,24 +40,15 @@ static void uart_fifo_callback(const struct device * dev, void * args){ } } - bool zephyr_transport_open(struct uxrCustomTransport * transport){ - zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; - - char uart_descriptor[8]; - sprintf(uart_descriptor,"UART_%d", params->fd); - params->uart_dev = device_get_binding(uart_descriptor); - if (!params->uart_dev) { - printk("Serial device not found\n"); - return false; - } + const struct device * uart_dev = (const struct device *) transport->args; ring_buf_init(&in_ringbuf, sizeof(uart_in_buffer), uart_out_buffer); - uart_irq_callback_set(params->uart_dev, uart_fifo_callback); + uart_irq_callback_set(uart_dev, uart_fifo_callback); /* Enable rx interrupts */ - uart_irq_rx_enable(params->uart_dev); + uart_irq_rx_enable(uart_dev); return true; } @@ -69,18 +60,18 @@ 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){ - zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args; + const struct device * uart_dev = (const struct device *) transport->args; for (size_t i = 0; i < len; i++) { - uart_poll_out(params->uart_dev, buf[i]); + uart_poll_out(uart_dev, buf[i]); } 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; + const struct device * uart_dev = (const struct device *) transport->args; size_t read = 0; int spent_time = 0; @@ -90,9 +81,9 @@ size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, spent_time++; } - uart_irq_rx_disable(params->uart_dev); + uart_irq_rx_disable(uart_dev); read = ring_buf_get(&in_ringbuf, buf, len); - uart_irq_rx_enable(params->uart_dev); + uart_irq_rx_enable(uart_dev); return read; } \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/serial/microros_transports.h b/modules/libmicroros/microros_transports/serial/microros_transports.h index 8b45c8c..1188037 100644 --- a/modules/libmicroros/microros_transports/serial/microros_transports.h +++ b/modules/libmicroros/microros_transports/serial/microros_transports.h @@ -24,13 +24,7 @@ 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); diff --git a/prj.conf b/prj.conf index ccb67ed..8afc017 100644 --- a/prj.conf +++ b/prj.conf @@ -13,13 +13,7 @@ CONFIG_APP_LINK_WITH_POSIX_SUBSYS=y CONFIG_POSIX_CLOCK=y CONFIG_STDOUT_CONSOLE=y -CONFIG_USB_DEVICE_STACK=y -CONFIG_USB_DEVICE_PRODUCT="Zephyr micro-ROS" -CONFIG_LOG=y -CONFIG_USB_CDC_ACM=y CONFIG_SERIAL=y CONFIG_UART_INTERRUPT_DRIVEN=y CONFIG_UART_LINE_CTRL=y -CONFIG_USB_DRIVER_LOG_LEVEL_ERR=y -CONFIG_USB_DEVICE_LOG_LEVEL_ERR=y -CONFIG_USB_CDC_ACM_RINGBUF_SIZE=2048 +CONFIG_RING_BUFFER=y diff --git a/src/main.c b/src/main.c index 0b684c7..440740e 100644 --- a/src/main.c +++ b/src/main.c @@ -33,7 +33,7 @@ void main(void) { rmw_uros_set_custom_transport( MICRO_ROS_FRAMING_REQUIRED, - (void *) &default_params, + (void *) DEVICE_DT_GET(DT_ALIAS(uros_serial_port)), zephyr_transport_open, zephyr_transport_close, zephyr_transport_write,