From 6c53932267a68e53b01fbe09531fbcd132575acd Mon Sep 17 00:00:00 2001 From: Timo Date: Sat, 23 Aug 2025 10:27:29 +0200 Subject: [PATCH] led0 --- boards/nucleo_h563zi.overlay | 15 ++- .../serial/microros_transports.c | 4 + prj.conf | 7 ++ src/main.cpp | 103 +++++++++--------- 4 files changed, 74 insertions(+), 55 deletions(-) diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay index 267997d..567a169 100644 --- a/boards/nucleo_h563zi.overlay +++ b/boards/nucleo_h563zi.overlay @@ -14,11 +14,24 @@ &uart4 { status = "okay"; - current-speed = <115200>; + current-speed = <921600>; pinctrl-0 = <&uart4_tx_pa0 &uart4_rx_pd0>; pinctrl-names = "default"; }; &usart3 { status = "okay"; +}; + +&timers1 { + status = "okay"; + + st,prescaler = <0>; /* Required */ + + pwm0: pwm { + compatible = "st,stm32-pwm"; + status = "okay"; + pinctrl-0 = <&tim1_ch1_pa8>; + pinctrl-names = "default"; + }; }; \ No newline at end of file diff --git a/modules/libmicroros/microros_transports/serial/microros_transports.c b/modules/libmicroros/microros_transports/serial/microros_transports.c index fe2ccf7..ef2af0f 100644 --- a/modules/libmicroros/microros_transports/serial/microros_transports.c +++ b/modules/libmicroros/microros_transports/serial/microros_transports.c @@ -84,11 +84,13 @@ 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; + /* printf("TX: "); for (size_t i = 0; i < len; i++) { printf("%02X ", buf[i]); } printf("\n"); + */ for (size_t i = 0; i < len; i++) { @@ -113,11 +115,13 @@ size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, read = ring_buf_get(&in_ringbuf, buf, len); uart_irq_rx_enable(params->uart_dev); + /* printf("RX: "); for (size_t i = 0; i < read; i++) { printf("%02X ", buf[i]); } printf("\n"); + */ return read; } \ No newline at end of file diff --git a/prj.conf b/prj.conf index da986dd..c6e6401 100644 --- a/prj.conf +++ b/prj.conf @@ -9,13 +9,20 @@ CONFIG_POSIX_API=y # logging CONFIG_LOG=y +CONFIG_STDOUT_CONSOLE=y +CONFIG_PRINTK=y +CONFIG_LOG_DEFAULT_LEVEL=3 # gpio support CONFIG_GPIO=y +CONFIG_LED=y # usb CONFIG_UART_INTERRUPT_DRIVEN=y +# pwm +CONFIG_PWM=y +CONFIG_PWM_STM32=y # micro ros CONFIG_MICROROS=y diff --git a/src/main.cpp b/src/main.cpp index dcefc11..3e587b6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,18 +5,16 @@ #include #include #include -#include #else #include #include #include #include -#include #endif #include #include -#include +#include #include #include @@ -27,69 +25,66 @@ #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);for(;;){};}} #define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}} -rcl_publisher_t publisher; -std_msgs__msg__Int32 msg; +rcl_subscription_t subscriber; -void timer_callback(rcl_timer_t * timer, int64_t last_call_time) +std_msgs__msg__Bool msg; +static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios); + + +void led0_callback(const void *msgin) { - RCLC_UNUSED(last_call_time); - if (timer != NULL) { - RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); - printk("msg: %d", msg.data++); - } + const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin; + + gpio_pin_configure_dt(&led0, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE); } int main(void) { - rmw_uros_set_custom_transport( - MICRO_ROS_FRAMING_REQUIRED, - (void *) &default_params, - zephyr_transport_open, - zephyr_transport_close, - zephyr_transport_write, - zephyr_transport_read - ); + if (!device_is_ready(led0.port)) { + printk("LED not ready\n"); + return -1; + } - rcl_allocator_t allocator = rcl_get_default_allocator(); - rclc_support_t support; + /* micro-ROS transport setup */ + rmw_uros_set_custom_transport( + MICRO_ROS_FRAMING_REQUIRED, + (void *)&default_params, + zephyr_transport_open, + zephyr_transport_close, + zephyr_transport_write, + zephyr_transport_read + ); - // create init_options - RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rclc_support_t support; - // create node - rcl_node_t node; - RCCHECK(rclc_node_init_default(&node, "zephyr_int32_publisher", "", &support)); + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); - // create publisher - RCCHECK(rclc_publisher_init_default( - &publisher, - &node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), - "zephyr_int32_publisher")); + rcl_node_t node; + RCCHECK(rclc_node_init_default(&node, "zephyr_servo_node", "", &support)); - // create timer, - rcl_timer_t timer; - const unsigned int timer_timeout = 1000; - RCCHECK(rclc_timer_init_default( - &timer, - &support, - RCL_MS_TO_NS(timer_timeout), - timer_callback)); + /* Create subscriber for /servo/position */ + RCCHECK(rclc_subscription_init_default( + &subscriber, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), + "/nucleo/led0" + )); - // create executor - rclc_executor_t executor; - RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); - RCCHECK(rclc_executor_add_timer(&executor, &timer)); + /* Create executor */ + rclc_executor_t executor; + RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); + RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &led0_callback, ON_NEW_DATA)); - msg.data = 0; + /* Spin executor loop */ + while (1) { + rclc_executor_spin_some(&executor, 100); + k_sleep(K_MSEC(10)); + } - while(1){ - rclc_executor_spin_some(&executor, 100); - usleep(100000); - } + /* Free resources (unreachable in infinite loop) */ + RCCHECK(rcl_subscription_fini(&subscriber, &node)) + RCCHECK(rcl_node_fini(&node)) - // free resources - RCCHECK(rcl_publisher_fini(&publisher, &node)) - RCCHECK(rcl_node_fini(&node)) - return 0; -} \ No newline at end of file + return 0; +}