init

.gitignore

@@ -0,0 +1,3 @@
+.debug
+.vscode
+build

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "modules/micro_ros_zephyr_module"]
+	path = modules/micro_ros_zephyr_module
+	url = https://github.com/micro-ROS/micro_ros_zephyr_module.git

CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.20.0)
+
+project(sherpa_ros2_sensor_node)
+
+list(APPEND ZEPHYR_EXTRA_MODULES ${CMAKE_CURRENT_SOURCE_DIR}/modules/micro_ros_zephyr_module/modules/libmicroros)
+find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
+
+target_sources(app PRIVATE src/main.cpp)
+
+if(${BOARD} STREQUAL "native_sim")
+  add_subdirectory(src/emulation)
+endif()

boards/native_sim.conf

@@ -0,0 +1,12 @@
+# gpio emulation
+CONFIG_GPIO_EMUL=y
+CONFIG_EMUL=y
+
+# network settings
+CONFIG_ETH_NATIVE_POSIX=y
+CONFIG_ETH_NATIVE_POSIX_DRV_NAME="eth0"
+
+CONFIG_NET_CONFIG_MY_IPV4_ADDR="10.43.30.100"
+CONFIG_MICROROS_AGENT_IP="10.43.30.100"
+CONFIG_NET_CONFIG_MY_IPV4_NETMASK="255.255.255.0"
+CONFIG_NET_CONFIG_MY_IPV4_GW="10.43.30.1"

boards/native_sim.overlay

@@ -0,0 +1,97 @@
+/ {
+
+    aliases {
+        led0 = &user_led;
+    };
+
+    chosen {
+        zephyr,display = &gpio_visualizer_display;
+    };
+
+    leds {
+        compatible = "gpio-leds";
+        user_led: user_led_0 {
+            gpios = <&gpioa_emul 0 GPIO_ACTIVE_HIGH>;
+            label = "LED 0";
+        };
+    };
+
+    gpioa_emul: gpioa {
+        compatible = "zephyr,gpio-emul";
+        gpio-controller;
+        #gpio-cells = <2>;
+        ngpios = <16>;
+        label = "GPIOA";
+    };
+
+    gpiob_emul: gpiob {
+        compatible = "zephyr,gpio-emul";
+        gpio-controller;
+        #gpio-cells = <2>;
+        ngpios = <16>;
+        label = "GPIOB";
+    };
+
+    gpioc_emul: gpioc {
+        compatible = "zephyr,gpio-emul";
+        gpio-controller;
+        #gpio-cells = <2>;
+        ngpios = <16>;
+        label = "GPIOC";
+    };
+
+    gpiod_emul: gpiod {
+        compatible = "zephyr,gpio-emul";
+        gpio-controller;
+        #gpio-cells = <2>;
+        ngpios = <16>;
+        label = "GPIOD";
+    };
+
+    gpioe_emul: gpioe {
+        compatible = "zephyr,gpio-emul";
+        gpio-controller;
+        #gpio-cells = <2>;
+        ngpios = <16>;
+        label = "GPIOE";
+    };
+
+    gpiof_emul: gpiof {
+        compatible = "zephyr,gpio-emul";
+        gpio-controller;
+        #gpio-cells = <2>;
+        ngpios = <16>;
+        label = "GPIOF";
+    };
+
+    gpiog_emul: gpiog {
+        compatible = "zephyr,gpio-emul";
+        gpio-controller;
+        #gpio-cells = <2>;
+        ngpios = <16>;
+        label = "GPIOG";
+    };
+
+    gpioh_emul: gpioh {
+        compatible = "zephyr,gpio-emul";
+        gpio-controller;
+        #gpio-cells = <2>;
+        ngpios = <16>;
+        label = "GPIOH";
+    };
+
+    gpioi_emul: gpioi {
+        compatible = "zephyr,gpio-emul";
+        gpio-controller;
+        #gpio-cells = <2>;
+        ngpios = <16>;
+        label = "GPIOI";
+    };
+
+    gpio_visualizer_display: sdl_dc {
+		compatible = "zephyr,sdl-dc";
+		height = <240>;
+		width = <320>;
+        label = "GPIO DISPLAY";
+	};
+};

boards/nucleo_h563zi.conf

@@ -0,0 +1,8 @@
+# network settings
+CONFIG_ETH_STM32_HAL=y
+CONFIG_NET_L2_ETHERNET=y
+
+CONFIG_NET_CONFIG_MY_IPV4_ADDR="10.42.30.100"
+CONFIG_MICROROS_AGENT_IP="10.42.30.100"
+CONFIG_NET_CONFIG_MY_IPV4_NETMASK="255.255.255.0"
+CONFIG_NET_CONFIG_MY_IPV4_GW="10.42.30.1"

boards/nucleo_h563zi.overlay

@@ -0,0 +1,13 @@
+/ {
+    aliases {
+        led0 = &user_led;
+    };
+
+    leds {
+        compatible = "gpio-leds";
+        user_led: user_led_0 {
+            gpios = <&gpiob 0 GPIO_ACTIVE_HIGH>;
+            label = "USER_LED_0";
+        };
+    };
+};

config.yml

@@ -0,0 +1,18 @@
+sample:
+  description: ROS2 node
+  name: ros2_node
+common:
+  min_ram: 2
+  min_flash: 16
+  tags: microros
+  integration_platforms:
+    - native_sim
+    - nucleo_h563zi
+  harness: console
+  harness_config:
+    type: one_line
+    regex:
+      - "Hello World! (.*)"
+tests:
+  sample.basic.helloworld:
+    tags: introduction

prj.conf

@@ -0,0 +1,33 @@
+# c version
+CONFIG_CPP=y
+CONFIG_NEWLIB_LIBC=y
+CONFIG_NEWLIB_LIBC_NANO=n
+CONFIG_PICOLIBC=n
+CONFIG_MINIMAL_LIBC=n
+CONFIG_POSIX_API=y
+
+# logging
+CONFIG_LOG=n
+
+# gpio support
+CONFIG_GPIO=y
+
+# display for native_sim
+CONFIG_DISPLAY=y
+CONFIG_SDL_DISPLAY=y
+
+# networking
+CONFIG_NETWORKING=y
+CONFIG_NET_CONFIG_SETTINGS=y
+CONFIG_NET_CONFIG_AUTO_INIT=y
+CONFIG_NET_CONFIG_NEED_IPV4=y
+
+CONFIG_NET_IPV4=y
+CONFIG_NET_IPV6=n
+CONFIG_NET_DHCPV4=n
+CONFIG_NET_UDP=y
+CONFIG_NET_TCP=n
+
+# micro ros
+CONFIG_MICROROS=y
+CONFIG_MICROROS_TRANSPORT_UDP=y

src/emulation/CMakeLists.txt

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

src/emulation/gpio_visualizer.cpp

@@ -0,0 +1,89 @@
+#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);
+}

src/emulation/gpio_visualizer.hpp

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

src/main.cpp

@@ -0,0 +1,25 @@
+#include <zephyr/kernel.h>
+#include <zephyr/device.h>
+#include <zephyr/sys/printk.h>
+#include <zephyr/devicetree.h>
+#include <zephyr/drivers/gpio.h>
+
+#include <rcl/rcl.h>
+
+#if defined(CONFIG_BOARD_NATIVE_SIM)
+#include "gpio_visualizer.hpp"
+#endif
+
+int main(void)
+{ 
+    #if defined(CONFIG_BOARD_NATIVE_SIM)
+    spawn_gpio_visualizer_thread();
+    #endif
+
+	while (1) {
+		printk("Hello World! %s\n", CONFIG_BOARD_TARGET);
+		k_sleep(K_SECONDS(2));
+	}
+
+	return 0;
+}