led0

boards/nucleo_h563zi.overlay

@@ -14,7 +14,7 @@
 
 &uart4 {
     status = "okay";
-    current-speed = <115200>;
+    current-speed = <921600>;
     pinctrl-0 = <&uart4_tx_pa0 &uart4_rx_pd0>;
     pinctrl-names = "default";
 };
@@ -22,3 +22,16 @@
 &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";
+    };
+};

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;
 }

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

src/main.cpp

@@ -5,18 +5,16 @@
 #include <zephyr/device.h>
 #include <zephyr/devicetree.h>
 #include <zephyr/drivers/gpio.h>
-#include <zephyr/posix/time.h>
 #else
 #include <zephyr.h>
 #include <device.h>
 #include <devicetree.h>
 #include <drivers/gpio.h>
-#include <posix/time.h>
 #endif
 
 #include <rcl/rcl.h>
 #include <rcl/error_handling.h>
-#include <std_msgs/msg/int32.h>
+#include <std_msgs/msg/bool.h>
 
 #include <rclc/rclc.h>
 #include <rclc/executor.h>
@@ -27,20 +25,27 @@
 #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)
 {
+    if (!device_is_ready(led0.port)) {
+        printk("LED not ready\n");
+        return -1;
+    }
+
+    /* micro-ROS transport setup */
     rmw_uros_set_custom_transport(
         MICRO_ROS_FRAMING_REQUIRED,
         (void *)&default_params,
@@ -53,43 +58,33 @@ int main(void)
     rcl_allocator_t allocator = rcl_get_default_allocator();
     rclc_support_t support;
 
-	// create init_options
     RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
 
-	// create node
     rcl_node_t node;
-	RCCHECK(rclc_node_init_default(&node, "zephyr_int32_publisher", "", &support));
+    RCCHECK(rclc_node_init_default(&node, "zephyr_servo_node", "", &support));
 
-	// create publisher
-	RCCHECK(rclc_publisher_init_default(
-		&publisher,
+    /* Create subscriber for /servo/position */
+    RCCHECK(rclc_subscription_init_default(
+        &subscriber,
         &node,
-		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
-		"zephyr_int32_publisher"));
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led0"
+    ));
 
-	// 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 executor
+    /* Create executor */
     rclc_executor_t executor;
     RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
-	RCCHECK(rclc_executor_add_timer(&executor, &timer));
-
-	msg.data = 0;
+    RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &led0_callback, ON_NEW_DATA));
 
+    /* Spin executor loop */
     while (1) {
         rclc_executor_spin_some(&executor, 100);
-		usleep(100000);
+        k_sleep(K_MSEC(10));
     }
 
-	// free resources
-	RCCHECK(rcl_publisher_fini(&publisher, &node))
+    /* Free resources (unreachable in infinite loop) */
+    RCCHECK(rcl_subscription_fini(&subscriber, &node))
     RCCHECK(rcl_node_fini(&node))
+
     return 0;
 }