wip: onboard pid

CMakeLists.txt

@@ -7,4 +7,7 @@ find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
 
 project(sherpa_ros2_sensor_node C CXX)
 
+set(CMAKE_BUILD_TYPE Debug)
+target_compile_options(app PRIVATE -O0 -g)
+
 add_subdirectory(src)

boards/nucleo_h563zi.overlay

@@ -26,10 +26,27 @@
     pinctrl-names = "default";
 };
 
+&usb {
+    status = "okay";
+    cdc_acm_uart0: cdc_acm_uart0 {
+        compatible = "zephyr,cdc-acm-uart";
+        label = "CDC_ACM_0";
+    };
+};
+
 &usart3 {
     status = "okay";
 };
 
+&timers5 {
+    status = "okay";
+    st,prescaler = <7>;
+
+    counter5: counter {
+        status = "okay";
+    };
+};
+
 &timers4 {
     status = "okay";
     st,prescaler = <119>;
@@ -39,11 +56,4 @@
         pinctrl-0 = <&tim4_ch3_pb8>;
         pinctrl-names = "default";
     };
-};
-
-&fdcan1 {
-    pinctrl-0 = <&fdcan1_rx_pd0 &fdcan1_tx_pd1>;
-    bitrate = <500000>;
-    bitrate-data = <4000000>;
-    status = "okay";
 };

modules/libmicroros/Kconfig

@@ -24,8 +24,7 @@ if MICROROS
             select RING_BUFFER
         config MICROROS_TRANSPORT_SERIAL_USB
             bool "micro-ROS USB serial transport"
-             select RING_BUFFER
+            select RING_BUFFER
-             select USB_DEVICE_STACK
         config MICROROS_TRANSPORT_UDP
             bool "micro-ROS UDP network transport"
         
@@ -106,7 +105,7 @@ if MICROROS
     
     config MICROROS_XRCE_DDS_MTU
         string "micro-ROS transport MTU"
-        default "62"
+        default "512"
         
     config MICROROS_XRCE_DDS_HISTORIC
         string "micro-ROS middleware memory slots"

modules/libmicroros/microros_transports/serial-usb/microros_transports.c

@@ -66,58 +66,20 @@ 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;
+    params->uart_dev = DEVICE_DT_GET(DT_NODELABEL(cdc_acm_uart0));
-    uint32_t baudrate, dtr = 0U;
-
-
-    params->uart_dev = device_get_binding("CDC_ACM_0");
     if (!params->uart_dev) {
         printk("CDC ACM device not found\n");
         return false;
     }
-
+    if (!device_is_ready(params->uart_dev)) {
-    ret = usb_enable(NULL);
+        printk("cdc0 not ready!\n");
-    if (ret != 0) {
-        printk("Failed to enable USB\n");
-        return false;
     }
 
     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);
-        if (dtr) {
-            break;
-        } else {
-            /* Give CPU resources to low priority threads. */
-            k_sleep(K_MSEC(100));
-        }
-    }
-
     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);
-    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);
-    if (ret) {
-        printk("Failed to set DSR, ret code %d\n", ret);
-    }
-
-    /* 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);
-    if (ret) {
-        printk("Failed to get baudrate, ret code %d\n", ret);
-    }
-
     uart_irq_callback_set(params->uart_dev, uart_fifo_callback);
 
     /* Enable rx interrupts */

prj.conf

@@ -17,7 +17,19 @@ CONFIG_GPIO=y
 CONFIG_LED=y
 
 # usb
-CONFIG_UART_INTERRUPT_DRIVEN=y
+CONFIG_SERIAL=y
+CONFIG_UART_LINE_CTRL=y
+CONFIG_USBD_CDC_ACM_CLASS=y
+CONFIG_USB_DEVICE_STACK_NEXT=y
+CONFIG_USBD_CDC_ACM_WORKQUEUE=y
+CONFIG_UDC_WORKQUEUE=y
+CONFIG_UDC_STM32=y
+CONFIG_UDC_STM32_STACK_SIZE=2048
+CONFIG_UDC_WORKQUEUE_STACK_SIZE=2048
+
+# timer
+CONFIG_COUNTER=y
+CONFIG_COUNTER_TIMER_STM32=y
 
 # pwm
 CONFIG_PWM=y
@@ -25,9 +37,14 @@ CONFIG_PWM_STM32=y
 
 # micro ros
 CONFIG_MICROROS=y
-CONFIG_MICROROS_TRANSPORT_FDCAN=y
+CONFIG_MICROROS_XRCE_DDS_MTU="256"
+CONFIG_MICROROS_TRANSPORT_SERIAL_USB=y
+#CONFIG_MICROROS_TRANSPORT_FDCAN=y
 
 # can FD
 CONFIG_CAN=y
 CONFIG_CAN_FD_MODE=y
-CONFIG_CAN_STM32_FDCAN=y
+CONFIG_CAN_STM32_FDCAN=y
+
+# debug
+CONFIG_DEBUG_OPTIMIZATIONS=n

src/CMakeLists.txt

@@ -2,7 +2,10 @@ cmake_minimum_required(VERSION 3.20.0)
 
 target_include_directories(app PRIVATE ./)
 
+add_subdirectory(./led_controller)
+add_subdirectory(./servo_controller)
 add_subdirectory(./microros)
+add_subdirectory(./usb)
 
 target_sources(app PRIVATE
     main.cpp

src/devices.cpp

@@ -1,6 +1,5 @@
 #include "devices.hpp"
 
-const struct device *fdcan1 = DEVICE_DT_GET(DT_NODELABEL(fdcan1));
 
 bool verify_are_devices_available(void) {
     bool devices_ready = true;
@@ -24,8 +23,8 @@ bool verify_are_devices_available(void) {
         printk("Servo0 not ready\n");
         devices_ready = false;
     }
-    if (!device_is_ready(fdcan1)) {
+    if (!device_is_ready(counter5)) {
-        printk("CAN FD 1 not ready\n");
+        printk("Counter 5 not ready\n");
         devices_ready = false;
     }
 

src/devices.hpp

@@ -18,7 +18,6 @@ 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));
-
+static const struct device *counter5 = DEVICE_DT_GET(DT_NODELABEL(counter5));
-extern const struct device *fdcan1;
 
 bool verify_are_devices_available(void);

src/led_controller/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+target_include_directories(app PRIVATE ./)
+
+#add_subdirectory()
+
+target_sources(app PRIVATE
+    led_controller.cpp
+)

src/led_controller/led_controller.cpp

@@ -0,0 +1,92 @@
+#include "led_controller.hpp"
+#include "devices.hpp"
+#include "rcl_util.hpp"
+
+#include <zephyr/drivers/gpio.h>
+#include <rcl/rcl.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+#include <std_msgs/msg/bool.h>
+
+std_msgs__msg__Bool led_msg;
+rcl_node_t LedController::node;
+rcl_subscription_t LedController::led0_subscription;
+rcl_subscription_t LedController::led1_subscription;
+rcl_subscription_t LedController::led2_subscription;
+
+
+void led0_callback(const void *msg){
+    gpio_pin_configure_dt(&led0, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+}
+
+
+void led1_callback(const void *msg){
+    gpio_pin_configure_dt(&led1, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+}
+
+
+void led2_callback(const void *msg){
+    gpio_pin_configure_dt(&led2, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+}
+
+
+
+rcl_ret_t LedController::setup(rclc_support_t* support, rclc_executor_t* executor) {
+    RCL_RETURN_ON_ERROR(rclc_node_init_default(
+        &LedController::node, 
+        "led_controller",
+        "",
+        support));
+
+    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
+        &LedController::led0_subscription,
+        &LedController::node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led0"));
+       
+    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
+        &LedController::led1_subscription,
+        &LedController::node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led1"));
+
+    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
+        &LedController::led2_subscription,
+        &LedController::node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led2"));
+
+    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
+        executor,
+        &LedController::led0_subscription,
+        &led_msg,
+        led0_callback,
+        ON_NEW_DATA
+    ));
+
+    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
+        executor,
+        &LedController::led1_subscription,
+        &led_msg,
+        led1_callback,
+        ON_NEW_DATA
+    ));
+
+    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
+        executor,
+        &LedController::led2_subscription,
+        &led_msg,
+        led2_callback,
+        ON_NEW_DATA
+    ));
+
+    return RCL_RET_OK;
+}
+
+
+void LedController::kill() {
+    rcl_subscription_fini(&LedController::led0_subscription, &LedController::node);
+    rcl_subscription_fini(&LedController::led1_subscription, &LedController::node);
+    rcl_subscription_fini(&LedController::led2_subscription, &LedController::node);
+    rcl_node_fini(&LedController::node);
+}

src/led_controller/led_controller.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include <rclc/executor.h>
+#include <rcl/rcl.h>
+#include <rclc/rclc.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+
+class LedController {
+public:
+    static rcl_ret_t setup(rclc_support_t* support, rclc_executor_t* executor);
+    static void kill();
+
+private:
+    static rcl_node_t node;
+    static rcl_subscription_t led0_subscription;
+    static rcl_subscription_t led1_subscription;
+    static rcl_subscription_t led2_subscription;
+};

src/main.cpp

@@ -10,12 +10,9 @@
 #include <rmw_microros/rmw_microros.h>
 
 #include "devices.hpp"
-#include "microros_node.hpp"
+#include "usb_cdc_acm.hpp"
-
+#include "servo_controller.hpp"
-#include "led0_subscriber.hpp"
+#include "microros_state_machine.hpp"
-#include "led1_subscriber.hpp"
-#include "led2_subscriber.hpp"
-#include "servo0_subscriber.hpp"
 
 
 int main(void)
@@ -25,6 +22,8 @@ int main(void)
         for (;;){}
     }
 
+    usb_init();
+
     rmw_uros_set_custom_transport(
         MICRO_ROS_FRAMING_REQUIRED,
         (void *)&default_params,
@@ -34,24 +33,9 @@ int main(void)
         zephyr_transport_read
     );
 
-    MicrorosExecutor node0 = MicrorosExecutor("nucleo_node0");
+    while (1) {
-
+        micro_ros_state_machine();
-    printk("Connecting to Agent....\n");
+    }
-
+    
-    while (!node0.connect()) {
-        printk("Waiting for connection ...\n");
-        k_msleep(1000);
-    };
-
-    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();
-
     return 0;
 }

src/microros/CMakeLists.txt

@@ -1,10 +1,9 @@
 cmake_minimum_required(VERSION 3.20.0)
 
-add_subdirectory(publishers)
+target_include_directories(app PRIVATE ./)
-add_subdirectory(subscribers)
 
-target_include_directories(app PRIVATE .)
+#add_subdirectory()
 
-target_sources(app PRIVATE 
+target_sources(app PRIVATE
-    microros_node.cpp
+    microros_state_machine.cpp
 )

src/microros/microros_clock.hpp

@@ -0,0 +1,4 @@
+#pragma once
+#include <rcl/time.h>
+
+static rcl_clock_t clock;

src/microros/microros_node.cpp

@@ -1,113 +0,0 @@
-#include "microros_node.hpp"
-
-#include <zephyr/kernel.h>
-#include <rcl/rcl.h>
-#include <rclc/rclc.h>
-#include <zephyr/sys/slist.h>
-#include <rosidl_runtime_c/message_type_support_struct.h>
-#include <stdlib.h>  // for malloc/free
-
-MicrorosExecutor::MicrorosExecutor(const char *name) {
-    this->node_name = name;
-    this->allocator = rcl_get_default_allocator();
-
-    // Initialize empty lists
-    sys_slist_init(&this->publishers_head);
-    sys_slist_init(&this->subscribers_head);
-}
-
-MicrorosExecutor::~MicrorosExecutor() {
-    // Free all publishers
-    struct publisher_node *pub;
-    SYS_SLIST_FOR_EACH_CONTAINER(&this->publishers_head, pub, node) {
-        rcl_publisher_fini(&pub->publisher, &this->node);
-        free(pub);
-    }
-
-    // Free all subscribers
-    struct subscription_node *sub;
-    SYS_SLIST_FOR_EACH_CONTAINER(&this->subscribers_head, sub, node) {
-        rcl_subscription_fini(&sub->subscription, &this->node);
-        free(sub);
-    }
-
-    rcl_node_fini(&this->node);
-}
-
-bool MicrorosExecutor::connect() {
-    static bool support_init = false;
-    static bool defaults_init = false;
-    static bool executor_init = 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 (!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;
-}
-
-bool MicrorosExecutor::execute() {
-    while (1) {
-        rclc_executor_spin_some(&this->executor, 100);
-        k_sleep(K_MSEC(10));
-    }
-}
-
-rcl_publisher_t* MicrorosExecutor::add_publisher(const rosidl_message_type_support_t *type_support, const char *topic_name) {
-    // Allocate new list node
-    struct publisher_node *pub_node = (struct publisher_node *)malloc(sizeof(struct publisher_node));
-    if (!pub_node) return nullptr;
-
-    if (rclc_publisher_init_default(&pub_node->publisher, &this->node, type_support, topic_name) != RCL_RET_OK) {
-        free(pub_node);
-        return nullptr;
-    }
-
-    // Append to list
-    sys_slist_append(&this->publishers_head, &pub_node->node);
-
-    printk("registered publisher %s\n", topic_name);
-    return &pub_node->publisher;
-}
-
-rcl_subscription_t* MicrorosExecutor::add_subscription(const rosidl_message_type_support_t *type_support, const char *topic_name, void* msg, rclc_callback_t callback) {
-    // Allocate new list node
-    struct subscription_node *sub_node = (struct subscription_node *)malloc(sizeof(struct subscription_node));
-    if (!sub_node) return nullptr;
-
-    if (rclc_subscription_init_default(&sub_node->subscription, &this->node, type_support, topic_name) != RCL_RET_OK) {
-        free(sub_node);
-        return nullptr;
-    }
-
-    // Append to list
-    sys_slist_append(&this->subscribers_head, &sub_node->node);
-
-    if (rclc_executor_add_subscription(&this->executor, &sub_node->subscription, msg, callback, ON_NEW_DATA) != RCL_RET_OK) {
-        free(sub_node);
-        return nullptr;
-    }
-
-    printk("registered subscriber %s\n", topic_name);
-    return &sub_node->subscription;
-}

src/microros/microros_node.hpp

@@ -1,40 +0,0 @@
-#pragma once
-
-#include <zephyr/sys/slist.h>
-#include <rcl/rcl.h>
-#include <rclc/rclc.h>
-#include <rclc/executor.h>
-#include <rosidl_runtime_c/message_type_support_struct.h>
-#include <zephyr/kernel.h>
-
-class MicrorosExecutor {
-private:
-    const char *node_name;
-
-    rcl_allocator_t allocator;
-    rclc_executor_t executor;
-    rcl_node_t node;
-    rclc_support_t support;
-
-    sys_slist_t publishers_head;
-    sys_slist_t subscribers_head;
-
-    struct publisher_node {
-        rcl_publisher_t publisher;
-        sys_snode_t node;  // embedded Zephyr list node
-    };
-
-    struct subscription_node {
-        rcl_subscription_t subscription;
-        sys_snode_t node;  // embedded Zephyr list node
-    };
-
-public:
-    MicrorosExecutor(const char *name);
-    ~MicrorosExecutor(void);
-
-    bool connect(void);
-    bool execute(void);
-    rcl_publisher_t* add_publisher(const rosidl_message_type_support_t *type_support, const char *topic_name);
-    rcl_subscription_t* add_subscription(const rosidl_message_type_support_t *type_support, const char *topic_name, void* msg, rclc_callback_t callback);
-};

src/microros/microros_state_machine.cpp

@@ -0,0 +1,98 @@
+#include "microros_state_machine.hpp"
+
+#include "microros_clock.hpp"
+#include "led_controller.hpp"
+#include "servo_controller.hpp"
+
+#include <zephyr/kernel.h>
+#include <rcl/rcl.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+#include <rmw_microros/rmw_microros.h>
+
+
+typedef enum {
+    ROS_AGENT_CONNECTION_UNINITIALIZED,
+    ROS_AGENT_CONNECTION_DISCONNECTED,
+    ROS_AGENT_CONNECTION_SETUP,
+    ROS_AGENT_CONNECTION_UNKNOWN,
+    ROS_AGENT_CONNECTION_CONNECTED,
+    ROS_AGENT_CONNECTION_ERROR,
+} micro_ros_state_t;
+
+static micro_ros_state_t micro_ros_state = ROS_AGENT_CONNECTION_UNINITIALIZED;
+
+void micro_ros_state_machine() {
+    static rcl_allocator_t allocator;
+    static rclc_support_t support;
+    static rclc_executor_t executor;
+    switch (micro_ros_state) {
+        case ROS_AGENT_CONNECTION_UNINITIALIZED:
+            allocator = rcl_get_default_allocator();
+            printk("allocator initialized!\r\n");
+            micro_ros_state = ROS_AGENT_CONNECTION_DISCONNECTED;
+
+        case ROS_AGENT_CONNECTION_DISCONNECTED:
+            LedController::kill();
+            ServoController::kill();
+            rclc_executor_fini(&executor);
+            rclc_support_fini(&support);
+            if (rclc_support_init(&support, 0, NULL, &allocator) != RCL_RET_OK) {
+                printk("Can't init rcl support!\r\n");
+                k_msleep(1000);
+                break;
+            }
+            if (rcl_clock_init(RCL_ROS_TIME, &clock, &allocator) != RCL_RET_OK) {
+                printf("Failed to initialize clock\n");
+                return;
+            }
+            if (rclc_executor_init(&executor, &support.context, 8, &allocator) != RCL_RET_OK){
+                printk("Can't init executor!\r\n");
+                break;
+            }
+            printk("rcl support initialized!\r\n");
+            micro_ros_state = ROS_AGENT_CONNECTION_SETUP;
+
+        case ROS_AGENT_CONNECTION_SETUP:
+            if (LedController::setup(&support, &executor) != RCL_RET_OK) {
+                printk("LED Controller cant be initialized!\r\n");
+                micro_ros_state = ROS_AGENT_CONNECTION_ERROR;
+                break;
+            }
+            if (ServoController::setup(&support, &executor) != RCL_RET_OK) {
+                printk("Servo Controller cant be initialized!\r\n");
+                micro_ros_state = ROS_AGENT_CONNECTION_ERROR;
+                break;
+            }
+            micro_ros_state = ROS_AGENT_CONNECTION_CONNECTED;
+
+        case ROS_AGENT_CONNECTION_UNKNOWN:
+            if (rmw_uros_ping_agent(150, 3) != RCL_RET_OK) {
+                micro_ros_state = ROS_AGENT_CONNECTION_DISCONNECTED;
+                printk("micro ros agent not reachable!\r\n");
+                break;
+            }
+            micro_ros_state = ROS_AGENT_CONNECTION_CONNECTED;
+            printk("micro ros agent reachable!\r\n");
+
+        case ROS_AGENT_CONNECTION_CONNECTED:
+            if (rclc_executor_spin_some(&executor, 100) != RCL_RET_OK) {
+                micro_ros_state = ROS_AGENT_CONNECTION_UNKNOWN;
+                printk("micro ros spin error!\r\n");
+                break;
+            }
+            if (k_uptime_get() % 20 == 0) {
+                if (ServoController::publish() != RCL_RET_OK) {
+                    micro_ros_state = ROS_AGENT_CONNECTION_UNKNOWN;
+                    printk("micro ros servo publish error!\r\n");
+                    break;
+                }
+            }
+            k_sleep(K_MSEC(1));
+            break;
+
+        case ROS_AGENT_CONNECTION_ERROR:
+        default:
+            micro_ros_state = ROS_AGENT_CONNECTION_ERROR;
+    }
+}

src/microros/microros_state_machine.hpp

@@ -0,0 +1,3 @@
+#pragma once
+
+void micro_ros_state_machine(void);

src/microros/publishers/CMakeLists.txt

@@ -1 +0,0 @@
-cmake_minimum_required(VERSION 3.20.0)

src/microros/rcl_util.hpp

@@ -0,0 +1,6 @@
+#include <rcl/error_handling.h>
+
+#define RCL_RETURN_ON_ERROR(fn_call) do { \
+    rcl_ret_t rc = (fn_call);             \
+    if (rc != RCL_RET_OK) return rc;      \
+} while(0)

src/microros/subscribers/CMakeLists.txt

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

src/microros/subscribers/led0/CMakeLists.txt

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

src/microros/subscribers/led0/led0_subscriber.cpp

@@ -1,26 +0,0 @@
-#include "led0_subscriber.hpp"
-
-#include <zephyr/drivers/gpio.h>
-#include <std_msgs/msg/bool.h>
-
-#include "devices.hpp"
-
-
-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);
-}
-
-
-void led0_register(MicrorosExecutor* node) {
-    node->add_subscription(
-        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
-        "/nucleo/led0",
-        &led0_msg,
-        led0_callback
-    );
-}

src/microros/subscribers/led0/led0_subscriber.hpp

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

src/microros/subscribers/led1/CMakeLists.txt

@@ -1,7 +0,0 @@
-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

@@ -1,26 +0,0 @@
-#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

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

src/microros/subscribers/led2/CMakeLists.txt

@@ -1,7 +0,0 @@
-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

@@ -1,26 +0,0 @@
-#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

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

src/microros/subscribers/servo0/CMakeLists.txt

@@ -1,7 +0,0 @@
-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

@@ -1,47 +0,0 @@
-#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

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

src/servo_controller/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+target_include_directories(app PRIVATE ./)
+
+#add_subdirectory()
+
+target_sources(app PRIVATE
+    servo_controller.cpp
+)

src/servo_controller/servo_controller.cpp

@@ -0,0 +1,162 @@
+#include "servo_controller.hpp"
+#include "devices.hpp"
+#include "rcl_util.hpp"
+#include "microros_clock.hpp"
+
+#include <std_msgs/msg/float32.h>
+#include <geometry_msgs/msg/transform_stamped.h>
+#include <zephyr/drivers/counter.h>
+#include <zephyr/kernel.h>
+
+const uint32_t min_pulse_ns = 500000;
+const uint32_t max_pulse_ns = 2500000;
+
+rcl_node_t ServoController::node;
+rcl_subscription_t ServoController::servo0_subscription;
+rcl_publisher_t ServoController::servo0_publisher;
+float ServoController::current_angle = 90.0f;
+float ServoController::target_angle = 90.0f;
+uint32_t ServoController::last_step = 0;
+float ServoController::current_velocity = 0.0f;
+float ServoController::last_error = 0.0f;
+float ServoController::integral = 0.0f;
+
+struct counter_alarm_cfg alarm_cfg;
+
+std_msgs__msg__Float32 servo_target_msg;
+
+void servo0_set_target_callback(const void *msg){
+    ServoController::target_angle = (float)((std_msgs__msg__Float32 *)msg)->data;
+}
+
+void servo_pid_work_cb(struct k_work *work)
+{
+    ServoController::servo_pid();
+}
+K_WORK_DEFINE(servo_pid_work, servo_pid_work_cb);
+
+
+void counter_cb(const struct device *dev, uint8_t chan_id, uint32_t ticks, void *user_data)
+{
+    k_work_submit(&servo_pid_work);
+    counter_set_channel_alarm(dev, chan_id, &alarm_cfg);
+}
+
+
+rcl_ret_t ServoController::setup(rclc_support_t* support, rclc_executor_t* executor)
+{
+    RCL_RETURN_ON_ERROR(rclc_node_init_default(
+        &ServoController::node, 
+        "servo_controller",
+        "",
+        support));
+
+    RCL_RETURN_ON_ERROR(rclc_publisher_init_default(
+        &ServoController::servo0_publisher,                          
+        &ServoController::node,                                     
+        ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TransformStamped),
+        "/tf"                             
+    ));
+
+    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
+        &ServoController::servo0_subscription,
+        &ServoController::node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
+        "/nucleo/servo0/target"));
+
+    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
+        executor,
+        &ServoController::servo0_subscription,
+        &servo_target_msg,
+        servo0_set_target_callback,
+        ON_NEW_DATA
+    ));
+
+
+    alarm_cfg = {
+        .callback = counter_cb,
+        .ticks = counter_us_to_ticks(counter5, 2000), // 2 ms = 500 Hz
+        .flags = 0,
+    };
+    
+    counter_set_channel_alarm(counter5, 0, &alarm_cfg);
+    counter_start(counter5);
+
+    return RCL_RET_OK;
+}
+
+
+void ServoController::servo_pid() {
+    uint32_t now = k_cycle_get_32();
+
+    // wait for sync
+    if (last_step == 0) {
+        last_step = now; 
+        return; 
+    }
+
+    // calc err & dt
+    float error = target_angle - current_angle;
+    float dt = (float)(now - last_step) / (float)CONFIG_SYS_CLOCK_HW_CYCLES_PER_SEC;
+
+    // pid
+    float derivative = (error - last_error) / dt;
+    integral += error * dt;
+    float pid_vel = kp * error + ki * integral + kd * derivative;
+
+    // clamp velocity
+    float target_velocity = fmaxf(-max_velocity, fminf(max_velocity, pid_vel));
+
+    // clamp acceleration
+    float max_dv = max_accel * dt;
+    current_velocity += fmaxf(-max_dv, fminf(max_dv, target_velocity - current_velocity));
+
+    // Update angle
+    current_angle += current_velocity * dt;
+    current_angle = fmaxf(0.0f, fminf(180.0f, current_angle));
+
+    last_error = error;
+    last_step = now;
+
+    uint32_t pulse_ns = min_pulse_ns + (current_angle / 180.0f) * (max_pulse_ns - min_pulse_ns);
+    int ret = pwm_set_pulse_dt(&servo0, pulse_ns);
+}
+
+rcl_ret_t ServoController::publish() {
+    geometry_msgs__msg__TransformStamped tf;  
+
+    int64_t now_ns = 0;
+    rcl_clock_get_now(&clock, &now_ns);
+    tf.header.stamp.sec     = now_ns / 1000000000ULL;
+    tf.header.stamp.nanosec = now_ns % 1000000000ULL;
+
+    // Fill header
+    char frame_id_buf[32] = "livox_base";
+    tf.header.frame_id.data = frame_id_buf;
+    tf.header.frame_id.size = strlen(frame_id_buf);
+    tf.header.frame_id.capacity = sizeof(frame_id_buf);
+
+    char child_id_buf[32] = "frame_default";
+    tf.child_frame_id.data = child_id_buf;
+    tf.child_frame_id.size = strlen(child_id_buf);
+    tf.child_frame_id.capacity = sizeof(child_id_buf);
+
+    // Rotation around Z
+    float yaw = (current_angle - 90.0f) * 3.141592653589793f / 180.0f;
+    tf.transform.translation.x = 0.0;
+    tf.transform.translation.y = 0.0;
+    tf.transform.translation.z = 0.0;
+
+    tf.transform.rotation.x = 0.0f;
+    tf.transform.rotation.y = 0.0f;
+    tf.transform.rotation.z = sinf(yaw / 2.0f);
+    tf.transform.rotation.w = cosf(yaw / 2.0f);
+
+    return rcl_publish(&ServoController::servo0_publisher, &tf, NULL);
+}
+
+void ServoController::kill() {
+    rcl_publisher_fini(&ServoController::servo0_publisher, &ServoController::node);
+    rcl_subscription_fini(&ServoController::servo0_subscription, &ServoController::node);
+    rcl_node_fini(&ServoController::node);
+}

src/servo_controller/servo_controller.hpp

@@ -0,0 +1,40 @@
+#pragma once
+#include <zephyr/types.h>
+#include <rclc/executor.h>
+#include <rcl/rcl.h>
+#include <rclc/rclc.h>
+#include <rosidl_runtime_c/message_type_support_struct.h>
+
+class ServoController {
+public:
+    static rcl_ret_t setup(rclc_support_t* support, rclc_executor_t* executor);
+    static void servo_pid();
+    static rcl_ret_t publish();
+    static void kill();
+
+    // angles
+    static float current_angle;
+    static float target_angle;
+
+private:
+    // ros
+    static rcl_node_t node;
+    static rcl_subscription_t servo0_subscription;
+    static rcl_publisher_t servo0_publisher;
+
+    // pid states
+    static uint32_t last_step;
+    static float current_velocity;
+    static float last_error;
+    static float integral;
+    static float dt;
+
+    // pid coefs
+    static constexpr float kp = 2.0f;
+    static constexpr float ki = 0.0f;
+    static constexpr float kd = 0.05f;
+
+    // Limits
+    static constexpr float max_velocity = 50.0f;  // deg/s
+    static constexpr float max_accel = 100.0f;     // deg/s^2
+};

src/usb/CMakeLists.txt

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

src/usb/usb_cdc_acm.cpp

@@ -0,0 +1,38 @@
+#include <zephyr/usb/usbd.h>
+
+#include "usb_cdc_acm.hpp"
+
+USBD_DEVICE_DEFINE(usb0, DEVICE_DT_GET(DT_NODELABEL(zephyr_udc0)), 0x2fe3, 0xffff);
+USBD_DESC_LANG_DEFINE(usb0_lang);
+USBD_DESC_MANUFACTURER_DEFINE(usb0_mfr, "Ohmman & Co. KG");
+USBD_DESC_PRODUCT_DEFINE(usb0_prod, "Microros Servo Node");
+
+USBD_DESC_CONFIG_DEFINE(fs_cfg_desc, "FS Configuration");
+USBD_CONFIGURATION_DEFINE(usb0_conf, USB_SCD_SELF_POWERED, 100, &fs_cfg_desc);
+
+static const char *const usb0_blocklist[] = {
+	"dfu_dfu",
+	NULL,
+};
+
+void usb_init(void) {
+    usbd_add_descriptor(&usb0, &usb0_lang);
+    usbd_add_descriptor(&usb0, &usb0_mfr);
+    usbd_add_descriptor(&usb0, &usb0_prod);
+    //usbd_add_descriptor(&usb0, &usb0_sn);
+
+    if (usbd_add_configuration(&usb0, USBD_SPEED_FS, &usb0_conf)) {
+		printk("Failed to add Full-Speed configuration\r\n");
+		return;
+	}
+
+    if (usbd_register_all_classes(&usb0, USBD_SPEED_FS, 1, usb0_blocklist)) {
+        printk("Failed to register usb classes\r\n");
+        return;
+    }
+
+	usbd_device_set_code_triple(&usb0, USBD_SPEED_FS, USB_BCC_APPLICATION, 0x02, 0x01);
+
+    usbd_init(&usb0);
+    usbd_enable(&usb0);
+}

src/usb/usb_cdc_acm.hpp

@@ -0,0 +1,3 @@
+#pragma once
+
+void usb_init(void);