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

led0

Browse Source
This commit is contained in:
Timo Schneider
2025-08-23 10:27:29 +02:00
parent 0bb1b5d026
commit 6c53932267
4 changed files with 74 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
boards/nucleo_h563zi.overlay
View File

@@ -14,7 +14,7 @@
&uart4 {
status = "okay";
current-speed = <115200>;
current-speed = <921600>;
pinctrl-0 = <&uart4_tx_pa0 &uart4_rx_pd0>;
pinctrl-names = "default";
};
@@ -22,3 +22,16 @@
&usart3 {
status = "okay";
};
&timers1 {
status = "okay";
st,prescaler = <0>; /* Required */
pwm0: pwm {
compatible = "st,stm32-pwm";
status = "okay";
pinctrl-0 = <&tim1_ch1_pa8>;
pinctrl-names = "default";
};
};

4
modules/libmicroros/microros_transports/serial/microros_transports.c
View File

@@ -84,11 +84,13 @@ bool zephyr_transport_close(struct uxrCustomTransport * transport){
size_t zephyr_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err){
zephyr_transport_params_t * params = (zephyr_transport_params_t*) transport->args;
/*
printf("TX: ");
for (size_t i = 0; i < len; i++) {
printf("%02X ", buf[i]);
}
printf("\n");
*/
for (size_t i = 0; i < len; i++)
{
@@ -113,11 +115,13 @@ size_t zephyr_transport_read(struct uxrCustomTransport* transport, uint8_t* buf,
read = ring_buf_get(&in_ringbuf, buf, len);
uart_irq_rx_enable(params->uart_dev);
/*
printf("RX: ");
for (size_t i = 0; i < read; i++) {
printf("%02X ", buf[i]);
}
printf("\n");
*/
return read;
}

7
prj.conf
View File

@@ -9,13 +9,20 @@ CONFIG_POSIX_API=y
# logging
CONFIG_LOG=y
CONFIG_STDOUT_CONSOLE=y
CONFIG_PRINTK=y
CONFIG_LOG_DEFAULT_LEVEL=3
# gpio support
CONFIG_GPIO=y
CONFIG_LED=y
# usb
CONFIG_UART_INTERRUPT_DRIVEN=y
# pwm
CONFIG_PWM=y
CONFIG_PWM_STM32=y
# micro ros
CONFIG_MICROROS=y

101
src/main.cpp
View File

@@ -5,18 +5,16 @@
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/posix/time.h>
#else
#include <zephyr.h>
#include <device.h>
#include <devicetree.h>
#include <drivers/gpio.h>
#include <posix/time.h>
#endif
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/bool.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
@@ -27,69 +25,66 @@
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);for(;;){};}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rcl_subscription_t subscriber;
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
std_msgs__msg__Bool msg;
static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
void led0_callback(const void *msgin)
{
RCLC_UNUSED(last_call_time);
if (timer != NULL) {
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
printk("msg: %d", msg.data++);
}
const std_msgs__msg__Bool *msg = (const std_msgs__msg__Bool *)msgin;
gpio_pin_configure_dt(&led0, msg->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
}
int main(void)
{
rmw_uros_set_custom_transport(
MICRO_ROS_FRAMING_REQUIRED,
(void *) &default_params,
zephyr_transport_open,
zephyr_transport_close,
zephyr_transport_write,
zephyr_transport_read
);
if (!device_is_ready(led0.port)) {
printk("LED not ready\n");
return -1;
}
rcl_allocator_t allocator = rcl_get_default_allocator();
rclc_support_t support;
/* micro-ROS transport setup */
rmw_uros_set_custom_transport(
MICRO_ROS_FRAMING_REQUIRED,
(void *)&default_params,
zephyr_transport_open,
zephyr_transport_close,
zephyr_transport_write,
zephyr_transport_read
);
// create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
rcl_allocator_t allocator = rcl_get_default_allocator();
rclc_support_t support;
// create node
rcl_node_t node;
RCCHECK(rclc_node_init_default(&node, "zephyr_int32_publisher", "", &support));
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// create publisher
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"zephyr_int32_publisher"));
rcl_node_t node;
RCCHECK(rclc_node_init_default(&node, "zephyr_servo_node", "", &support));
// create timer,
rcl_timer_t timer;
const unsigned int timer_timeout = 1000;
RCCHECK(rclc_timer_init_default(
&timer,
&support,
RCL_MS_TO_NS(timer_timeout),
timer_callback));
/* Create subscriber for /servo/position */
RCCHECK(rclc_subscription_init_default(
&subscriber,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
"/nucleo/led0"
));
// create executor
rclc_executor_t executor;
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
/* Create executor */
rclc_executor_t executor;
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &led0_callback, ON_NEW_DATA));
msg.data = 0;
/* Spin executor loop */
while (1) {
rclc_executor_spin_some(&executor, 100);
k_sleep(K_MSEC(10));
}
while(1){
rclc_executor_spin_some(&executor, 100);
usleep(100000);
}
/* Free resources (unreachable in infinite loop) */
RCCHECK(rcl_subscription_fini(&subscriber, &node))
RCCHECK(rcl_node_fini(&node))
// free resources
RCCHECK(rcl_publisher_fini(&publisher, &node))
RCCHECK(rcl_node_fini(&node))
return 0;
return 0;
}