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

working implementation

Browse Source
This commit is contained in:
Timo Schneider
2025-08-22 16:20:41 +02:00
parent ef5035b92b
commit 0bb1b5d026
26 changed files with 1104 additions and 257 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/emulation/CMakeLists.txt
View File

@@ -1,3 +0,0 @@
target_include_directories(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
target_sources(app PRIVATE gpio_visualizer.cpp)

89
src/emulation/gpio_visualizer.cpp
View File

@@ -1,89 +0,0 @@
#include "gpio_visualizer.hpp"
#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/sys/printk.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio/gpio_emul.h>
#include <zephyr/drivers/display.h>
#define GPIO_VISUALIZER_STACK_SIZE 64 * 1024 * 1024 // 64 MB
#define GPIO_VISUALIZER_PRIORITY 10
#define NR_OF_GPIO_PORTS 9
#define NR_OF_GPIO_PINS_PER_PORT 16
#define DISPLAY_WIDTH 320
#define DISPLAY_HEIGHT 240
#define DISPLAY_COLOR_DEPTH 4
#define GPIO_PIXEL_SIZE 20
K_THREAD_STACK_DEFINE(gpio_visualizer__thread_stack, GPIO_VISUALIZER_STACK_SIZE);
struct k_thread gpio_visualizer_thread_data;
const struct device *gpioa = device_get_binding("GPIOA");
const struct device *gpiob = device_get_binding("GPIOB");
const struct device *gpioc = device_get_binding("GPIOC");
const struct device *gpiod = device_get_binding("GPIOD");
const struct device *gpioe = device_get_binding("GPIOE");
const struct device *gpiof = device_get_binding("GPIOF");
const struct device *gpiog = device_get_binding("GPIOG");
const struct device *gpioh = device_get_binding("GPIOH");
const struct device *gpioi = device_get_binding("GPIOI");
void draw_led(uint8_t port, uint8_t pin, uint8_t state, uint8_t *framebuf) {
uint32_t x_base = GPIO_PIXEL_SIZE * pin;
uint32_t y_base = GPIO_PIXEL_SIZE * port;
for (uint32_t i = 0; i < GPIO_PIXEL_SIZE * GPIO_PIXEL_SIZE; i++) {
uint32_t x = x_base + (i % GPIO_PIXEL_SIZE);
uint32_t y = y_base + (i / GPIO_PIXEL_SIZE);
framebuf[x * DISPLAY_COLOR_DEPTH + 2 + DISPLAY_WIDTH * DISPLAY_COLOR_DEPTH * y] = state * 0xFF;
}
}
void gpio_visualizer_thread(void *ARG1, void *ARG2, void *ARG3) {
const struct device* gpio_ports[NR_OF_GPIO_PORTS] = {gpioa, gpiob, gpioc, gpiod, gpioe, gpiof, gpiog, gpioh, gpioi};
const struct device *gpio_display = device_get_binding("GPIO DISPLAY");
struct display_buffer_descriptor desc;
uint8_t buf[DISPLAY_WIDTH * DISPLAY_HEIGHT * DISPLAY_COLOR_DEPTH] = {0};
desc.buf_size = sizeof(buf);
desc.width = DISPLAY_WIDTH;
desc.height = DISPLAY_HEIGHT;
desc.pitch = DISPLAY_WIDTH;
for (uint32_t i = 0; i < desc.buf_size; i++) {
if (i % DISPLAY_COLOR_DEPTH == DISPLAY_COLOR_DEPTH - 1) {
buf[i] = 0xFF;
}
}
display_set_contrast(gpio_display, 100);
display_blanking_off(gpio_display);
int running = 1;
while (running) {
for (uint8_t port_idx = 0; port_idx < NR_OF_GPIO_PORTS; port_idx++) {
for (uint8_t pin_idx = 0; pin_idx < NR_OF_GPIO_PINS_PER_PORT; pin_idx++) {
uint8_t state = gpio_emul_output_get(gpio_ports[port_idx], pin_idx);
draw_led(port_idx, pin_idx, state, buf);
}
}
display_write(gpio_display, 0, 0, &desc, buf);
k_sleep(K_MSEC(16));
}
}
void spawn_gpio_visualizer_thread(void) {
k_thread_create(&gpio_visualizer_thread_data, gpio_visualizer__thread_stack,
GPIO_VISUALIZER_STACK_SIZE,
gpio_visualizer_thread,
NULL, NULL, NULL,
GPIO_VISUALIZER_PRIORITY, 0, K_NO_WAIT);
}

1
src/emulation/gpio_visualizer.hpp
View File

@@ -1 +0,0 @@
void spawn_gpio_visualizer_thread(void);

111
src/main.cpp
View File

@@ -1,32 +1,95 @@
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <uxr/client/config.h>
#include <uxr/client/client.h>
#include <zephyr/kernel.h>
#include <version.h>
// IP and port of the PC running XRCE-DDS agent
#define AGENT_IP "192.168.1.100"
#define AGENT_PORT 8888
#if ZEPHYR_VERSION_CODE >= ZEPHYR_VERSION(3,1,0)
#include <zephyr/kernel.h>
#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 <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>
#include <microros_transports.h>
#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;
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
RCLC_UNUSED(last_call_time);
if (timer != NULL) {
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
printk("msg: %d", msg.data++);
}
}
int main(void)
{
// Initialize micro-ROS support for Zephyr UDP transport
rmw_uros_set_custom_transport(
true, // enable
NULL, // user data (none)
zephyr_transport_open,
zephyr_transport_close,
zephyr_transport_write,
zephyr_transport_read);
rmw_uros_set_custom_transport(
MICRO_ROS_FRAMING_REQUIRED,
(void *) &default_params,
zephyr_transport_open,
zephyr_transport_close,
zephyr_transport_write,
zephyr_transport_read
);
// Initialize rcl context, node, publisher, subscriber, etc...
rcl_allocator_t allocator = rcl_get_default_allocator();
rclc_support_t support;
// Your app code here
// create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
while (1) {
// Spin or sleep
k_sleep(K_MSEC(100));
}
// create node
rcl_node_t node;
RCCHECK(rclc_node_init_default(&node, "zephyr_int32_publisher", "", &support));
return 0;
}
// create publisher
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"zephyr_int32_publisher"));
// 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 executor
rclc_executor_t executor;
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
msg.data = 0;
while(1){
rclc_executor_spin_some(&executor, 100);
usleep(100000);
}
// free resources
RCCHECK(rcl_publisher_fini(&publisher, &node))
RCCHECK(rcl_node_fini(&node))
return 0;
}