added servo

boards/nucleo_h563zi.overlay

@@ -1,20 +1,27 @@
 / {
     aliases {
-        led0 = &user_led;
+        led0 = &red_led_1;
+        led1 = &yellow_led_1;
+        led2 = &green_led_1;
+
+        servo0 = &pwm_servo0;
     };
 
-    leds {
+    pwmleds: pwmleds {
-        compatible = "gpio-leds";
+		compatible = "pwm-leds";
-        user_led: user_led_0 {
+		status = "okay";
-            gpios = <&gpiob 0 GPIO_ACTIVE_HIGH>;
+
-            label = "USER_LED_0";
+		pwm_servo0: pwm_servo0 {
-        };
+			pwms = <&pwm4 3 PWM_MSEC(20) PWM_POLARITY_NORMAL>;
-    };
+			label = "servo 0";
+		};
+	};
+
 };
 
 &uart4 {
     status = "okay";
-    current-speed = <921600>;
+    current-speed = <460800>;
     pinctrl-0 = <&uart4_tx_pa0 &uart4_rx_pd0>;
     pinctrl-names = "default";
 };
@@ -23,15 +30,13 @@
     status = "okay";
 };
 
-&timers1 {
+&timers4 {
     status = "okay";
+    st,prescaler = <119>;
 
-    st,prescaler = <0>;   /* Required */
+    pwm4: pwm {
-
-    pwm0: pwm {
-        compatible = "st,stm32-pwm";
         status = "okay";
-        pinctrl-0 = <&tim1_ch1_pa8>;
+        pinctrl-0 = <&tim4_ch3_pb8>;
         pinctrl-names = "default";
     };
 };

src/devices.cpp

@@ -4,7 +4,22 @@ bool verify_are_devices_available(void) {
     bool devices_ready = true;
 
     if (!device_is_ready(led0.port)) {
-        printk("LED not ready\n");
+        printk("LED0 not ready\n");
+        devices_ready = false;
+    }
+
+    if (!device_is_ready(led1.port)) {
+        printk("LED1 not ready\n");
+        devices_ready = false;
+    }
+
+    if (!device_is_ready(led2.port)) {
+        printk("LED2 not ready\n");
+        devices_ready = false;
+    }
+
+    if (!pwm_is_ready_dt(&servo0)) {
+        printk("Servo0 not ready\n");
         devices_ready = false;
     }
 

src/devices.hpp

@@ -6,12 +6,17 @@
 #include <zephyr/device.h>
 #include <zephyr/devicetree.h>
 #include <zephyr/drivers/gpio.h>
+#include <zephyr/drivers/pwm.h>
 #else
 #include <device.h>
 #include <devicetree.h>
-#include <drivers/gpio.h>
+#include <drivers/pwm.h>
 #endif
 
 static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
+static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios);
+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));
 
 bool verify_are_devices_available(void);

src/main.cpp

@@ -13,12 +13,15 @@
 #include "microros_node.hpp"
 
 #include "led0_subscriber.hpp"
+#include "led1_subscriber.hpp"
+#include "led2_subscriber.hpp"
+#include "servo0_subscriber.hpp"
 
 
 int main(void)
 {
     if (!verify_are_devices_available()) {
-        printk("##### Not all devices are available, aborting launch #####");
+        printk("##### Not all devices are available, aborting launch #####\n");
         for (;;){}
     }
 
@@ -34,10 +37,16 @@ int main(void)
     MicrorosExecutor node0 = MicrorosExecutor("nucleo_node0");
 
     while (!node0.connect()) {
-        printk("Waiting for connection ...");
+        printk("Waiting for connection ...\n");
     };
 
+    printk("Connection to Agent node established!\n");
+    printk("Starting executor ...\n");
+
     led0_register(&node0);
+    led1_register(&node0);
+    led2_register(&node0);
+    servo0_register(&node0);
 
     node0.execute();
 

src/microros/microros_node.cpp

@@ -41,7 +41,7 @@ bool MicrorosExecutor::connect() {
     if (rclc_node_init_default(&this->node, this->node_name, "", &this->support) != RCL_RET_OK)
         return false;
 
-    if (rclc_executor_init(&this->executor, &this->support.context, 1, &this->allocator) != RCL_RET_OK)
+    if (rclc_executor_init(&this->executor, &this->support.context, 32, &this->allocator) != RCL_RET_OK)
         return false;
 
     return true;
@@ -67,6 +67,7 @@ rcl_publisher_t* MicrorosExecutor::add_publisher(const rosidl_message_type_suppo
     // Append to list
     sys_slist_append(&this->publishers_head, &pub_node->node);
 
+    printk("registered publisher %s\n", topic_name);
     return &pub_node->publisher;
 }
 
@@ -88,5 +89,6 @@ rcl_subscription_t* MicrorosExecutor::add_subscription(const rosidl_message_type
         return nullptr;
     }
 
+    printk("registered subscriber %s\n", topic_name);
     return &sub_node->subscription;
 }

src/microros/subscribers/CMakeLists.txt

@@ -1,3 +1,7 @@
 cmake_minimum_required(VERSION 3.20.0)
 
 add_subdirectory(led0)
+add_subdirectory(led1)
+add_subdirectory(led2)
+
+add_subdirectory(servo0)

src/microros/subscribers/led0/led0_subscriber.cpp

@@ -6,11 +6,12 @@
 #include "devices.hpp"
 
 
-std_msgs__msg__Bool msg;
+std_msgs__msg__Bool led0_msg;
 
 void led0_callback(const void *msgin){
     const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin;
 
+    printk("Set led0 to %s\n", msg->data ? "on" : "off");
     gpio_pin_configure_dt(&led0, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
 }
 
@@ -19,7 +20,7 @@ void led0_register(MicrorosExecutor* node) {
     node->add_subscription(
         ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
         "/nucleo/led0",
-        &msg,
+        &led0_msg,
         led0_callback
     );
 }

src/microros/subscribers/led1/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+target_include_directories(app PRIVATE .)
+
+target_sources(app PRIVATE 
+    led1_subscriber.cpp
+)

src/microros/subscribers/led1/led1_subscriber.cpp

@@ -0,0 +1,26 @@
+#include "led1_subscriber.hpp"
+
+#include <zephyr/drivers/gpio.h>
+#include <std_msgs/msg/bool.h>
+
+#include "devices.hpp"
+
+
+std_msgs__msg__Bool led1_msg;
+
+void led1_callback(const void *msgin){
+    const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin;
+
+    printk("Set led1 to %s\n", msg->data ? "on" : "off");
+    gpio_pin_configure_dt(&led1, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+}
+
+
+void led1_register(MicrorosExecutor* node) {
+    node->add_subscription(
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led1",
+        &led1_msg,
+        led1_callback
+    );
+}

src/microros/subscribers/led1/led1_subscriber.hpp

@@ -0,0 +1,5 @@
+#pragma once
+
+#include "microros_node.hpp"
+
+void led1_register(MicrorosExecutor* node);

src/microros/subscribers/led2/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+target_include_directories(app PRIVATE .)
+
+target_sources(app PRIVATE 
+    led2_subscriber.cpp
+)

src/microros/subscribers/led2/led2_subscriber.cpp

@@ -0,0 +1,26 @@
+#include "led2_subscriber.hpp"
+
+#include <zephyr/drivers/gpio.h>
+#include <std_msgs/msg/bool.h>
+
+#include "devices.hpp"
+
+
+std_msgs__msg__Bool led2_msg;
+
+void led2_callback(const void *msgin){
+    const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin;
+
+    printk("Set led2 to %s\n", msg->data ? "on" : "off");
+    gpio_pin_configure_dt(&led2, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+}
+
+
+void led2_register(MicrorosExecutor* node) {
+    node->add_subscription(
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led2",
+        &led2_msg,
+        led2_callback
+    );
+}

src/microros/subscribers/led2/led2_subscriber.hpp

@@ -0,0 +1,5 @@
+#pragma once
+
+#include "microros_node.hpp"
+
+void led2_register(MicrorosExecutor* node);

src/microros/subscribers/servo0/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+target_include_directories(app PRIVATE .)
+
+target_sources(app PRIVATE 
+    servo0_subscriber.cpp
+)

src/microros/subscribers/servo0/servo0_subscriber.cpp

@@ -0,0 +1,45 @@
+#include "servo0_subscriber.hpp"
+
+#include <zephyr/drivers/pwm.h>
+#include <std_msgs/msg/float32.h>
+
+#include "devices.hpp"
+
+
+std_msgs__msg__Float32 servo_msg;
+
+const uint32_t min_pulse_ns = 500000;
+const uint32_t max_pulse_ns = 2500000;
+
+
+void servo_callback(const void *msgin){
+    const std_msgs__msg__Float32 *msg = (const std_msgs__msg__Float32 *)msgin;
+
+    float angle = msg->data;
+
+    // Clamp angle to valid range
+    if (angle < 0.0f) angle = 0.0f;
+    if (angle > 180.0f) angle = 180.0f;
+    
+    uint32_t pulse_ns = min_pulse_ns + (angle / 180.0f) * (max_pulse_ns - min_pulse_ns);
+
+    printk("Set servo to %d.%d° = %d.%d ms°\n", int(angle), int(angle * 100) % 100, pulse_ns / 1000000, (pulse_ns / 1000) % 1000);
+
+    int ret = pwm_set_pulse_dt(&servo0, pulse_ns);
+
+    if (ret == 0) {
+        printk("PWM pulse set successfully.\n");
+    } else {
+        printk("Failed to set PWM pulse! Error: %d\n", ret);
+    }
+}
+
+
+void servo0_register(MicrorosExecutor* node) {
+    node->add_subscription(
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
+        "/nucleo/servo0",
+        &servo_msg,
+        servo_callback
+    );
+}

src/microros/subscribers/servo0/servo0_subscriber.hpp

@@ -0,0 +1,5 @@
+#pragma once
+
+#include "microros_node.hpp"
+
+void servo0_register(MicrorosExecutor* node);