diff --git a/.gitignore b/.gitignore index 6725b901b92bf..7f798c415c784 100644 --- a/.gitignore +++ b/.gitignore @@ -9,6 +9,7 @@ *.o *.a !atmel-samd/asf/**/*.a +!ports/espressif/microros-lib/**/*.a *.elf *.bin !*.toml.bin diff --git a/.gitmodules b/.gitmodules index a5226ffafdd52..067dd612b8038 100644 --- a/.gitmodules +++ b/.gitmodules @@ -412,3 +412,6 @@ [submodule "ports/analog/msdk"] path = ports/analog/msdk url = https://github.com/analogdevicesinc/msdk.git +[submodule "ports/espressif/microros-lib"] + path = ports/espressif/microros-lib + url = https://github.com/hierophect/microros-lib.git diff --git a/ports/espressif/Makefile b/ports/espressif/Makefile index bdf0fd53f6876..657c23257c23f 100644 --- a/ports/espressif/Makefile +++ b/ports/espressif/Makefile @@ -436,6 +436,55 @@ CFLAGS += -isystem esp-camera/driver/include CFLAGS += -isystem esp-camera/conversions/include endif +ifneq ($(CIRCUITPY_RCLCPY),0) +CFLAGS += -isystem microros-lib/include +CFLAGS += -isystem microros-lib/include/rcl +CFLAGS += -isystem microros-lib/include/statistics_msgs +CFLAGS += -isystem microros-lib/include/composition_interfaces +CFLAGS += -isystem microros-lib/include/example_interfaces +CFLAGS += -isystem microros-lib/include/rmw_microxrcedds_c +CFLAGS += -isystem microros-lib/include/test_msgs +CFLAGS += -isystem microros-lib/include/std_msgs +CFLAGS += -isystem microros-lib/include/rcutils +CFLAGS += -isystem microros-lib/include/lifecycle_msgs +CFLAGS += -isystem microros-lib/include/rosidl_typesupport_interface +CFLAGS += -isystem microros-lib/include/service_msgs +CFLAGS += -isystem microros-lib/include/visualization_msgs +CFLAGS += -isystem microros-lib/include/rosidl_dynamic_typesupport +CFLAGS += -isystem microros-lib/include/stereo_msgs +CFLAGS += -isystem microros-lib/include/ucdr +CFLAGS += -isystem microros-lib/include/rosidl_typesupport_c +CFLAGS += -isystem microros-lib/include/std_srvs +CFLAGS += -isystem microros-lib/include/rcl_lifecycle +CFLAGS += -isystem microros-lib/include/action_msgs +CFLAGS += -isystem microros-lib/include/micro_ros_utilities +CFLAGS += -isystem microros-lib/include/rcl_action +CFLAGS += -isystem microros-lib/include/rcl_logging_interface +CFLAGS += -isystem microros-lib/include/type_description_interfaces +CFLAGS += -isystem microros-lib/include/nav_msgs +CFLAGS += -isystem microros-lib/include/actionlib_msgs +CFLAGS += -isystem microros-lib/include/rmw +CFLAGS += -isystem microros-lib/include/rclc_parameter +CFLAGS += -isystem microros-lib/include/geometry_msgs +CFLAGS += -isystem microros-lib/include/sensor_msgs +CFLAGS += -isystem microros-lib/include/trajectory_msgs +CFLAGS += -isystem microros-lib/include/shape_msgs +CFLAGS += -isystem microros-lib/include/rosidl_runtime_c +CFLAGS += -isystem microros-lib/include/rclc +CFLAGS += -isystem microros-lib/include/rosgraph_msgs +CFLAGS += -isystem microros-lib/include/rclc_lifecycle +CFLAGS += -isystem microros-lib/include/rcl_interfaces +CFLAGS += -isystem microros-lib/include/diagnostic_msgs +CFLAGS += -isystem microros-lib/include/micro_ros_msgs +CFLAGS += -isystem microros-lib/include/rosidl_typesupport_introspection_c +CFLAGS += -isystem microros-lib/include/uxr +CFLAGS += -isystem microros-lib/include/unique_identifier_msgs +CFLAGS += -isystem microros-lib/include/rosidl_typesupport_microxrcedds_c +CFLAGS += -isystem microros-lib/include/builtin_interfaces +CFLAGS += -isystem microros-lib/include/tracetools +CFLAGS += -isystem microros-lib/include/rmw_microros +endif + ifneq ($(CIRCUITPY_ESPIDF),0) SRC_ESPIDF := \ $(wildcard common-hal/espidf/*.c) \ @@ -674,6 +723,16 @@ ESP_IDF_COMPONENTS_EXPANDED += $(BUILD)/esp-idf/esp-idf/esp-camera/libesp-camera #$(error $(ESP_IDF_COMPONENTS_EXPANDED)) endif +ifneq ($(CIRCUITPY_RCLCPY),0) +ifeq ($(IDF_TARGET),esp32) +BINARY_BLOBS += microros-lib/esp32/libmicroros.a +else ifeq ($(IDF_TARGET),esp32s2) +BINARY_BLOBS+= microros-lib/esp32s2/libmicroros.a +else ifeq ($(IDF_TARGET),esp32s3) +BINARY_BLOBS += microros-lib/esp32s3/libmicroros.a +endif +endif + ifneq ($(VALID_BOARD),) # From esp-idf/components/bootloader/Kconfig.projbuild # BOOTLOADER_OFFSET is determined by chip type, based on the ROM bootloader, and is not changeable. diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/board.c b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/board.c new file mode 100644 index 0000000000000..a3a9eec047145 --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/board.c @@ -0,0 +1,9 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#include "supervisor/board.h" + +// Use the MP_WEAK supervisor/shared/board.c versions of routines not defined here. diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/jtagcfg.cfg b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/jtagcfg.cfg new file mode 100644 index 0000000000000..80e222632d809 --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/jtagcfg.cfg @@ -0,0 +1,12 @@ +# SPDX-License-Identifier: GPL-2.0-or-later +# +# Example OpenOCD configuration file for ESP32-S3 connected via ESP USB Bridge board +# +# For example, OpenOCD can be started for ESP32-S3 debugging on +# +# openocd -f board/esp32s3-bridge.cfg +# + +# Source the ESP32-S3 configuration file +source [find target/esp32s3.cfg] +adapter speed 1000 diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.h b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.h new file mode 100644 index 0000000000000..f5e781ca5b0c7 --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.h @@ -0,0 +1,17 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2019 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#pragma once + +// Micropython setup + +#define MICROPY_HW_BOARD_NAME "ESP32-S3-DevKitC-1-N8R2 (ROS version)" +#define MICROPY_HW_MCU_NAME "ESP32S3" + +#define MICROPY_HW_NEOPIXEL (&pin_GPIO48) + +#define DEFAULT_UART_BUS_RX (&pin_GPIO44) +#define DEFAULT_UART_BUS_TX (&pin_GPIO43) diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.mk b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.mk new file mode 100644 index 0000000000000..336b9f4dd8926 --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/mpconfigboard.mk @@ -0,0 +1,16 @@ +USB_VID = 0x303A +USB_PID = 0x7003 +USB_PRODUCT = "ESP32-S3-DevKitC-1-N8R2 CPY/ROS" +USB_MANUFACTURER = "Espressif" + +IDF_TARGET = esp32s3 + +CIRCUITPY_ESP_FLASH_SIZE = 8MB +CIRCUITPY_ESP_FLASH_MODE = qio +CIRCUITPY_ESP_FLASH_FREQ = 80m + +CIRCUITPY_ESP_PSRAM_SIZE = 2MB +CIRCUITPY_ESP_PSRAM_MODE = qio +CIRCUITPY_ESP_PSRAM_FREQ = 80m + +CIRCUITPY_RCLCPY = 1 diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/pins.c b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/pins.c new file mode 100644 index 0000000000000..3bb64f434d02f --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/pins.c @@ -0,0 +1,55 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/board/__init__.h" + +static const mp_rom_map_elem_t board_module_globals_table[] = { + CIRCUITPYTHON_BOARD_DICT_STANDARD_ITEMS + + { MP_ROM_QSTR(MP_QSTR_IO0), MP_ROM_PTR(&pin_GPIO0) }, + { MP_ROM_QSTR(MP_QSTR_IO1), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_IO2), MP_ROM_PTR(&pin_GPIO2) }, + { MP_ROM_QSTR(MP_QSTR_IO3), MP_ROM_PTR(&pin_GPIO3) }, + { MP_ROM_QSTR(MP_QSTR_IO4), MP_ROM_PTR(&pin_GPIO4) }, + { MP_ROM_QSTR(MP_QSTR_IO5), MP_ROM_PTR(&pin_GPIO5) }, + { MP_ROM_QSTR(MP_QSTR_IO6), MP_ROM_PTR(&pin_GPIO6) }, + { MP_ROM_QSTR(MP_QSTR_IO7), MP_ROM_PTR(&pin_GPIO7) }, + { MP_ROM_QSTR(MP_QSTR_IO8), MP_ROM_PTR(&pin_GPIO8) }, + { MP_ROM_QSTR(MP_QSTR_IO9), MP_ROM_PTR(&pin_GPIO9) }, + { MP_ROM_QSTR(MP_QSTR_IO10), MP_ROM_PTR(&pin_GPIO10) }, + { MP_ROM_QSTR(MP_QSTR_IO11), MP_ROM_PTR(&pin_GPIO11) }, + { MP_ROM_QSTR(MP_QSTR_IO12), MP_ROM_PTR(&pin_GPIO12) }, + { MP_ROM_QSTR(MP_QSTR_IO13), MP_ROM_PTR(&pin_GPIO13) }, + { MP_ROM_QSTR(MP_QSTR_IO14), MP_ROM_PTR(&pin_GPIO14) }, + { MP_ROM_QSTR(MP_QSTR_IO15), MP_ROM_PTR(&pin_GPIO15) }, + { MP_ROM_QSTR(MP_QSTR_IO16), MP_ROM_PTR(&pin_GPIO16) }, + { MP_ROM_QSTR(MP_QSTR_IO17), MP_ROM_PTR(&pin_GPIO17) }, + { MP_ROM_QSTR(MP_QSTR_IO18), MP_ROM_PTR(&pin_GPIO18) }, + { MP_ROM_QSTR(MP_QSTR_IO19), MP_ROM_PTR(&pin_GPIO19) }, + { MP_ROM_QSTR(MP_QSTR_IO20), MP_ROM_PTR(&pin_GPIO20) }, + { MP_ROM_QSTR(MP_QSTR_IO21), MP_ROM_PTR(&pin_GPIO21) }, + { MP_ROM_QSTR(MP_QSTR_IO35), MP_ROM_PTR(&pin_GPIO35) }, + { MP_ROM_QSTR(MP_QSTR_IO36), MP_ROM_PTR(&pin_GPIO36) }, + { MP_ROM_QSTR(MP_QSTR_IO37), MP_ROM_PTR(&pin_GPIO37) }, + { MP_ROM_QSTR(MP_QSTR_IO38), MP_ROM_PTR(&pin_GPIO38) }, + { MP_ROM_QSTR(MP_QSTR_IO39), MP_ROM_PTR(&pin_GPIO39) }, + { MP_ROM_QSTR(MP_QSTR_IO40), MP_ROM_PTR(&pin_GPIO40) }, + { MP_ROM_QSTR(MP_QSTR_IO41), MP_ROM_PTR(&pin_GPIO41) }, + { MP_ROM_QSTR(MP_QSTR_IO42), MP_ROM_PTR(&pin_GPIO42) }, + { MP_ROM_QSTR(MP_QSTR_IO43), MP_ROM_PTR(&pin_GPIO43) }, + { MP_ROM_QSTR(MP_QSTR_IO44), MP_ROM_PTR(&pin_GPIO44) }, + { MP_ROM_QSTR(MP_QSTR_IO45), MP_ROM_PTR(&pin_GPIO45) }, + { MP_ROM_QSTR(MP_QSTR_IO46), MP_ROM_PTR(&pin_GPIO46) }, + { MP_ROM_QSTR(MP_QSTR_IO47), MP_ROM_PTR(&pin_GPIO47) }, + { MP_ROM_QSTR(MP_QSTR_IO48), MP_ROM_PTR(&pin_GPIO48) }, + { MP_ROM_QSTR(MP_QSTR_NEOPIXEL), MP_ROM_PTR(&pin_GPIO48) }, + + { MP_ROM_QSTR(MP_QSTR_TX), MP_ROM_PTR(&pin_GPIO43) }, + { MP_ROM_QSTR(MP_QSTR_RX), MP_ROM_PTR(&pin_GPIO44) }, + + { MP_ROM_QSTR(MP_QSTR_UART), MP_ROM_PTR(&board_uart_obj) }, +}; +MP_DEFINE_CONST_DICT(board_module_globals, board_module_globals_table); diff --git a/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/sdkconfig b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/sdkconfig new file mode 100644 index 0000000000000..bc9c7c3855c2c --- /dev/null +++ b/ports/espressif/boards/espressif_esp32s3_devkitc_1_n8r2_ros/sdkconfig @@ -0,0 +1,6 @@ +CONFIG_MICRO_ROS_ESP_XRCE_DDS_MIDDLEWARE=y +CONFIG_MICRO_ROS_ESP_NETIF_WLAN=y +CONFIG_MICRO_ROS_AGENT_IP="192.168.1.100" +CONFIG_MICRO_ROS_AGENT_PORT="8888" +CONFIG_MICRO_ROS_APP_STACK=16000 +CONFIG_MICRO_ROS_APP_TASK_PRIO=5 \ No newline at end of file diff --git a/ports/espressif/boards/m5stack_cardputer_ros/board.c b/ports/espressif/boards/m5stack_cardputer_ros/board.c new file mode 100644 index 0000000000000..ec6c2b98f0f9d --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/board.c @@ -0,0 +1,95 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#include "mpconfigboard.h" +#include "supervisor/board.h" +#include "supervisor/shared/serial.h" +#include "shared-bindings/busio/SPI.h" +#include "shared-bindings/fourwire/FourWire.h" +#include "shared-bindings/microcontroller/Pin.h" +#include "shared-module/displayio/__init__.h" +#include "shared-module/displayio/mipi_constants.h" +#include "shared-bindings/board/__init__.h" +#include "py/runtime.h" +#include "py/ringbuf.h" +#include "shared/runtime/interrupt_char.h" + +#define DELAY 0x80 + +uint8_t display_init_sequence[] = { + // SWRESET and Delay 140ms + 0x01, 0 | DELAY, 140, + // SLPOUT and Delay 10ms + 0x11, 0 | DELAY, 10, + // COLMOD 65k colors and 16 bit 5-6-5 + 0x3A, 1, 0x55, + // INVON Iiversion on + 0x21, 0, + // NORON normal operation (full update) + 0x13, 0, + // MADCTL columns RTL, page/column reverse order + 0x36, 1, 0x60, + // RAMCTRL color word little endian + 0xB0, 2, 0x00, 0xF8, + // DIPON display on + 0x29, 0, +}; + + +// Overrides the weakly linked function from supervisor/shared/board.c +void board_init(void) { + + busio_spi_obj_t *spi = common_hal_board_create_spi(0); + fourwire_fourwire_obj_t *bus = &allocate_display_bus()->fourwire_bus; + bus->base.type = &fourwire_fourwire_type; + + // see here for inspiration: https://github.com/m5stack/M5GFX/blob/33d7d3135e816a86a008fae8ab3757938cee95d2/src/M5GFX.cpp#L1350 + common_hal_fourwire_fourwire_construct( + bus, + spi, + &pin_GPIO34, // DC + &pin_GPIO37, // CS + &pin_GPIO33, // RST + 40000000, // baudrate + 0, // polarity + 0 // phase + ); + busdisplay_busdisplay_obj_t *display = &allocate_display()->display; + display->base.type = &busdisplay_busdisplay_type; + + common_hal_busdisplay_busdisplay_construct( + display, + bus, + 240, // width (after rotation) + 135, // height (after rotation) + 40, // column start + 53, // row start + 0, // rotation + 16, // color depth + false, // grayscale + false, // pixels in a byte share a row. Only valid for depths < 8 + 1, // bytes per cell. Only valid for depths < 8 + false, // reverse_pixels_in_byte. Only valid for depths < 8 + false, // reverse_pixels_in_word + MIPI_COMMAND_SET_COLUMN_ADDRESS, // set column command + MIPI_COMMAND_SET_PAGE_ADDRESS, // set row command + MIPI_COMMAND_WRITE_MEMORY_START, // write memory command + display_init_sequence, + sizeof(display_init_sequence), + &pin_GPIO38, // backlight pin + NO_BRIGHTNESS_COMMAND, + 1.0f, // brightness + false, // single_byte_bounds + false, // data_as_commands + true, // auto_refresh + 60, // native_frames_per_second + true, // backlight_on_high + false, // SH1107_addressing + 350 // backlight pwm frequency + ); +} + +// TODO: Should we turn off the display when asleep, in board_deinit() ? diff --git a/ports/espressif/boards/m5stack_cardputer_ros/cardputer_keyboard.c b/ports/espressif/boards/m5stack_cardputer_ros/cardputer_keyboard.c new file mode 100644 index 0000000000000..4bb7dd7c42ba3 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/cardputer_keyboard.c @@ -0,0 +1,236 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2016 Scott Shawcroft +// +// SPDX-License-Identifier: MIT + +#include "py/obj.h" +#include "py/objstr.h" +#include "py/runtime.h" + +#include "supervisor/shared/serial.h" +#include "shared-bindings/keypad/EventQueue.h" +#include "shared-bindings/keypad_demux/DemuxKeyMatrix.h" +#include "shared-bindings/microcontroller/Pin.h" +#include "shared-module/keypad/EventQueue.h" +#include "shared-module/keypad_demux/DemuxKeyMatrix.h" +#include "supervisor/shared/reload.h" + +#include "keymap.h" + +//| """M5Stack Cardputer keyboard integration. +//| """ +//| +//| """The KEYBOARD object is an instance of DemuxKeyMatrix, configured with correct pins. +//| The pins cannot be used for any other purposes (even though exposed in the board module). +//| By default all keyboard events are consumed and routed to the standard input - there is +//| not much use of the KEYBOARD object in this configuration - just read the input via sys.stdin. +//| +//| If you need to manually process individual key up / key down events via KEYBOARD.events, +//| call `detach_serial()`. +//| """" +//| KEYBOARD: keypad_demux.DemuxKeymatrix +//| +keypad_demux_demuxkeymatrix_obj_t cardputer_keyboard_obj; +bool cardputer_keyboard_serial_attached = false; + +void cardputer_keyboard_init(void); +void keyboard_seq(const char *seq); +void update_keyboard(keypad_eventqueue_obj_t *queue); + +//| def detach_serial() -> None: +//| """Stops consuming keyboard events and routing them to sys.stdin.""" +//| ... +//| +static mp_obj_t detach_serial(void) { + cardputer_keyboard_serial_attached = false; + common_hal_keypad_eventqueue_set_event_handler(cardputer_keyboard_obj.events, NULL); + return mp_const_none; +} +static MP_DEFINE_CONST_FUN_OBJ_0(detach_serial_obj, detach_serial); + +//| def attach_serial() -> None: +//| """Starts consuming keyboard events and routing them to sys.stdin.""" +//| ... +//| +static mp_obj_t attach_serial(void) { + common_hal_keypad_eventqueue_set_event_handler(cardputer_keyboard_obj.events, update_keyboard); + cardputer_keyboard_serial_attached = true; + return mp_const_none; +} +static MP_DEFINE_CONST_FUN_OBJ_0(attach_serial_obj, attach_serial); + +//| def key_to_char(key: int, shifted: bool) -> str | None: +//| """Converts a key index to the respective key (with or without shift modifier). +//| Returns None for functional & modifier keys or whenever not 0 <= key < 56. +//| """ +//| ... +//| +static mp_obj_t key_to_char(mp_obj_t key_obj, mp_obj_t shifted_obj) { + mp_int_t key = mp_obj_get_int(key_obj); + if (key < 0 || key > (mp_int_t)(sizeof keymap / sizeof *keymap) || keymap[key] == 0) { + return mp_const_none; + } else if (shifted_obj == mp_const_true) { + return mp_obj_new_str(&keymap_shifted[key], 1); + } else { + return mp_obj_new_str(&keymap[key], 1); + } +} +static MP_DEFINE_CONST_FUN_OBJ_2(key_to_char_obj, key_to_char); + +// Ring buffer of characters consumed from keyboard events (when serial attached) +ringbuf_t keyqueue; +char keybuf[32]; + +keypad_event_obj_t event; +char keystate[56]; + +// Keyboard pins +const mcu_pin_obj_t *row_addr_pins[] = { + &pin_GPIO8, + &pin_GPIO9, + &pin_GPIO11, +}; + +const mcu_pin_obj_t *column_pins[] = { + &pin_GPIO13, + &pin_GPIO15, + &pin_GPIO3, + &pin_GPIO4, + &pin_GPIO5, + &pin_GPIO6, + &pin_GPIO7 +}; + +void cardputer_keyboard_init(void) { + cardputer_keyboard_obj.base.type = &keypad_demux_demuxkeymatrix_type; + common_hal_keypad_demux_demuxkeymatrix_construct( + &cardputer_keyboard_obj, // self + 3, // num_row_addr_pins + row_addr_pins, // row_addr_pins + 7, // num_column_pins + column_pins, // column_pins + 0.01f, // interval + 20, // max_events + 2 // debounce_threshold + ); + demuxkeymatrix_never_reset(&cardputer_keyboard_obj); + + ringbuf_init(&keyqueue, (uint8_t *)keybuf, sizeof(keybuf)); + attach_serial(); +} + +// Overrides the weakly linked function from supervisor/shared/serial.c +void board_serial_init(void) { + cardputer_keyboard_init(); +} + +// Overrides the weakly linked function from supervisor/shared/serial.c +bool board_serial_connected(void) { + return cardputer_keyboard_serial_attached; +} + +// Overrides the weakly linked function from supervisor/shared/serial.c +uint32_t board_serial_bytes_available(void) { + if (cardputer_keyboard_serial_attached) { + return ringbuf_num_filled(&keyqueue); + } else { + return 0; + } +} + +// Overrides the weakly linked function from supervisor/shared/serial.c +char board_serial_read(void) { + if (cardputer_keyboard_serial_attached) { + return ringbuf_get(&keyqueue); + } else { + return 0; + } +} + +void keyboard_seq(const char *seq) { + while (*seq) { + ringbuf_put(&keyqueue, *seq++); + } +} + +void update_keyboard(keypad_eventqueue_obj_t *queue) { + uint8_t ascii = 0; + + if (common_hal_keypad_eventqueue_get_length(queue) == 0) { + return; + } + + while (common_hal_keypad_eventqueue_get_into(queue, &event)) { + if (event.pressed) { + keystate[event.key_number] = 1; + + if (keystate[KEY_CTRL]) { + if (keystate[KEY_ALT] && keystate[KEY_BACKSPACE]) { + reload_initiate(RUN_REASON_REPL_RELOAD); + } + ascii = keymap[event.key_number]; + if (ascii >= 'a' && ascii <= 'z') { + ascii -= 'a' - 1; + } + + if (ascii == mp_interrupt_char) { + mp_sched_keyboard_interrupt(); + } + } else if (keystate[KEY_SHIFT]) { + ascii = keymap_shifted[event.key_number]; + } else if (keystate[KEY_FN] && event.key_number != KEY_FN) { + switch (event.key_number | FN_MOD) + { + case KEY_DOWN: + keyboard_seq("\e[B"); + break; + case KEY_UP: + keyboard_seq("\e[A"); + break; + case KEY_DELETE: + keyboard_seq("\e[3~"); + break; + case KEY_LEFT: + keyboard_seq("\e[D"); + break; + case KEY_RIGHT: + keyboard_seq("\e[C"); + break; + case KEY_ESC: + ringbuf_put(&keyqueue, '\e'); + break; + } + } else { + ascii = keymap[event.key_number]; + } + + if (ascii > 0) { + if (keystate[KEY_ALT]) { + ringbuf_put(&keyqueue, '\e'); + } else if (keystate[KEY_OPT]) { + ringbuf_put(&keyqueue, '\x10'); + } + ringbuf_put(&keyqueue, ascii); + } + } else { + keystate[event.key_number] = 0; + } + } +} + +static const mp_rom_map_elem_t cardputer_keyboard_module_globals_table[] = { + {MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_cardputer_keyboard)}, + {MP_ROM_QSTR(MP_QSTR_KEYBOARD), MP_ROM_PTR(&cardputer_keyboard_obj)}, + {MP_ROM_QSTR(MP_QSTR_attach_serial), MP_ROM_PTR(&attach_serial_obj)}, + {MP_ROM_QSTR(MP_QSTR_detach_serial), MP_ROM_PTR(&detach_serial_obj)}, + {MP_ROM_QSTR(MP_QSTR_key_to_char), MP_ROM_PTR(&key_to_char_obj)}, +}; +MP_DEFINE_CONST_DICT(cardputer_keyboard_module_globals, cardputer_keyboard_module_globals_table); + +const mp_obj_module_t cardputer_keyboard_module = { + .base = {&mp_type_module}, + .globals = (mp_obj_dict_t *)&cardputer_keyboard_module_globals, +}; + +MP_REGISTER_MODULE(MP_QSTR_cardputer_keyboard, cardputer_keyboard_module); diff --git a/ports/espressif/boards/m5stack_cardputer_ros/keymap.h b/ports/espressif/boards/m5stack_cardputer_ros/keymap.h new file mode 100644 index 0000000000000..0256fafaa0f57 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/keymap.h @@ -0,0 +1,214 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#pragma once + +#define SHIFT_MOD 0x40 +#define FN_MOD 0x80 + +#define KEY_OPT 0 +#define KEY_Z 1 +#define KEY_C 2 +#define KEY_B 3 +#define KEY_M 4 +#define KEY_DOT 5 +#define KEY_SPACE 6 +#define KEY_SHIFT 7 +#define KEY_S 8 +#define KEY_F 9 +#define KEY_H 10 +#define KEY_K 11 +#define KEY_SEMICOLON 12 +#define KEY_ENTER 13 +#define KEY_Q 14 +#define KEY_E 15 +#define KEY_T 16 +#define KEY_U 17 +#define KEY_O 18 +#define KEY_LEFT_BRACKET 19 +#define KEY_BACKSLASH 20 +#define KEY_1 21 +#define KEY_3 22 +#define KEY_5 23 +#define KEY_7 24 +#define KEY_9 25 +#define KEY_UNDERSCORE 26 +#define KEY_BACKSPACE 27 +#define KEY_CTRL 28 +#define KEY_ALT 29 +#define KEY_X 30 +#define KEY_V 31 +#define KEY_N 32 +#define KEY_COMMA 33 +#define KEY_SLASH 34 +#define KEY_FN 35 +#define KEY_A 36 +#define KEY_D 37 +#define KEY_G 38 +#define KEY_J 39 +#define KEY_L 40 +#define KEY_APOSTROPHE 41 +#define KEY_TAB 42 +#define KEY_W 43 +#define KEY_R 44 +#define KEY_Y 45 +#define KEY_I 46 +#define KEY_P 47 +#define KEY_RIGHT_BRACKET 48 +#define KEY_GRAVE 49 +#define KEY_2 50 +#define KEY_4 51 +#define KEY_6 52 +#define KEY_8 53 +#define KEY_0 54 +#define KEY_EQUALS 55 + +#define KEY_GREATER (5 | SHIFT_MOD) +#define KEY_COLON (12 | SHIFT_MOD) +#define KEY_LEFT_CURLY_BRACKET (19 | SHIFT_MOD) +#define KEY_PIPE (20 | SHIFT_MOD) +#define KEY_EXCLAMATION (21 | SHIFT_MOD) +#define KEY_HASH (22 | SHIFT_MOD) +#define KEY_PERCENT (23 | SHIFT_MOD) +#define KEY_AMPERSAND (24 | SHIFT_MOD) +#define KEY_OPEN_PARENTHESIS (25 | SHIFT_MOD) +#define KEY_MINUS (26 | SHIFT_MOD) +#define KEY_LESS (33 | SHIFT_MOD) +#define KEY_QUESTION (34 | SHIFT_MOD) +#define KEY_DOUBLE_QUOTE (41 | SHIFT_MOD) +#define KEY_RIGHT_CURLY_BRACKET (48 | SHIFT_MOD) +#define KEY_TILDE (49 | SHIFT_MOD) +#define KEY_AT (50 | SHIFT_MOD) +#define KEY_DOLLAR (51 | SHIFT_MOD) +#define KEY_CARET (52 | SHIFT_MOD) +#define KEY_ASTERISK (53 | SHIFT_MOD) +#define KEY_CLOSE_PARENTHESIS (54 | SHIFT_MOD) +#define KEY_PLUS (55 | SHIFT_MOD) + +#define KEY_DOWN (5 | FN_MOD) +#define KEY_UP (12 | FN_MOD) +#define KEY_DELETE (27 | FN_MOD) +#define KEY_LEFT (33 | FN_MOD) +#define KEY_RIGHT (34 | FN_MOD) +#define KEY_ESC (49 | FN_MOD) + +const char keymap[56] = { + 0, // KEY_OPT + 'z', // KEY_Z + 'c', // KEY_C + 'b', // KEY_B + 'm', // KEY_M + '.', // KEY_DOT + ' ', // KEY_SPACE + 0, // KEY_SHIFT + 's', // KEY_S + 'f', // KEY_F + 'h', // KEY_H + 'k', // KEY_K + ';', // KEY_SEMICOLON + '\r',// KEY_ENTER + 'q', // KEY_Q + 'e', // KEY_E + 't', // KEY_T + 'u', // KEY_U + 'o', // KEY_O + '[', // KEY_LEFT_BRACKET + '\\',// KEY_BACKSLASH + '1', // KEY_1 + '3', // KEY_3 + '5', // KEY_5 + '7', // KEY_7 + '9', // KEY_9 + '_', // KEY_UNDERSCORE + '\b',// KEY_BACKSPACE + 0, // KEY_CTRL + 0, // KEY_ALT + 'x', // KEY_X + 'v', // KEY_V + 'n', // KEY_N + ',', // KEY_COMMA + '/', // KEY_SLASH + 0, // KEY_FN + 'a', // KEY_A + 'd', // KEY_D + 'g', // KEY_G + 'j', // KEY_J + 'l', // KEY_L + '\'',// KEY_APOSTROPHE + '\t',// KEY_TAB + 'w', // KEY_W + 'r', // KEY_R + 'y', // KEY_Y + 'i', // KEY_I + 'p', // KEY_P + ']', // KEY_RIGHT_BRACKET + '`', // KEY_GRAVE + '2', // KEY_2 + '4', // KEY_4 + '6', // KEY_6 + '8', // KEY_8 + '0', // KEY_0 + '=' // KEY_EQUALS +}; + +const char keymap_shifted[56] = { + 0, // KEY_OPT + 'Z', // KEY_Z + 'C', // KEY_C + 'B', // KEY_B + 'M', // KEY_M + '>', // KEY_DOT -> '>' + ' ', // KEY_SPACE + 0, // KEY_SHIFT + 'S', // KEY_S + 'F', // KEY_F + 'H', // KEY_H + 'K', // KEY_K + ':', // KEY_SEMICOLON -> ':' + '\r',// KEY_ENTER + 'Q', // KEY_Q + 'E', // KEY_E + 'T', // KEY_T + 'U', // KEY_U + 'O', // KEY_O + '{', // KEY_LEFT_BRACKET -> '{' + '|', // KEY_BACKSLASH -> '|' + '!', // KEY_1 -> '!' + '#', // KEY_3 -> '#' + '%', // KEY_5 -> '%' + '&', // KEY_7 -> '&' + '(', // KEY_9 -> '(' + '-', // KEY_UNDERSCORE -> '-' + '\b',// KEY_BACKSPACE + 0, // KEY_CTRL + 0, // KEY_ALT + 'X', // KEY_X + 'V', // KEY_V + 'N', // KEY_N + '<', // KEY_COMMA -> '<' + '?', // KEY_SLASH -> '?' + 0, // KEY_FN + 'A', // KEY_A + 'D', // KEY_D + 'G', // KEY_G + 'J', // KEY_J + 'L', // KEY_L + '"', // KEY_APOSTROPHE -> '"' + '\t',// KEY_TAB + 'W', // KEY_W + 'R', // KEY_R + 'Y', // KEY_Y + 'I', // KEY_I + 'P', // KEY_P + '}', // KEY_RIGHT_BRACKET -> '}' + '~', // KEY_GRAVE -> '~' + '@', // KEY_2 -> '@' + '$', // KEY_4 -> '$' + '^', // KEY_6 -> '^' + '*', // KEY_8 -> '*' + ')', // KEY_0 -> ')' + '+' // KEY_EQUALS -> '+' +}; diff --git a/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.h b/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.h new file mode 100644 index 0000000000000..4b62c7d76e6a1 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.h @@ -0,0 +1,19 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2019 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#pragma once + +// Micropython setup + +#define MICROPY_HW_BOARD_NAME "M5Stack Cardputer (ROS version)" +#define MICROPY_HW_MCU_NAME "ESP32S3" + +#define MICROPY_HW_NEOPIXEL (&pin_GPIO21) +#define DEFAULT_I2C_BUS_SCL (&pin_GPIO1) +#define DEFAULT_I2C_BUS_SDA (&pin_GPIO2) +#define CIRCUITPY_BOARD_SPI (2) +#define CIRCUITPY_BOARD_SPI_PIN {{.clock = &pin_GPIO36, .mosi = &pin_GPIO35}, \ + {.clock = &pin_GPIO40, .mosi = &pin_GPIO14, .miso = &pin_GPIO39}} diff --git a/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.mk b/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.mk new file mode 100644 index 0000000000000..8d296939a1a7b --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/mpconfigboard.mk @@ -0,0 +1,17 @@ +USB_VID = 0x303A +USB_PID = 0x81DA +USB_PRODUCT = "M5Stack Cardputer - Circuitpython/ROS" +USB_MANUFACTURER = "M5STACK" + +IDF_TARGET = esp32s3 + +CIRCUITPY_ESP_FLASH_MODE = qio +CIRCUITPY_ESP_FLASH_FREQ = 80m +CIRCUITPY_ESP_FLASH_SIZE = 8MB +CIRCUITPY_ESPCAMERA = 0 +CIRCUITPY_RCLCPY = 1 + +CIRCUITPY_GIFIO = 1 +CIRCUITPY_MAX3421E = 0 + +SRC_C += boards/$(BOARD)/cardputer_keyboard.c diff --git a/ports/espressif/boards/m5stack_cardputer_ros/pins.c b/ports/espressif/boards/m5stack_cardputer_ros/pins.c new file mode 100644 index 0000000000000..ba309e0dcbff1 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/pins.c @@ -0,0 +1,86 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2020 Scott Shawcroft for Adafruit Industries +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/board/__init__.h" + +#include "shared-module/displayio/__init__.h" +CIRCUITPY_BOARD_BUS_SINGLETON(sd_spi, spi, 1) +static const mp_rom_map_elem_t board_module_globals_table[] = { + CIRCUITPYTHON_BOARD_DICT_STANDARD_ITEMS + + // Port A + { MP_ROM_QSTR(MP_QSTR_TX), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_PORTA1), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_D1), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_G1), MP_ROM_PTR(&pin_GPIO1) }, + { MP_ROM_QSTR(MP_QSTR_RX), MP_ROM_PTR(&pin_GPIO2) }, + { MP_ROM_QSTR(MP_QSTR_PORTA2), MP_ROM_PTR(&pin_GPIO2) }, + { MP_ROM_QSTR(MP_QSTR_D2), MP_ROM_PTR(&pin_GPIO2) }, + { MP_ROM_QSTR(MP_QSTR_G2), MP_ROM_PTR(&pin_GPIO2) }, + + // Keyboard + { MP_ROM_QSTR(MP_QSTR_KB_COL_0), MP_ROM_PTR(&pin_GPIO13) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_1), MP_ROM_PTR(&pin_GPIO15) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_2), MP_ROM_PTR(&pin_GPIO3) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_3), MP_ROM_PTR(&pin_GPIO4) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_4), MP_ROM_PTR(&pin_GPIO5) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_5), MP_ROM_PTR(&pin_GPIO6) }, + { MP_ROM_QSTR(MP_QSTR_KB_COL_6), MP_ROM_PTR(&pin_GPIO7) }, + { MP_ROM_QSTR(MP_QSTR_KB_A_0), MP_ROM_PTR(&pin_GPIO8) }, + { MP_ROM_QSTR(MP_QSTR_KB_A_1), MP_ROM_PTR(&pin_GPIO9) }, + { MP_ROM_QSTR(MP_QSTR_KB_A_2), MP_ROM_PTR(&pin_GPIO11) }, + + // Neopixel + { MP_ROM_QSTR(MP_QSTR_NEOPIXEL), MP_ROM_PTR(&pin_GPIO21) }, + + // Speaker + { MP_ROM_QSTR(MP_QSTR_I2S_BIT_CLOCK), MP_ROM_PTR(&pin_GPIO41) }, + { MP_ROM_QSTR(MP_QSTR_I2S_DATA), MP_ROM_PTR(&pin_GPIO42) }, + { MP_ROM_QSTR(MP_QSTR_I2S_WORD_SELECT), MP_ROM_PTR(&pin_GPIO43) }, + + // Mic + { MP_ROM_QSTR(MP_QSTR_MIC_DATA), MP_ROM_PTR(&pin_GPIO46) }, + { MP_ROM_QSTR(MP_QSTR_MIC_CLK), MP_ROM_PTR(&pin_GPIO43) }, + + // IR + { MP_ROM_QSTR(MP_QSTR_IR_TX), MP_ROM_PTR(&pin_GPIO44) }, + + // SD + { MP_ROM_QSTR(MP_QSTR_SD_MOSI), MP_ROM_PTR(&pin_GPIO14) }, + { MP_ROM_QSTR(MP_QSTR_SD_SCK), MP_ROM_PTR(&pin_GPIO40) }, + { MP_ROM_QSTR(MP_QSTR_SD_MISO), MP_ROM_PTR(&pin_GPIO39) }, + { MP_ROM_QSTR(MP_QSTR_SD_CS), MP_ROM_PTR(&pin_GPIO12) }, + + + // Display + { MP_ROM_QSTR(MP_QSTR_TFT_RST), MP_ROM_PTR(&pin_GPIO33) }, + { MP_ROM_QSTR(MP_QSTR_TFT_RESET), MP_ROM_PTR(&pin_GPIO33) }, + { MP_ROM_QSTR(MP_QSTR_TFT_DC), MP_ROM_PTR(&pin_GPIO34) }, + { MP_ROM_QSTR(MP_QSTR_TFT_RS), MP_ROM_PTR(&pin_GPIO34) }, + { MP_ROM_QSTR(MP_QSTR_TFT_MOSI), MP_ROM_PTR(&pin_GPIO35) }, + { MP_ROM_QSTR(MP_QSTR_TFT_DATA), MP_ROM_PTR(&pin_GPIO35) }, + { MP_ROM_QSTR(MP_QSTR_TFT_SCK), MP_ROM_PTR(&pin_GPIO36) }, + { MP_ROM_QSTR(MP_QSTR_TFT_CS), MP_ROM_PTR(&pin_GPIO37) }, + { MP_ROM_QSTR(MP_QSTR_TFT_BACKLIGHT), MP_ROM_PTR(&pin_GPIO38) }, + { MP_ROM_QSTR(MP_QSTR_TFT_BL), MP_ROM_PTR(&pin_GPIO38) }, + + // Button + { MP_ROM_QSTR(MP_QSTR_BUTTON), MP_ROM_PTR(&pin_GPIO0) }, + { MP_ROM_QSTR(MP_QSTR_BOOT0), MP_ROM_PTR(&pin_GPIO0) }, + + // Battery + { MP_ROM_QSTR(MP_QSTR_BAT_ADC), MP_ROM_PTR(&pin_GPIO10) }, + + // Other + { MP_ROM_QSTR(MP_QSTR_I2C), MP_ROM_PTR(&board_i2c_obj) }, + { MP_ROM_QSTR(MP_QSTR_PORTA_I2C), MP_ROM_PTR(&board_i2c_obj) }, + { MP_ROM_QSTR(MP_QSTR_TFT_SPI), MP_ROM_PTR(&board_spi_obj) }, + { MP_ROM_QSTR(MP_QSTR_SD_SPI), MP_ROM_PTR(&board_sd_spi_obj) }, + + // Display object + { MP_ROM_QSTR(MP_QSTR_DISPLAY), MP_ROM_PTR(&displays[0].display)}, +}; +MP_DEFINE_CONST_DICT(board_module_globals, board_module_globals_table); diff --git a/ports/espressif/boards/m5stack_cardputer_ros/sdkconfig b/ports/espressif/boards/m5stack_cardputer_ros/sdkconfig new file mode 100644 index 0000000000000..e962866216039 --- /dev/null +++ b/ports/espressif/boards/m5stack_cardputer_ros/sdkconfig @@ -0,0 +1,14 @@ +# +# Espressif IoT Development Framework Configuration +# +# +# Component config +# +# +# LWIP +# +# end of LWIP + +# end of Component config + +# end of Espressif IoT Development Framework Configuration diff --git a/ports/espressif/common-hal/rclcpy/Node.c b/ports/espressif/common-hal/rclcpy/Node.c new file mode 100644 index 0000000000000..3e068a231f93c --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/Node.c @@ -0,0 +1,25 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/rclcpy/Node.h" +#include "shared-bindings/rclcpy/__init__.h" + +void common_hal_rclcpy_node_construct(rclcpy_node_obj_t *self, + const char *node_name, const char *node_namespace) { + + rclc_node_init_default(&self->rcl_node, node_name, node_namespace, &rclcpy_default_context.rcl_support); + if (!rcl_node_is_valid(&self->rcl_node)) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS node failed to initialize")); + } +} + +const char * common_hal_rclcpy_node_get_name(rclcpy_node_obj_t *self) { + return rcl_node_get_name(&self->rcl_node); +} + +const char * common_hal_rclcpy_node_get_namespace(rclcpy_node_obj_t *self) { + return rcl_node_get_namespace(&self->rcl_node); +} diff --git a/ports/espressif/common-hal/rclcpy/Node.h b/ports/espressif/common-hal/rclcpy/Node.h new file mode 100644 index 0000000000000..f6b47fd360235 --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/Node.h @@ -0,0 +1,22 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once + +#include "py/obj.h" +#include "py/runtime.h" + +#include +#include +#include +#include +#include + +typedef struct { + mp_obj_base_t base; + rcl_node_t rcl_node; +} rclcpy_node_obj_t; + diff --git a/ports/espressif/common-hal/rclcpy/Publisher.c b/ports/espressif/common-hal/rclcpy/Publisher.c new file mode 100644 index 0000000000000..77287de5ede02 --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/Publisher.c @@ -0,0 +1,43 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/rclcpy/Publisher.h" + + +void common_hal_rclcpy_publisher_construct(rclcpy_publisher_obj_t *self, rclcpy_node_obj_t * node, + const char *topic_name) { + + // Create Int32 type object + // TODO: support other message types through class imports + const rosidl_message_type_support_t * type_support = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32); + + // Creates a reliable Int32 publisher + rcl_ret_t rc = rclc_publisher_init_default( + &self->rcl_publisher, &node->rcl_node, + type_support, topic_name); + + if (RCL_RET_OK != rc) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS topic failed to initialize")); + } +} + +void common_hal_rclcpy_publisher_publish_int32(rclcpy_publisher_obj_t * self, int32_t data) { + // Int32 message object + std_msgs__msg__Int32 msg; + + // Set message value + msg.data = data; + + // Publish message + rcl_ret_t rc = rcl_publish(&self->rcl_publisher, &msg, NULL); + if (RCL_RET_OK != rc) { + mp_raise_RuntimeError(MP_ERROR_TEXT("Could not publish to ROS topic")); + } +} + +const char * common_hal_rclcpy_publisher_get_topic_name(rclcpy_publisher_obj_t *self) { + return rcl_publisher_get_topic_name(&self->rcl_publisher); +} \ No newline at end of file diff --git a/ports/espressif/common-hal/rclcpy/Publisher.h b/ports/espressif/common-hal/rclcpy/Publisher.h new file mode 100644 index 0000000000000..0286e74ac7a74 --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/Publisher.h @@ -0,0 +1,27 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once + +#include "py/obj.h" + +#include "common-hal/rclcpy/Node.h" +#include "common-hal/rclcpy/__init__.h" + +#include +#include +#include +#include +#include + +#include + + +typedef struct { + mp_obj_base_t base; + rclcpy_node_obj_t * node; + rcl_publisher_t rcl_publisher; +} rclcpy_publisher_obj_t; diff --git a/ports/espressif/common-hal/rclcpy/__init__.c b/ports/espressif/common-hal/rclcpy/__init__.c new file mode 100644 index 0000000000000..c852b35685c20 --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/__init__.c @@ -0,0 +1,69 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/rclcpy/__init__.h" + +rclcpy_context_t rclcpy_default_context = { + .initialized = false, +}; + +static void* microros_allocate(size_t size, void* state) { + (void) state; + return m_malloc(size); +} + +static void microros_deallocate(void* pointer, void* state) { + (void) state; + m_free(pointer); +} + +static void* microros_reallocate(void* pointer, size_t size, void* state) { + (void) state; + return m_realloc(pointer, size); +} + +static void* microros_zero_allocate(size_t number_of_elements, size_t size_of_element, void* state) { + (void) state; + size_t total_size = number_of_elements * size_of_element; + void* ptr = m_malloc(total_size); + if (ptr != NULL) { + memset(ptr, 0, total_size); + } + return ptr; +} + +void common_hal_rclcpy_init(void) { + // Get empty allocator + rcl_allocator_t custom_allocator = rcutils_get_zero_initialized_allocator(); + + // Set custom allocation methods + custom_allocator.allocate = microros_allocate; + custom_allocator.deallocate = microros_deallocate; + custom_allocator.reallocate = microros_reallocate; + custom_allocator.zero_allocate = microros_zero_allocate; + + // Set custom allocator as default + if (!rcutils_set_default_allocator(&custom_allocator)) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS memory allocator failure")); + } + + rclcpy_default_context.rcl_allocator = custom_allocator; + rcl_ret_t ret; + ret = rclc_support_init(&rclcpy_default_context.rcl_support, 0, NULL, &rclcpy_default_context.rcl_allocator); + if (ret != RCL_RET_OK) { + mp_raise_RuntimeError(MP_ERROR_TEXT("ROS failed to initialize")); + } else { + rclcpy_default_context.initialized = true; + } +} + +rclcpy_context_t * common_hal_rclcpy_get_default_context(void) { + return &rclcpy_default_context; +} + +bool common_hal_rclcpy_default_context_is_initialized(void) { + return rclcpy_default_context.initialized; +} \ No newline at end of file diff --git a/ports/espressif/common-hal/rclcpy/__init__.h b/ports/espressif/common-hal/rclcpy/__init__.h new file mode 100644 index 0000000000000..8000fde28a7d4 --- /dev/null +++ b/ports/espressif/common-hal/rclcpy/__init__.h @@ -0,0 +1,24 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once + +#include "py/obj.h" +#include "py/runtime.h" + +#include +#include +#include +#include +#include + +typedef struct { + bool initialized; + rcl_allocator_t rcl_allocator; + rclc_support_t rcl_support; +} rclcpy_context_t; + +extern rclcpy_context_t rclcpy_default_context; \ No newline at end of file diff --git a/ports/espressif/microros-lib b/ports/espressif/microros-lib new file mode 160000 index 0000000000000..4b2a696dec7f0 --- /dev/null +++ b/ports/espressif/microros-lib @@ -0,0 +1 @@ +Subproject commit 4b2a696dec7f0f865e014ea4b83f2332df7e9560 diff --git a/ports/espressif/mpconfigport.mk b/ports/espressif/mpconfigport.mk index ccc305761c4dc..516111e5a8586 100644 --- a/ports/espressif/mpconfigport.mk +++ b/ports/espressif/mpconfigport.mk @@ -64,6 +64,7 @@ CIRCUITPY_HASHLIB ?= 1 CIRCUITPY_I2CTARGET ?= 0 CIRCUITPY_MAX3421E ?= 1 CIRCUITPY_MEMORYMAP ?= 1 +CIRCUITPY_RCLCPY ?= 0 CIRCUITPY_NVM ?= 1 CIRCUITPY_PARALLELDISPLAYBUS ?= 1 CIRCUITPY_PS2IO ?= 1 diff --git a/py/circuitpy_defns.mk b/py/circuitpy_defns.mk index f73d52a6c4b12..72f5e406019db 100644 --- a/py/circuitpy_defns.mk +++ b/py/circuitpy_defns.mk @@ -288,6 +288,9 @@ endif ifeq ($(CIRCUITPY_MICROCONTROLLER),1) SRC_PATTERNS += microcontroller/% endif +ifeq ($(CIRCUITPY_RCLCPY),1) +SRC_PATTERNS += rclcpy/% +endif ifeq ($(CIRCUITPY_MDNS),1) SRC_PATTERNS += mdns/% endif @@ -540,6 +543,9 @@ SRC_COMMON_HAL_ALL = \ pulseio/__init__.c \ pwmio/PWMOut.c \ pwmio/__init__.c \ + rclcpy/__init__.c \ + rclcpy/Node.c \ + rclcpy/Publisher.c \ rgbmatrix/RGBMatrix.c \ rgbmatrix/__init__.c \ rotaryio/IncrementalEncoder.c \ diff --git a/py/circuitpy_mpconfig.mk b/py/circuitpy_mpconfig.mk index b081e0f52dbde..4e5cef505a2fd 100644 --- a/py/circuitpy_mpconfig.mk +++ b/py/circuitpy_mpconfig.mk @@ -388,6 +388,9 @@ CFLAGS += -DCIRCUITPY_MEMORYMONITOR=$(CIRCUITPY_MEMORYMONITOR) CIRCUITPY_MICROCONTROLLER ?= 1 CFLAGS += -DCIRCUITPY_MICROCONTROLLER=$(CIRCUITPY_MICROCONTROLLER) +CIRCUITPY_RCLCPY ?= 1 +CFLAGS += -DCIRCUITPY_RCLCPY=$(CIRCUITPY_RCLCPY) + CIRCUITPY_MDNS ?= $(CIRCUITPY_WIFI) CFLAGS += -DCIRCUITPY_MDNS=$(CIRCUITPY_MDNS) diff --git a/shared-bindings/rclcpy/Node.c b/shared-bindings/rclcpy/Node.c new file mode 100644 index 0000000000000..7dd887e53cf0a --- /dev/null +++ b/shared-bindings/rclcpy/Node.c @@ -0,0 +1,93 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include +#include "shared-bindings/rclcpy/Node.h" +#include "shared-bindings/rclcpy/Publisher.h" +#include "py/objproperty.h" +#include "py/objtype.h" +#include "py/runtime.h" + +static mp_obj_t rclcpy_node_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { + enum { ARG_node_name, ARG_namespace }; + static const mp_arg_t allowed_args[] = { + { MP_QSTR_node_name, MP_ARG_REQUIRED | MP_ARG_OBJ }, + { MP_QSTR_namespace, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} }, + }; + mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)]; + mp_arg_parse_all_kw_array(n_args, n_kw, all_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); + const char *node_name = mp_obj_str_get_str(args[ARG_node_name].u_obj); + const char *namespace = ""; + if (args[ARG_namespace].u_obj != mp_const_none) { + namespace = mp_obj_str_get_str(args[ARG_namespace].u_obj); + } + + rclcpy_node_obj_t *self = mp_obj_malloc_with_finaliser(rclcpy_node_obj_t, &rclcpy_node_type); + common_hal_rclcpy_node_construct(self, node_name, namespace); + return (mp_obj_t)self; +} + +static mp_obj_t rclcpy_node_create_publisher(mp_obj_t self_in, mp_obj_t topic) { + rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in); + const char *topic_name = mp_obj_str_get_str(topic); + + rclcpy_publisher_obj_t *publisher = mp_obj_malloc_with_finaliser(rclcpy_publisher_obj_t, &rclcpy_publisher_type); + common_hal_rclcpy_publisher_construct(publisher, self,topic_name); + return (mp_obj_t)publisher; +} +static MP_DEFINE_CONST_FUN_OBJ_2(rclcpy_node_create_publisher_obj, rclcpy_node_create_publisher); + +static mp_obj_t rclcpy_node_get_name(mp_obj_t self_in) { + // TODO: probably a good idea + // check_for_deinit(self); + rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in); + const char * name_str = common_hal_rclcpy_node_get_name(self); + return mp_obj_new_str(name_str,strlen(name_str)); +} +static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_node_get_name_obj, rclcpy_node_get_name); + +static mp_obj_t rclcpy_node_get_namespace(mp_obj_t self_in) { + // TODO: probably a good idea + // check_for_deinit(self); + rclcpy_node_obj_t *self = MP_OBJ_TO_PTR(self_in); + const char * namespace_str = common_hal_rclcpy_node_get_namespace(self); + return mp_obj_new_str(namespace_str,strlen(namespace_str)); +} +static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_node_get_namespace_obj, rclcpy_node_get_namespace); + +static const mp_rom_map_elem_t rclcpy_node_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_create_publisher), MP_ROM_PTR(&rclcpy_node_create_publisher_obj) }, + { MP_ROM_QSTR(MP_QSTR_get_name), MP_ROM_PTR(&rclcpy_node_get_name_obj) }, + { MP_ROM_QSTR(MP_QSTR_get_namespace), MP_ROM_PTR(&rclcpy_node_get_namespace_obj) }, +}; +static MP_DEFINE_CONST_DICT(rclcpy_node_locals_dict, rclcpy_node_locals_dict_table); + + +MP_DEFINE_CONST_OBJ_TYPE( + rclcpy_node_type, + MP_QSTR_Node, + MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS, + make_new, rclcpy_node_make_new, + locals_dict, &rclcpy_node_locals_dict +); + +// Examples: + +// MP_DEFINE_CONST_OBJ_TYPE( +// rtc_rtc_type, +// MP_QSTR_RTC, +// MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS, +// make_new, rtc_rtc_make_new, +// locals_dict, &rtc_rtc_locals_dict +// ); + +// MP_DEFINE_CONST_OBJ_TYPE( +// socketpool_socket_type, +// MP_QSTR_Socket, +// MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS, +// locals_dict, &socketpool_socket_locals_dict, +// protocol, &socket_stream_p +// ); \ No newline at end of file diff --git a/shared-bindings/rclcpy/Node.h b/shared-bindings/rclcpy/Node.h new file mode 100644 index 0000000000000..08278ae1c0a30 --- /dev/null +++ b/shared-bindings/rclcpy/Node.h @@ -0,0 +1,15 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once +#include "common-hal/rclcpy/Node.h" + +extern const mp_obj_type_t rclcpy_node_type; + +void common_hal_rclcpy_node_construct(rclcpy_node_obj_t *self, + const char *node_name, const char *namespace); +const char * common_hal_rclcpy_node_get_name(rclcpy_node_obj_t *self); +const char * common_hal_rclcpy_node_get_namespace(rclcpy_node_obj_t *self); \ No newline at end of file diff --git a/shared-bindings/rclcpy/Publisher.c b/shared-bindings/rclcpy/Publisher.c new file mode 100644 index 0000000000000..790043ed1d6a7 --- /dev/null +++ b/shared-bindings/rclcpy/Publisher.c @@ -0,0 +1,48 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include +#include "shared-bindings/rclcpy/Publisher.h" +#include "py/objproperty.h" +#include "py/objtype.h" +#include "py/runtime.h" + +static mp_obj_t rclcpy_publisher_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { + mp_raise_NotImplementedError(MP_ERROR_TEXT("Publishers can only be created from a parent node")); +} + +static mp_obj_t rclcpy_publisher_publish_int32(mp_obj_t self_in, mp_obj_t in_msg) { + rclcpy_publisher_obj_t *self = MP_OBJ_TO_PTR(self_in); + int32_t msg = mp_obj_get_int(in_msg); + common_hal_rclcpy_publisher_publish_int32(self,msg); + return mp_const_none; +} +static MP_DEFINE_CONST_FUN_OBJ_2(rclcpy_publisher_publish_int32_obj, rclcpy_publisher_publish_int32); + +static mp_obj_t rclcpy_publisher_get_topic_name(mp_obj_t self_in) { + // TODO: probably a good idea + // check_for_deinit(self); + rclcpy_publisher_obj_t *self = MP_OBJ_TO_PTR(self_in); + const char * topic_str = common_hal_rclcpy_publisher_get_topic_name(self); + return mp_obj_new_str(topic_str,strlen(topic_str)); +} +static MP_DEFINE_CONST_FUN_OBJ_1(rclcpy_publisher_get_topic_name_obj, rclcpy_publisher_get_topic_name); + + +static const mp_rom_map_elem_t rclcpy_publisher_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_publish_int32), MP_ROM_PTR(&rclcpy_publisher_publish_int32_obj) }, + { MP_ROM_QSTR(MP_QSTR_get_topic_name), MP_ROM_PTR(&rclcpy_publisher_get_topic_name_obj) }, +}; +static MP_DEFINE_CONST_DICT(rclcpy_publisher_locals_dict, rclcpy_publisher_locals_dict_table); + + +MP_DEFINE_CONST_OBJ_TYPE( + rclcpy_publisher_type, + MP_QSTR_Publisher, + MP_TYPE_FLAG_HAS_SPECIAL_ACCESSORS, + make_new, rclcpy_publisher_make_new, + locals_dict, &rclcpy_publisher_locals_dict +); diff --git a/shared-bindings/rclcpy/Publisher.h b/shared-bindings/rclcpy/Publisher.h new file mode 100644 index 0000000000000..11ff1007ab35d --- /dev/null +++ b/shared-bindings/rclcpy/Publisher.h @@ -0,0 +1,15 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once +#include "common-hal/rclcpy/Publisher.h" + +extern const mp_obj_type_t rclcpy_publisher_type; + +void common_hal_rclcpy_publisher_construct(rclcpy_publisher_obj_t *self, rclcpy_node_obj_t * node, + const char *topic_name); +void common_hal_rclcpy_publisher_publish_int32(rclcpy_publisher_obj_t * self, int32_t data); +const char * common_hal_rclcpy_publisher_get_topic_name(rclcpy_publisher_obj_t *self); diff --git a/shared-bindings/rclcpy/__init__.c b/shared-bindings/rclcpy/__init__.c new file mode 100644 index 0000000000000..27477cedff42a --- /dev/null +++ b/shared-bindings/rclcpy/__init__.c @@ -0,0 +1,41 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#include "shared-bindings/rclcpy/__init__.h" +#include "shared-bindings/rclcpy/Node.h" +#include "shared-bindings/rclcpy/Publisher.h" + +#include "py/obj.h" +#include "py/objproperty.h" +#include "py/objstr.h" +#include "py/runtime.h" + +static mp_obj_t rclcpy_init(void) { + common_hal_rclcpy_init(); + return mp_const_none; +} +static MP_DEFINE_CONST_FUN_OBJ_0(rclcpy_init_obj, rclcpy_init); + + +// TODO: parallel implementation to Node constructor +// static mp_obj_t rclcpy_create_node(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) { +// } + +static const mp_rom_map_elem_t rclcpy_module_globals_table[] = { + { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_rclcpy) }, + { MP_ROM_QSTR(MP_QSTR_Node), MP_ROM_PTR(&rclcpy_node_type) }, + { MP_ROM_QSTR(MP_QSTR_Publisher), MP_ROM_PTR(&rclcpy_publisher_type) }, + { MP_ROM_QSTR(MP_QSTR_init), MP_ROM_PTR(&rclcpy_init_obj) }, +}; + +static MP_DEFINE_CONST_DICT(rclcpy_module_globals, rclcpy_module_globals_table); + +const mp_obj_module_t rclcpy_module = { + .base = { &mp_type_module }, + .globals = (mp_obj_dict_t *)&rclcpy_module_globals, +}; + +MP_REGISTER_MODULE(MP_QSTR_rclcpy, rclcpy_module); diff --git a/shared-bindings/rclcpy/__init__.h b/shared-bindings/rclcpy/__init__.h new file mode 100644 index 0000000000000..83e7416b102b3 --- /dev/null +++ b/shared-bindings/rclcpy/__init__.h @@ -0,0 +1,12 @@ +// This file is part of the CircuitPython project: https://circuitpython.org +// +// SPDX-FileCopyrightText: Copyright (c) 2025 Lucian Copeland +// +// SPDX-License-Identifier: MIT + +#pragma once +#include "common-hal/rclcpy/__init__.h" + +void common_hal_rclcpy_init(void); +rclcpy_context_t * common_hal_rclcpy_get_default_context(void); +bool common_hal_rclcpy_default_context_is_initialized(void);