From 52b0dd74c093f75faeecdfbf87cbe31e6eb16e3c Mon Sep 17 00:00:00 2001 From: Timo Date: Fri, 12 Sep 2025 15:45:14 +0200 Subject: [PATCH] wip: onboard pid --- CMakeLists.txt | 3 + boards/nucleo_h563zi.overlay | 24 ++- modules/libmicroros/Kconfig | 5 +- .../serial-usb/microros_transports.c | 44 +---- prj.conf | 23 ++- src/CMakeLists.txt | 3 + src/devices.cpp | 5 +- src/devices.hpp | 3 +- src/led_controller/CMakeLists.txt | 9 + src/led_controller/led_controller.cpp | 92 ++++++++++ src/led_controller/led_controller.hpp | 17 ++ src/main.cpp | 34 +--- src/microros/CMakeLists.txt | 9 +- src/microros/microros_clock.hpp | 4 + src/microros/microros_node.cpp | 113 ------------ src/microros/microros_node.hpp | 40 ----- src/microros/microros_state_machine.cpp | 98 +++++++++++ src/microros/microros_state_machine.hpp | 3 + src/microros/publishers/CMakeLists.txt | 1 - src/microros/rcl_util.hpp | 6 + src/microros/subscribers/CMakeLists.txt | 7 - src/microros/subscribers/led0/CMakeLists.txt | 7 - .../subscribers/led0/led0_subscriber.cpp | 26 --- .../subscribers/led0/led0_subscriber.hpp | 5 - src/microros/subscribers/led1/CMakeLists.txt | 7 - .../subscribers/led1/led1_subscriber.cpp | 26 --- .../subscribers/led1/led1_subscriber.hpp | 5 - src/microros/subscribers/led2/CMakeLists.txt | 7 - .../subscribers/led2/led2_subscriber.cpp | 26 --- .../subscribers/led2/led2_subscriber.hpp | 5 - .../subscribers/servo0/CMakeLists.txt | 7 - .../subscribers/servo0/servo0_subscriber.cpp | 47 ----- .../subscribers/servo0/servo0_subscriber.hpp | 5 - src/servo_controller/CMakeLists.txt | 9 + src/servo_controller/servo_controller.cpp | 162 ++++++++++++++++++ src/servo_controller/servo_controller.hpp | 40 +++++ src/usb/CMakeLists.txt | 7 + src/usb/usb_cdc_acm.cpp | 38 ++++ src/usb/usb_cdc_acm.hpp | 3 + 39 files changed, 552 insertions(+), 423 deletions(-) create mode 100644 src/led_controller/CMakeLists.txt create mode 100644 src/led_controller/led_controller.cpp create mode 100644 src/led_controller/led_controller.hpp create mode 100644 src/microros/microros_clock.hpp delete mode 100644 src/microros/microros_node.cpp delete mode 100644 src/microros/microros_node.hpp create mode 100644 src/microros/microros_state_machine.cpp create mode 100644 src/microros/microros_state_machine.hpp delete mode 100644 src/microros/publishers/CMakeLists.txt create mode 100644 src/microros/rcl_util.hpp delete mode 100644 src/microros/subscribers/CMakeLists.txt delete mode 100644 src/microros/subscribers/led0/CMakeLists.txt delete mode 100644 src/microros/subscribers/led0/led0_subscriber.cpp delete mode 100644 src/microros/subscribers/led0/led0_subscriber.hpp delete mode 100644 src/microros/subscribers/led1/CMakeLists.txt delete mode 100644 src/microros/subscribers/led1/led1_subscriber.cpp delete mode 100644 src/microros/subscribers/led1/led1_subscriber.hpp delete mode 100644 src/microros/subscribers/led2/CMakeLists.txt delete mode 100644 src/microros/subscribers/led2/led2_subscriber.cpp delete mode 100644 src/microros/subscribers/led2/led2_subscriber.hpp delete mode 100644 src/microros/subscribers/servo0/CMakeLists.txt delete mode 100644 src/microros/subscribers/servo0/servo0_subscriber.cpp delete mode 100644 src/microros/subscribers/servo0/servo0_subscriber.hpp create mode 100644 src/servo_controller/CMakeLists.txt create mode 100644 src/servo_controller/servo_controller.cpp create mode 100644 src/servo_controller/servo_controller.hpp create mode 100644 src/usb/CMakeLists.txt create mode 100644 src/usb/usb_cdc_acm.cpp create mode 100644 src/usb/usb_cdc_acm.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b44f908..dba6c3f 100644 --- a/CMakeLists.txt +++ b/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) \ No newline at end of file diff --git a/boards/nucleo_h563zi.overlay b/boards/nucleo_h563zi.overlay index 8a459ba..bf92017 100644 --- a/boards/nucleo_h563zi.overlay +++ b/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"; }; \ No newline at end of file diff --git a/modules/libmicroros/Kconfig b/modules/libmicroros/Kconfig index 18abf59..0cd7b7e 100644 --- a/modules/libmicroros/Kconfig +++ b/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 USB_DEVICE_STACK + select RING_BUFFER 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" diff --git a/modules/libmicroros/microros_transports/serial-usb/microros_transports.c b/modules/libmicroros/microros_transports/serial-usb/microros_transports.c index 289e382..ef2f3c5 100644 --- a/modules/libmicroros/microros_transports/serial-usb/microros_transports.c +++ b/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; - uint32_t baudrate, dtr = 0U; - - - params->uart_dev = device_get_binding("CDC_ACM_0"); + params->uart_dev = DEVICE_DT_GET(DT_NODELABEL(cdc_acm_uart0)); if (!params->uart_dev) { printk("CDC ACM device not found\n"); return false; } - - ret = usb_enable(NULL); - if (ret != 0) { - printk("Failed to enable USB\n"); - return false; + if (!device_is_ready(params->uart_dev)) { + printk("cdc0 not ready!\n"); } 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 */ diff --git a/prj.conf b/prj.conf index f1f0b50..d872671 100644 --- a/prj.conf +++ b/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 \ No newline at end of file +CONFIG_CAN_STM32_FDCAN=y + +# debug +CONFIG_DEBUG_OPTIMIZATIONS=n \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3e6ef70..fe2e221 100644 --- a/src/CMakeLists.txt +++ b/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 diff --git a/src/devices.cpp b/src/devices.cpp index 7ce8dc8..abff511 100644 --- a/src/devices.cpp +++ b/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)) { - printk("CAN FD 1 not ready\n"); + if (!device_is_ready(counter5)) { + printk("Counter 5 not ready\n"); devices_ready = false; } diff --git a/src/devices.hpp b/src/devices.hpp index 5dfddee..7b931bd 100644 --- a/src/devices.hpp +++ b/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)); - -extern const struct device *fdcan1; +static const struct device *counter5 = DEVICE_DT_GET(DT_NODELABEL(counter5)); bool verify_are_devices_available(void); \ No newline at end of file diff --git a/src/led_controller/CMakeLists.txt b/src/led_controller/CMakeLists.txt new file mode 100644 index 0000000..7802d36 --- /dev/null +++ b/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 +) \ No newline at end of file diff --git a/src/led_controller/led_controller.cpp b/src/led_controller/led_controller.cpp new file mode 100644 index 0000000..066da3d --- /dev/null +++ b/src/led_controller/led_controller.cpp @@ -0,0 +1,92 @@ +#include "led_controller.hpp" +#include "devices.hpp" +#include "rcl_util.hpp" + +#include +#include +#include +#include +#include + +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); +} \ No newline at end of file diff --git a/src/led_controller/led_controller.hpp b/src/led_controller/led_controller.hpp new file mode 100644 index 0000000..a0c944a --- /dev/null +++ b/src/led_controller/led_controller.hpp @@ -0,0 +1,17 @@ +#pragma once +#include +#include +#include +#include + +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; +}; diff --git a/src/main.cpp b/src/main.cpp index fbb916a..8e8c8c7 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,12 +10,9 @@ #include #include "devices.hpp" -#include "microros_node.hpp" - -#include "led0_subscriber.hpp" -#include "led1_subscriber.hpp" -#include "led2_subscriber.hpp" -#include "servo0_subscriber.hpp" +#include "usb_cdc_acm.hpp" +#include "servo_controller.hpp" +#include "microros_state_machine.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"); - - 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(); - + while (1) { + micro_ros_state_machine(); + } + return 0; } diff --git a/src/microros/CMakeLists.txt b/src/microros/CMakeLists.txt index 58bce52..982dfbb 100644 --- a/src/microros/CMakeLists.txt +++ b/src/microros/CMakeLists.txt @@ -1,10 +1,9 @@ cmake_minimum_required(VERSION 3.20.0) -add_subdirectory(publishers) -add_subdirectory(subscribers) +target_include_directories(app PRIVATE ./) -target_include_directories(app PRIVATE .) +#add_subdirectory() -target_sources(app PRIVATE - microros_node.cpp +target_sources(app PRIVATE + microros_state_machine.cpp ) \ No newline at end of file diff --git a/src/microros/microros_clock.hpp b/src/microros/microros_clock.hpp new file mode 100644 index 0000000..b0be45e --- /dev/null +++ b/src/microros/microros_clock.hpp @@ -0,0 +1,4 @@ +#pragma once +#include + +static rcl_clock_t clock; \ No newline at end of file diff --git a/src/microros/microros_node.cpp b/src/microros/microros_node.cpp deleted file mode 100644 index d36b437..0000000 --- a/src/microros/microros_node.cpp +++ /dev/null @@ -1,113 +0,0 @@ -#include "microros_node.hpp" - -#include -#include -#include -#include -#include -#include // 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; -} diff --git a/src/microros/microros_node.hpp b/src/microros/microros_node.hpp deleted file mode 100644 index 1cf9ee3..0000000 --- a/src/microros/microros_node.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -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); -}; diff --git a/src/microros/microros_state_machine.cpp b/src/microros/microros_state_machine.cpp new file mode 100644 index 0000000..5f4423c --- /dev/null +++ b/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 +#include +#include +#include +#include + + +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; + } +} diff --git a/src/microros/microros_state_machine.hpp b/src/microros/microros_state_machine.hpp new file mode 100644 index 0000000..8e3644b --- /dev/null +++ b/src/microros/microros_state_machine.hpp @@ -0,0 +1,3 @@ +#pragma once + +void micro_ros_state_machine(void); \ No newline at end of file diff --git a/src/microros/publishers/CMakeLists.txt b/src/microros/publishers/CMakeLists.txt deleted file mode 100644 index d3b7c32..0000000 --- a/src/microros/publishers/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -cmake_minimum_required(VERSION 3.20.0) diff --git a/src/microros/rcl_util.hpp b/src/microros/rcl_util.hpp new file mode 100644 index 0000000..2c2faa5 --- /dev/null +++ b/src/microros/rcl_util.hpp @@ -0,0 +1,6 @@ +#include + +#define RCL_RETURN_ON_ERROR(fn_call) do { \ + rcl_ret_t rc = (fn_call); \ + if (rc != RCL_RET_OK) return rc; \ +} while(0) \ No newline at end of file diff --git a/src/microros/subscribers/CMakeLists.txt b/src/microros/subscribers/CMakeLists.txt deleted file mode 100644 index a00dcf3..0000000 --- a/src/microros/subscribers/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.20.0) - -add_subdirectory(led0) -add_subdirectory(led1) -add_subdirectory(led2) - -add_subdirectory(servo0) diff --git a/src/microros/subscribers/led0/CMakeLists.txt b/src/microros/subscribers/led0/CMakeLists.txt deleted file mode 100644 index 1d0de19..0000000 --- a/src/microros/subscribers/led0/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.20.0) - -target_include_directories(app PRIVATE .) - -target_sources(app PRIVATE - led0_subscriber.cpp -) \ No newline at end of file diff --git a/src/microros/subscribers/led0/led0_subscriber.cpp b/src/microros/subscribers/led0/led0_subscriber.cpp deleted file mode 100644 index 1b8ea10..0000000 --- a/src/microros/subscribers/led0/led0_subscriber.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "led0_subscriber.hpp" - -#include -#include - -#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 - ); -} diff --git a/src/microros/subscribers/led0/led0_subscriber.hpp b/src/microros/subscribers/led0/led0_subscriber.hpp deleted file mode 100644 index 6123c58..0000000 --- a/src/microros/subscribers/led0/led0_subscriber.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#include "microros_node.hpp" - -void led0_register(MicrorosExecutor* node); diff --git a/src/microros/subscribers/led1/CMakeLists.txt b/src/microros/subscribers/led1/CMakeLists.txt deleted file mode 100644 index b613866..0000000 --- a/src/microros/subscribers/led1/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.20.0) - -target_include_directories(app PRIVATE .) - -target_sources(app PRIVATE - led1_subscriber.cpp -) \ No newline at end of file diff --git a/src/microros/subscribers/led1/led1_subscriber.cpp b/src/microros/subscribers/led1/led1_subscriber.cpp deleted file mode 100644 index 97dabba..0000000 --- a/src/microros/subscribers/led1/led1_subscriber.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "led1_subscriber.hpp" - -#include -#include - -#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 - ); -} diff --git a/src/microros/subscribers/led1/led1_subscriber.hpp b/src/microros/subscribers/led1/led1_subscriber.hpp deleted file mode 100644 index dd583d7..0000000 --- a/src/microros/subscribers/led1/led1_subscriber.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#include "microros_node.hpp" - -void led1_register(MicrorosExecutor* node); diff --git a/src/microros/subscribers/led2/CMakeLists.txt b/src/microros/subscribers/led2/CMakeLists.txt deleted file mode 100644 index 69eaef0..0000000 --- a/src/microros/subscribers/led2/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.20.0) - -target_include_directories(app PRIVATE .) - -target_sources(app PRIVATE - led2_subscriber.cpp -) \ No newline at end of file diff --git a/src/microros/subscribers/led2/led2_subscriber.cpp b/src/microros/subscribers/led2/led2_subscriber.cpp deleted file mode 100644 index 808dcc2..0000000 --- a/src/microros/subscribers/led2/led2_subscriber.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "led2_subscriber.hpp" - -#include -#include - -#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 - ); -} diff --git a/src/microros/subscribers/led2/led2_subscriber.hpp b/src/microros/subscribers/led2/led2_subscriber.hpp deleted file mode 100644 index 4e61633..0000000 --- a/src/microros/subscribers/led2/led2_subscriber.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#include "microros_node.hpp" - -void led2_register(MicrorosExecutor* node); diff --git a/src/microros/subscribers/servo0/CMakeLists.txt b/src/microros/subscribers/servo0/CMakeLists.txt deleted file mode 100644 index 27d91b5..0000000 --- a/src/microros/subscribers/servo0/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.20.0) - -target_include_directories(app PRIVATE .) - -target_sources(app PRIVATE - servo0_subscriber.cpp -) \ No newline at end of file diff --git a/src/microros/subscribers/servo0/servo0_subscriber.cpp b/src/microros/subscribers/servo0/servo0_subscriber.cpp deleted file mode 100644 index 0e18766..0000000 --- a/src/microros/subscribers/servo0/servo0_subscriber.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "servo0_subscriber.hpp" - -#include -#include - -#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 - ); -} diff --git a/src/microros/subscribers/servo0/servo0_subscriber.hpp b/src/microros/subscribers/servo0/servo0_subscriber.hpp deleted file mode 100644 index 4c00dd5..0000000 --- a/src/microros/subscribers/servo0/servo0_subscriber.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#include "microros_node.hpp" - -void servo0_register(MicrorosExecutor* node); diff --git a/src/servo_controller/CMakeLists.txt b/src/servo_controller/CMakeLists.txt new file mode 100644 index 0000000..0d7278e --- /dev/null +++ b/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 +) \ No newline at end of file diff --git a/src/servo_controller/servo_controller.cpp b/src/servo_controller/servo_controller.cpp new file mode 100644 index 0000000..74bb0dd --- /dev/null +++ b/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 +#include +#include +#include + +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); +} \ No newline at end of file diff --git a/src/servo_controller/servo_controller.hpp b/src/servo_controller/servo_controller.hpp new file mode 100644 index 0000000..7efe93c --- /dev/null +++ b/src/servo_controller/servo_controller.hpp @@ -0,0 +1,40 @@ +#pragma once +#include +#include +#include +#include +#include + +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 +}; diff --git a/src/usb/CMakeLists.txt b/src/usb/CMakeLists.txt new file mode 100644 index 0000000..600c47a --- /dev/null +++ b/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 +) \ No newline at end of file diff --git a/src/usb/usb_cdc_acm.cpp b/src/usb/usb_cdc_acm.cpp new file mode 100644 index 0000000..7045d98 --- /dev/null +++ b/src/usb/usb_cdc_acm.cpp @@ -0,0 +1,38 @@ +#include + +#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); +} \ No newline at end of file diff --git a/src/usb/usb_cdc_acm.hpp b/src/usb/usb_cdc_acm.hpp new file mode 100644 index 0000000..f7c8fa0 --- /dev/null +++ b/src/usb/usb_cdc_acm.hpp @@ -0,0 +1,3 @@ +#pragma once + +void usb_init(void); \ No newline at end of file