SHERPA/microros_turntable_controller
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip: onboard pid

Browse Source
This commit is contained in:
Timo Schneider
2025-09-12 15:45:14 +02:00
parent fb9fb9bdf0
commit 52b0dd74c0
39 changed files with 552 additions and 423 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@@ -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)

24
boards/nucleo_h563zi.overlay
View File

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

5
modules/libmicroros/Kconfig
View File

@@ -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"

44
modules/libmicroros/microros_transports/serial-usb/microros_transports.c
View File

@@ -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 */

23
prj.conf
View File

@@ -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

3
src/CMakeLists.txt
View File

@@ -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

5
src/devices.cpp
View File

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

3
src/devices.hpp
View File

@@ -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);

9
src/led_controller/CMakeLists.txt Normal file
View File

@@ -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
)

92
src/led_controller/led_controller.cpp Normal file
View File

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

17
src/led_controller/led_controller.hpp Normal file
View File

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

34
src/main.cpp
View File

@@ -10,12 +10,9 @@
#include <rmw_microros/rmw_microros.h>
#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;
}

9
src/microros/CMakeLists.txt
View File

@@ -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
)

4
src/microros/microros_clock.hpp Normal file
View File

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

113
src/microros/microros_node.cpp
View File

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

40
src/microros/microros_node.hpp
View File

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

98
src/microros/microros_state_machine.cpp Normal file
View File

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

3
src/microros/microros_state_machine.hpp Normal file
View File

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

1
src/microros/publishers/CMakeLists.txt
View File

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

6
src/microros/rcl_util.hpp Normal file
View File

@@ -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)

7
src/microros/subscribers/CMakeLists.txt
View File

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

7
src/microros/subscribers/led0/CMakeLists.txt
View File

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

26
src/microros/subscribers/led0/led0_subscriber.cpp
View File

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

5
src/microros/subscribers/led0/led0_subscriber.hpp
View File

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

7
src/microros/subscribers/led1/CMakeLists.txt
View File

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

26
src/microros/subscribers/led1/led1_subscriber.cpp
View File

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

5
src/microros/subscribers/led1/led1_subscriber.hpp
View File

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

7
src/microros/subscribers/led2/CMakeLists.txt
View File

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

26
src/microros/subscribers/led2/led2_subscriber.cpp
View File

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

5
src/microros/subscribers/led2/led2_subscriber.hpp
View File

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

7
src/microros/subscribers/servo0/CMakeLists.txt
View File

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

47
src/microros/subscribers/servo0/servo0_subscriber.cpp
View File

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

5
src/microros/subscribers/servo0/servo0_subscriber.hpp
View File

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

9
src/servo_controller/CMakeLists.txt Normal file
View File

@@ -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
)

162
src/servo_controller/servo_controller.cpp Normal file
View File

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

40
src/servo_controller/servo_controller.hpp Normal file
View File

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

7
src/usb/CMakeLists.txt Normal file
View File

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

38
src/usb/usb_cdc_acm.cpp Normal file
View File

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

3
src/usb/usb_cdc_acm.hpp Normal file
View File

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