From fd61dd08b2f2b186f696d556df9d4edcd1c4f0d8 Mon Sep 17 00:00:00 2001 From: danwillm Date: Thu, 21 Jul 2022 11:33:58 +0100 Subject: [PATCH] drivers: add all of opengloves driver --- CMakeLists.txt | 3 + src/xrt/drivers/CMakeLists.txt | 15 + .../bluetooth/opengloves_bt_serial.c | 91 ++++++ .../bluetooth/opengloves_bt_serial.h | 31 ++ .../bluetooth/opengloves_prober_bt.c | 81 +++++ .../bluetooth/opengloves_prober_bt.h | 26 ++ .../communication/opengloves_communication.h | 58 ++++ .../serial/opengloves_prober_serial.c | 107 +++++++ .../serial/opengloves_prober_serial.h | 27 ++ .../communication/serial/opengloves_serial.c | 106 +++++++ .../communication/serial/opengloves_serial.h | 31 ++ .../opengloves/encoding/alpha_encoding.cpp | 250 +++++++++++++++ .../opengloves/encoding/alpha_encoding.h | 22 ++ .../drivers/opengloves/encoding/encoding.h | 71 +++++ .../drivers/opengloves/opengloves_device.c | 290 ++++++++++++++++++ .../drivers/opengloves/opengloves_device.h | 24 ++ .../drivers/opengloves/opengloves_interface.h | 36 +++ .../drivers/opengloves/opengloves_prober.c | 116 +++++++ 18 files changed, 1385 insertions(+) create mode 100644 src/xrt/drivers/opengloves/communication/bluetooth/opengloves_bt_serial.c create mode 100644 src/xrt/drivers/opengloves/communication/bluetooth/opengloves_bt_serial.h create mode 100644 src/xrt/drivers/opengloves/communication/bluetooth/opengloves_prober_bt.c create mode 100644 src/xrt/drivers/opengloves/communication/bluetooth/opengloves_prober_bt.h create mode 100644 src/xrt/drivers/opengloves/communication/opengloves_communication.h create mode 100644 src/xrt/drivers/opengloves/communication/serial/opengloves_prober_serial.c create mode 100644 src/xrt/drivers/opengloves/communication/serial/opengloves_prober_serial.h create mode 100644 src/xrt/drivers/opengloves/communication/serial/opengloves_serial.c create mode 100644 src/xrt/drivers/opengloves/communication/serial/opengloves_serial.h create mode 100644 src/xrt/drivers/opengloves/encoding/alpha_encoding.cpp create mode 100644 src/xrt/drivers/opengloves/encoding/alpha_encoding.h create mode 100644 src/xrt/drivers/opengloves/encoding/encoding.h create mode 100644 src/xrt/drivers/opengloves/opengloves_device.c create mode 100644 src/xrt/drivers/opengloves/opengloves_device.h create mode 100644 src/xrt/drivers/opengloves/opengloves_interface.h create mode 100644 src/xrt/drivers/opengloves/opengloves_prober.c diff --git a/CMakeLists.txt b/CMakeLists.txt index a286ab357..486b9a0ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -285,6 +285,7 @@ option_with_deps(XRT_BUILD_DRIVER_HYDRA "Enable Hydra driver" DEPENDS XRT_HAVE_I option_with_deps(XRT_BUILD_DRIVER_ILLIXR "Enable ILLIXR driver" DEPENDS ILLIXR_PATH) option_with_deps(XRT_BUILD_DRIVER_NS "Enable North Star driver" DEPENDS XRT_HAVE_INTERNAL_HID) option_with_deps(XRT_BUILD_DRIVER_OHMD "Enable OpenHMD driver" DEPENDS OPENHMD_FOUND) +option_with_deps(XRT_BUILD_DRIVER_OPENGLOVES "Enable OpenGloves driver" DEPENDS XRT_HAVE_LIBUDEV XRT_HAVE_BLUETOOTH) option_with_deps(XRT_BUILD_DRIVER_PSMV "Enable Playstation Move driver" DEPENDS XRT_HAVE_INTERNAL_HID) option_with_deps(XRT_BUILD_DRIVER_PSVR "Enable PSVR HMD driver" DEPENDS XRT_HAVE_HIDAPI) option_with_deps(XRT_BUILD_DRIVER_QWERTY "Enable Qwerty driver" DEPENDS XRT_HAVE_SDL2) @@ -336,6 +337,7 @@ list( "ILLIXR" "NS" "OHMD" + "OPENGLOVES" "PSMV" "PSVR" "REALSENSE" @@ -504,6 +506,7 @@ message(STATUS "# DRIVER_HYDRA: ${XRT_BUILD_DRIVER_HYDRA}") message(STATUS "# DRIVER_ILLIXR: ${XRT_BUILD_DRIVER_ILLIXR}") message(STATUS "# DRIVER_NS: ${XRT_BUILD_DRIVER_NS}") message(STATUS "# DRIVER_OHMD: ${XRT_BUILD_DRIVER_OHMD}") +message(STATUS "# DRIVER_OPENGLOVES: ${XRT_BUILD_DRIVER_OPENGLOVES}") message(STATUS "# DRIVER_PSMV: ${XRT_BUILD_DRIVER_PSMV}") message(STATUS "# DRIVER_PSVR: ${XRT_BUILD_DRIVER_PSVR}") message(STATUS "# DRIVER_QWERTY: ${XRT_BUILD_DRIVER_QWERTY}") diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index a8dc36727..2115463a3 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -113,6 +113,21 @@ if(XRT_BUILD_DRIVER_OHMD) list(APPEND ENABLED_HEADSET_DRIVERS openhmd) endif() +if(XRT_BUILD_DRIVER_OPENGLOVES) + add_library( + drv_opengloves STATIC + opengloves/opengloves_interface.h opengloves/opengloves_device.c opengloves/opengloves_prober.c opengloves/opengloves_device.h opengloves/communication/serial/opengloves_serial.h opengloves/communication/serial/opengloves_serial.c opengloves/encoding/alpha_encoding.h opengloves/encoding/alpha_encoding.cpp opengloves/encoding/encoding.h opengloves/communication/bluetooth/opengloves_bt_serial.h opengloves/communication/bluetooth/opengloves_bt_serial.c opengloves/communication/opengloves_communication.h opengloves/communication/serial/opengloves_prober_serial.h opengloves/communication/serial/opengloves_prober_serial.c opengloves/communication/bluetooth/opengloves_prober_bt.h opengloves/communication/bluetooth/opengloves_prober_bt.c) + target_link_libraries(drv_opengloves + PRIVATE + xrt-interfaces + aux_util + aux_os + bluetooth + ) + list(APPEND ENABLED_DRIVERS opengloves) + +endif() + if(XRT_BUILD_DRIVER_PSMV) add_library(drv_psmv STATIC psmv/psmv_driver.c psmv/psmv_interface.h) target_link_libraries( diff --git a/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_bt_serial.c b/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_bt_serial.c new file mode 100644 index 000000000..2c7a42553 --- /dev/null +++ b/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_bt_serial.c @@ -0,0 +1,91 @@ +// Copyright 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves bluetooth serial implementation + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#include +#include +#include +#include + +#include "util/u_misc.h" +#include "util/u_debug.h" + +#include "opengloves_bt_serial.h" + +#define OPENGLOVES_PROBER_LOG_LEVEL U_LOGGING_TRACE + +#define OPENGLOVES_ERROR(...) U_LOG_IFL_E(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) +#define OPENGLOVES_WARN(...) U_LOG_IFL_W(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) +#define OPENGLOVES_INFO(...) U_LOG_IFL_I(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) + +static void +opengloves_bt_close(struct opengloves_bt_device *btdev) +{ + if (btdev->sock != 0) { + close(btdev->sock); + btdev->sock = 0; + } +} + +static int +opengloves_bt_read(struct opengloves_communication_device *ocdev, char *data, size_t length) +{ + struct opengloves_bt_device *obdev = (struct opengloves_bt_device *)ocdev; + + return read(obdev->sock, data, length); +} + +static int +opengloves_bt_write(struct opengloves_communication_device *ocdev, const uint8_t *data, size_t length) +{ + struct opengloves_bt_device *obdev = (struct opengloves_bt_device *)obdev; + + return write(obdev->sock, data, length); +} + +static void +opengloves_bt_destroy(struct opengloves_communication_device *ocdev) +{ + struct opengloves_bt_device *obdev = (struct opengloves_bt_device *)obdev; + + opengloves_bt_close(obdev); + free(obdev); +} + + + +int +opengloves_bt_open(const char *btaddr, struct opengloves_communication_device **out_comm_dev) +{ + struct opengloves_bt_device *obdev = U_TYPED_CALLOC(struct opengloves_bt_device); + + obdev->base.read = opengloves_bt_read; + obdev->base.write = opengloves_bt_write; + obdev->base.destroy = opengloves_bt_destroy; + + // allocate a socket + obdev->sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); + + // set the connection parameters (who to connect to) + struct sockaddr_rc addr = {0}; + addr.rc_family = AF_BLUETOOTH; + addr.rc_channel = (uint8_t)1; + str2ba(btaddr, &addr.rc_bdaddr); + + // connect to server + int ret = connect(obdev->sock, (struct sockaddr *)&addr, sizeof(addr)); + + if (ret < 0) { + OPENGLOVES_ERROR("Failed to connect to device! %s", strerror(errno)); + return ret; + } + + *out_comm_dev = &obdev->base; + + return 0; +} diff --git a/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_bt_serial.h b/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_bt_serial.h new file mode 100644 index 000000000..0a0912289 --- /dev/null +++ b/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_bt_serial.h @@ -0,0 +1,31 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Bluetooth Serial interface for OpenGloves. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#pragma once + +#include "../opengloves_communication.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +struct opengloves_bt_device +{ + struct opengloves_communication_device base; + int sock; +}; + +int +opengloves_bt_open(const char *btaddr, struct opengloves_communication_device **out_comm_dev); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_prober_bt.c b/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_prober_bt.c new file mode 100644 index 000000000..4ce785e0a --- /dev/null +++ b/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_prober_bt.c @@ -0,0 +1,81 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves bluetooth prober implementation. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#include + +#include +#include +#include +#include + +#include "util/u_debug.h" +#include "xrt/xrt_defines.h" + +#include "opengloves_bt_serial.h" +#include "opengloves_prober_bt.h" +#include "util/u_misc.h" + +#define OPENGLOVES_PROBER_LOG_LEVEL U_LOGGING_TRACE + +#define OPENGLOVES_ERROR(...) U_LOG_IFL_E(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) +#define OPENGLOVES_WARN(...) U_LOG_IFL_W(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) +#define OPENGLOVES_INFO(...) U_LOG_IFL_I(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) + +#define OPENGLOVES_BT_MAX_ADDRESS_LEN 19 +#define OPENGLOVES_BT_MAX_NAME_LEN 248 +#define OPENGLOVES_BT_MAX_DEVICES 255 + +int +opengloves_get_bt_devices(const char *bt_name, struct opengloves_communication_device **out_ocd) +{ + char addr[OPENGLOVES_BT_MAX_ADDRESS_LEN] = {0}; + char name[OPENGLOVES_BT_MAX_NAME_LEN] = {0}; + + int dev_id = hci_get_route(NULL); + int sock = hci_open_dev(dev_id); + + if (dev_id < 0 || sock < 0) { + OPENGLOVES_ERROR("Failed to open socket!"); + + return -1; + } + + int max_rsp = OPENGLOVES_BT_MAX_DEVICES; // maximum devices to find + inquiry_info *ii = U_TYPED_ARRAY_CALLOC(inquiry_info, max_rsp); + + int num_rsp = hci_inquiry(dev_id, 1, max_rsp, NULL, &ii, IREQ_CACHE_FLUSH); + + if (num_rsp < 0) { + OPENGLOVES_ERROR("device inquiry failed!"); + + free(ii); + close(sock); + + return -1; + } + + for (int i = 0; i < num_rsp; i++) { + ba2str(&(ii + i)->bdaddr, addr); + memset(name, 0, sizeof(name)); + + hci_read_remote_name(sock, &(ii + i)->bdaddr, sizeof(name), name, 0); + + if (!strcmp(name, bt_name) && *out_ocd == NULL) { + OPENGLOVES_INFO("Found bt device! %s", name); + + opengloves_bt_open(addr, out_ocd); + + break; + } + } + + free(ii); + close(sock); + return 0; +} diff --git a/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_prober_bt.h b/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_prober_bt.h new file mode 100644 index 000000000..f91a0624c --- /dev/null +++ b/src/xrt/drivers/opengloves/communication/bluetooth/opengloves_prober_bt.h @@ -0,0 +1,26 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves bluetooth prober. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#pragma once + +#include "../opengloves_communication.h" + +#define LUCIDGLOVES_BT_L_NAME "lucidgloves-left" +#define LUCIDGLOVES_BT_R_NAME "lucidgloves-right" + +#ifdef __cplusplus +extern "C" { +#endif + +int +opengloves_get_bt_devices(const char *bt_name, struct opengloves_communication_device **out_ocd); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/opengloves/communication/opengloves_communication.h b/src/xrt/drivers/opengloves/communication/opengloves_communication.h new file mode 100644 index 000000000..4560e038e --- /dev/null +++ b/src/xrt/drivers/opengloves/communication/opengloves_communication.h @@ -0,0 +1,58 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Communication structures for OpenGloves + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#pragma once +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/*! + * @interface opengloves_communication_device + * + * Interface for a communication method + * + * @ingroup drv_opengloves + */ +struct opengloves_communication_device +{ + int (*read)(struct opengloves_communication_device *comm_dev, char *data, size_t size); + + int (*write)(struct opengloves_communication_device *comm_dev, const uint8_t *data, size_t size); + + void (*destroy)(struct opengloves_communication_device *comm_dev); +}; + +static inline int +opengloves_communication_device_read(struct opengloves_communication_device *comm_dev, char *data, size_t size) +{ + return comm_dev->read(comm_dev, data, size); +} + +static inline int +opengloves_communication_device_write(struct opengloves_communication_device *comm_dev, + const uint8_t *data, + size_t size) +{ + return comm_dev->write(comm_dev, data, size); +} + + +static inline void +opengloves_communication_device_destory(struct opengloves_communication_device *comm_dev) +{ + comm_dev->destroy(comm_dev); +} + + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/opengloves/communication/serial/opengloves_prober_serial.c b/src/xrt/drivers/opengloves/communication/serial/opengloves_prober_serial.c new file mode 100644 index 000000000..a43c99673 --- /dev/null +++ b/src/xrt/drivers/opengloves/communication/serial/opengloves_prober_serial.c @@ -0,0 +1,107 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves serial prober implementation. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#include +#include +#include +#include + +#include "util/u_debug.h" +#include "xrt/xrt_defines.h" + +#include "opengloves_prober_serial.h" +#include "opengloves_serial.h" + +#define OPENGLOVES_PROBER_LOG_LEVEL U_LOGGING_TRACE + +#define OPENGLOVES_ERROR(...) U_LOG_IFL_E(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) +#define OPENGLOVES_INFO(...) U_LOG_IFL_I(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) + +#define OPENGLOVES_TTY_PATH_SIZE 14 + +static int +opengloves_udev_get_sysattr_u16_base16(struct udev_device *dev, const char *name, uint16_t *out_value) +{ + const char *str = udev_device_get_sysattr_value(dev, name); + if (str == NULL) { + return -1; + } + + *out_value = (uint16_t)strtol(str, NULL, 16); + + return 0; +} + +static int +opengloves_serial_device_found(const char *sysfs_path, struct opengloves_communication_device **ocdev) +{ + // ttyUSBx comes after the last / in sysfs_path + const char *tty_name = strrchr(sysfs_path, '/') + 1; + + char tty_path[OPENGLOVES_TTY_PATH_SIZE] = {0}; + int cx = snprintf(tty_path, OPENGLOVES_TTY_PATH_SIZE, "/dev/%s", tty_name); + + if (cx < 0) { + OPENGLOVES_ERROR("Failed to create tty path!"); + return 0; + } + + OPENGLOVES_INFO("Device discovered! Attempting connection to %s", tty_path); + + int ret = opengloves_serial_open(tty_path, ocdev); + if (ret < 0) { + OPENGLOVES_ERROR("Failed to connect to serial device, %s", strerror(-ret)); + return 0; + } + + OPENGLOVES_INFO("Successfully connected to device"); + + return 1; +} + +int +opengloves_get_serial_devices(uint16_t vid, uint16_t pid, struct opengloves_communication_device **out_ocd) +{ + struct udev *ud = udev_new(); + + struct udev_enumerate *tty_enumerate = udev_enumerate_new(ud); + + udev_enumerate_add_match_subsystem(tty_enumerate, "tty"); + udev_enumerate_scan_devices(tty_enumerate); + + struct udev_list_entry *tty_devices; + tty_devices = udev_enumerate_get_list_entry(tty_enumerate); + + struct udev_list_entry *tty_dev_list_entry; + + int dev_count = 0; + udev_list_entry_foreach(tty_dev_list_entry, tty_devices) + { + const char *sysfs_path = udev_list_entry_get_name(tty_dev_list_entry); + struct udev_device *raw_dev = udev_device_new_from_syspath(ud, sysfs_path); + + struct udev_device *parent_dev = raw_dev; + while (parent_dev != NULL) { + uint16_t vendor_id; + uint16_t product_id; + opengloves_udev_get_sysattr_u16_base16(parent_dev, "idVendor", &vendor_id); + opengloves_udev_get_sysattr_u16_base16(parent_dev, "idProduct", &product_id); + + // if vendor and product id match what was requested + if (vendor_id == vid && product_id == pid && *out_ocd == NULL) + dev_count = dev_count + opengloves_serial_device_found(sysfs_path, out_ocd); + + parent_dev = udev_device_get_parent(parent_dev); + } + } + + udev_enumerate_unref(tty_enumerate); + + return dev_count; +} diff --git a/src/xrt/drivers/opengloves/communication/serial/opengloves_prober_serial.h b/src/xrt/drivers/opengloves/communication/serial/opengloves_prober_serial.h new file mode 100644 index 000000000..5a18d38c0 --- /dev/null +++ b/src/xrt/drivers/opengloves/communication/serial/opengloves_prober_serial.h @@ -0,0 +1,27 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Serial prober interface for OpenGloves. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#pragma once + +#include "../opengloves_communication.h" + +#define LUCIDGLOVES_USB_VID 0x1a86 +#define LUCIDGLOVES_USB_L_PID 0x7523 // left hand pid +#define LUCIDGLOVES_USB_R_PID 0x7524 // right hand pid +#ifdef __cplusplus +extern "C" { +#endif + + +int +opengloves_get_serial_devices(uint16_t vid, uint16_t pid, struct opengloves_communication_device **out_ocd); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/opengloves/communication/serial/opengloves_serial.c b/src/xrt/drivers/opengloves/communication/serial/opengloves_serial.c new file mode 100644 index 000000000..49c1c42f4 --- /dev/null +++ b/src/xrt/drivers/opengloves/communication/serial/opengloves_serial.c @@ -0,0 +1,106 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief USB Serial implementation for OpenGloves. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#include +#include +#include + +#include +#include +#include +#include + +#include "opengloves_serial.h" + +#include "util/u_misc.h" + + +static int +opengloves_serial_read(struct opengloves_communication_device *ocdev, char *data, size_t length) +{ + struct opengloves_serial_device *osdev = (struct opengloves_serial_device *)ocdev; + int ret = read(osdev->fd, data, length); + + return ret; +} + +static int +opengloves_serial_write(struct opengloves_communication_device *ocdev, const uint8_t *data, size_t length) +{ + struct opengloves_serial_device *osdev = (struct opengloves_serial_device *)ocdev; + + return write(osdev->fd, data, length); +} + +static void +opengloves_serial_destroy(struct opengloves_communication_device *ocdev) +{ + struct opengloves_serial_device *osdev = (struct opengloves_serial_device *)ocdev; + + close(osdev->fd); + free(osdev); +} + +int +opengloves_serial_open(const char *path, struct opengloves_communication_device **out_comm_dev) +{ + int fd = open(path, O_RDWR); + + // error opening file + if (fd < 0) { + return -errno; + } + + // read existing settings + struct termios tty; + if (tcgetattr(fd, &tty) != 0) { + return -errno; + } + + tty.c_cflag &= ~PARENB; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; + tty.c_cflag |= CREAD | CLOCAL; + tty.c_cflag &= ~CRTSCTS; + + tty.c_lflag &= ~ICANON; + tty.c_lflag &= ~ECHO; + tty.c_lflag &= ~ECHOE; + tty.c_lflag &= ~ECHONL; + tty.c_lflag &= ~ISIG; + tty.c_iflag &= ~(IXON | IXOFF | IXANY); + tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL); + + tty.c_oflag &= ~OPOST; + tty.c_oflag &= ~ONLCR; + + tty.c_cc[VTIME] = 10; + tty.c_cc[VMIN] = 0; + + // baud rates + cfsetispeed(&tty, B115200); + cfsetospeed(&tty, B115200); + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + return -errno; + } + + struct opengloves_serial_device *osdev = U_TYPED_CALLOC(struct opengloves_serial_device); + + osdev->base.read = opengloves_serial_read; + osdev->base.write = opengloves_serial_write; + osdev->base.destroy = opengloves_serial_destroy; + + osdev->fd = fd; + + *out_comm_dev = &osdev->base; + + return 0; +} diff --git a/src/xrt/drivers/opengloves/communication/serial/opengloves_serial.h b/src/xrt/drivers/opengloves/communication/serial/opengloves_serial.h new file mode 100644 index 000000000..dd37c72de --- /dev/null +++ b/src/xrt/drivers/opengloves/communication/serial/opengloves_serial.h @@ -0,0 +1,31 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief USB Serial interface for OpenGloves. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#pragma once +#include +#include + +#include "../opengloves_communication.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct opengloves_serial_device +{ + struct opengloves_communication_device base; + int fd; +}; + +int +opengloves_serial_open(const char *path, struct opengloves_communication_device **out_comm_dev); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/opengloves/encoding/alpha_encoding.cpp b/src/xrt/drivers/opengloves/encoding/alpha_encoding.cpp new file mode 100644 index 000000000..e830fae32 --- /dev/null +++ b/src/xrt/drivers/opengloves/encoding/alpha_encoding.cpp @@ -0,0 +1,250 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves Alpha Encoding Decoding implementation. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#include +#include + +#include +#include "util/u_logging.h" + +#include "alpha_encoding.h" +#include "encoding.h" + +enum opengloves_alpha_encoding_key +{ + OPENGLOVES_ALPHA_ENCODING_FinThumb, + OPENGLOVES_ALPHA_ENCODING_FinSplayThumb, + + OPENGLOVES_ALPHA_ENCODING_FinIndex, + OPENGLOVES_ALPHA_ENCODING_FinSplayIndex, + + OPENGLOVES_ALPHA_ENCODING_FinMiddle, + OPENGLOVES_ALPHA_ENCODING_FinSplayMiddle, + + OPENGLOVES_ALPHA_ENCODING_FinRing, + OPENGLOVES_ALPHA_ENCODING_FinSplayRing, + + OPENGLOVES_ALPHA_ENCODING_FinPinky, + OPENGLOVES_ALPHA_ENCODING_FinSplayPinky, + + OPENGLOVES_ALPHA_ENCODING_FinJointThumb0, + OPENGLOVES_ALPHA_ENCODING_FinJointThumb1, + OPENGLOVES_ALPHA_ENCODING_FinJointThumb2, + OPENGLOVES_ALPHA_ENCODING_FinJointThumb3, // unused in input but used for parity to other fingers in the array + + + OPENGLOVES_ALPHA_ENCODING_FinJointIndex0, + OPENGLOVES_ALPHA_ENCODING_FinJointIndex1, + OPENGLOVES_ALPHA_ENCODING_FinJointIndex2, + OPENGLOVES_ALPHA_ENCODING_FinJointIndex3, + + + OPENGLOVES_ALPHA_ENCODING_FinJointMiddle0, + OPENGLOVES_ALPHA_ENCODING_FinJointMiddle1, + OPENGLOVES_ALPHA_ENCODING_FinJointMiddle2, + OPENGLOVES_ALPHA_ENCODING_FinJointMiddle3, + + + OPENGLOVES_ALPHA_ENCODING_FinJointRing0, + OPENGLOVES_ALPHA_ENCODING_FinJointRing1, + OPENGLOVES_ALPHA_ENCODING_FinJointRing2, + OPENGLOVES_ALPHA_ENCODING_FinJointRing3, + + + OPENGLOVES_ALPHA_ENCODING_FinJointPinky0, + OPENGLOVES_ALPHA_ENCODING_FinJointPinky1, + OPENGLOVES_ALPHA_ENCODING_FinJointPinky2, + OPENGLOVES_ALPHA_ENCODING_FinJointPinky3, + + OPENGLOVES_ALPHA_ENCODING_JoyX, + OPENGLOVES_ALPHA_ENCODING_JoyY, + OPENGLOVES_ALPHA_ENCODING_JoyBtn, + + OPENGLOVES_ALPHA_ENCODING_TrgValue, + OPENGLOVES_ALPHA_ENCODING_BtnTrg, + OPENGLOVES_ALPHA_ENCODING_BtnA, + OPENGLOVES_ALPHA_ENCODING_BtnB, + + OPENGLOVES_ALPHA_ENCODING_GesGrab, + OPENGLOVES_ALPHA_ENCODING_GesPinch, + + OPENGLOVES_ALPHA_ENCODING_BtnMenu, + OPENGLOVES_ALPHA_ENCODING_BtnCalib, + + OPENGLOVES_ALPHA_ENCODING_MAX +}; + +#define OPENGLOVES_ALPHA_ENCODING_VAL_IN_MAP_E_0 + +static const std::string opengloves_alpha_encoding_key_characters = "ABCDEFGHIJKLMNOPQRSTUVWXYZ()"; + +static bool +opengloves_alpha_encoding_is_key_character(const char character) +{ + return opengloves_alpha_encoding_key_characters.find(character) != std::string::npos; +} + +static const std::map opengloves_alpha_encoding_key_string{ + {"A", OPENGLOVES_ALPHA_ENCODING_FinThumb}, // whole thumb curl (default curl value for thumb joints) + {"(AB)", OPENGLOVES_ALPHA_ENCODING_FinSplayThumb}, // whole thumb splay thumb joint 3 (doesn't exist, but keeps + // consistency with the other fingers + {"B", OPENGLOVES_ALPHA_ENCODING_FinIndex}, // whole index curl (default curl value for index joints) + {"(BB)", OPENGLOVES_ALPHA_ENCODING_FinSplayIndex}, // whole index splay + + {"C", OPENGLOVES_ALPHA_ENCODING_FinMiddle}, // whole middle curl (default curl value for middle joints) + {"(CB)", OPENGLOVES_ALPHA_ENCODING_FinSplayMiddle}, // whole middle splay + + {"D", OPENGLOVES_ALPHA_ENCODING_FinRing}, // whole ring curl (default curl value for + {"(DB)", OPENGLOVES_ALPHA_ENCODING_FinSplayRing}, // whole ring splay + // ring joints) + {"E", OPENGLOVES_ALPHA_ENCODING_FinPinky}, // whole pinky curl (default curl value + {"(EB)", OPENGLOVES_ALPHA_ENCODING_FinSplayPinky}, // whole pinky splay + // for pinky joints + {"(AAA)", OPENGLOVES_ALPHA_ENCODING_FinJointThumb0}, // thumb joint 0 + {"(AAB)", OPENGLOVES_ALPHA_ENCODING_FinJointThumb1}, // thumb joint 1 + {"(AAC)", OPENGLOVES_ALPHA_ENCODING_FinJointThumb2}, // thumb joint 2 + {"(AAD)", OPENGLOVES_ALPHA_ENCODING_FinJointThumb3}, + {"(BAA)", OPENGLOVES_ALPHA_ENCODING_FinJointIndex0}, // index joint 0 + {"(BAB)", OPENGLOVES_ALPHA_ENCODING_FinJointIndex1}, // index joint 1 + {"(BAC)", OPENGLOVES_ALPHA_ENCODING_FinJointIndex2}, // index joint 2 + {"(BAD)", OPENGLOVES_ALPHA_ENCODING_FinJointIndex3}, // index joint 3 + {"(CAA)", OPENGLOVES_ALPHA_ENCODING_FinJointMiddle0}, // middle joint 0 + {"(CAB)", OPENGLOVES_ALPHA_ENCODING_FinJointMiddle1}, // middle joint 1 + {"(CAC)", OPENGLOVES_ALPHA_ENCODING_FinJointMiddle2}, // middle joint 2 + {"(CAD)", OPENGLOVES_ALPHA_ENCODING_FinJointMiddle3}, // middle joint 3 + {"(DAA)", OPENGLOVES_ALPHA_ENCODING_FinJointRing0}, // ring joint 0 + {"(DAB)", OPENGLOVES_ALPHA_ENCODING_FinJointRing1}, // ring joint 1 + {"(DAC)", OPENGLOVES_ALPHA_ENCODING_FinJointRing2}, // ring joint 2 + {"(DAD)", OPENGLOVES_ALPHA_ENCODING_FinJointRing3}, // ring joint 3 + {"(EAA)", OPENGLOVES_ALPHA_ENCODING_FinJointPinky0}, // pinky joint 0 + {"(EAB)", OPENGLOVES_ALPHA_ENCODING_FinJointPinky1}, // pinky joint 1 + {"(EAC)", OPENGLOVES_ALPHA_ENCODING_FinJointPinky2}, // pinky joint 2 + {"(EAD)", OPENGLOVES_ALPHA_ENCODING_FinJointPinky3}, // pinky joint 3 + {"F", OPENGLOVES_ALPHA_ENCODING_JoyX}, // joystick x component + {"G", OPENGLOVES_ALPHA_ENCODING_JoyY}, // joystick y component + {"H", OPENGLOVES_ALPHA_ENCODING_JoyBtn}, // joystick button + {"I", OPENGLOVES_ALPHA_ENCODING_BtnTrg}, // trigger button + {"J", OPENGLOVES_ALPHA_ENCODING_BtnA}, // A button + {"K", OPENGLOVES_ALPHA_ENCODING_BtnB}, // B button + {"L", OPENGLOVES_ALPHA_ENCODING_GesGrab}, // grab gesture (boolean) + {"M", OPENGLOVES_ALPHA_ENCODING_GesPinch}, // pinch gesture (boolean) + {"N", OPENGLOVES_ALPHA_ENCODING_BtnMenu}, // system button pressed (opens SteamVR menu) + {"O", OPENGLOVES_ALPHA_ENCODING_BtnCalib}, // calibration button + {"P", OPENGLOVES_ALPHA_ENCODING_TrgValue}, // analog trigger value + {"", OPENGLOVES_ALPHA_ENCODING_MAX} // Junk key +}; + +static std::map +opengloves_alpha_encoding_parse_to_map(const std::string &str) +{ + std::map result; + + size_t i = 0; + while (i < str.length()) { + // Advance until we get an alphabetic character (no point in looking at values that don't have a key + // associated with them) + + if (str[i] >= 0 && opengloves_alpha_encoding_is_key_character(str[i])) { + std::string key = {str[i]}; + i++; + + // we're going to be parsing a "long key", i.e. (AB) for thumb finger splay. Long keys must + // always be enclosed in brackets + if (key[0] == '(') { + while (str[i] >= 0 && opengloves_alpha_encoding_is_key_character(str[i]) && + i < str.length()) { + key += str[i]; + i++; + } + } + + std::string value; + while (str[i] >= 0 && isdigit(str[i]) && i < str.length()) { + value += str[i]; + i++; + } + + // Even if the value is empty we still want to use the key, it means that we have a button that + // is pressed (it only appears in the packet if it is) + if (opengloves_alpha_encoding_key_string.find(key) != + opengloves_alpha_encoding_key_string.end()) + result.insert_or_assign(opengloves_alpha_encoding_key_string.at(key), value); + else + U_LOG_W("Unable to insert key: %s into input map as it was not found", key.c_str()); + } else + i++; + } + + return result; +} + + +void +opengloves_alpha_encoding_decode(const char *data, struct opengloves_input *out) +{ + std::map input_map = opengloves_alpha_encoding_parse_to_map(data); + + try { + // five fingers, 2 (curl + splay) + for (int i = 0; i < 5; i++) { + int enum_position = i * 2; + // curls + if (input_map.find(enum_position) != input_map.end()) { + float fin_curl_value = std::stof(input_map.at(enum_position)); + std::fill(std::begin(out->flexion[i]), std::begin(out->flexion[i]) + 4, + fin_curl_value / OPENGLOVES_ENCODING_MAX_ANALOG_VALUE); + } + + // splay + if (input_map.find(enum_position + 1) != input_map.end()) + out->splay[i] = + (std::stof(input_map.at(enum_position + 1)) / OPENGLOVES_ENCODING_MAX_ANALOG_VALUE - + 0.5f) * + 2.0f; + } + + int current_finger_joint = OPENGLOVES_ALPHA_ENCODING_FinJointThumb0; + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 4; j++) { + // individual joint curls + out->flexion[i][j] = input_map.find(current_finger_joint) != input_map.end() + ? (std::stof(input_map.at(current_finger_joint)) / + OPENGLOVES_ENCODING_MAX_ANALOG_VALUE) + // use the curl of the previous joint + : out->flexion[i][j > 0 ? j - 1 : 0]; + current_finger_joint++; + } + } + + // joysticks + if (input_map.find(OPENGLOVES_ALPHA_ENCODING_JoyX) != input_map.end()) + out->joysticks.main.x = 2 * std::stof(input_map.at(OPENGLOVES_ALPHA_ENCODING_JoyX)) / + OPENGLOVES_ENCODING_MAX_ANALOG_VALUE - + 1; + if (input_map.find(OPENGLOVES_ALPHA_ENCODING_JoyY) != input_map.end()) + out->joysticks.main.y = 2 * std::stof(input_map.at(OPENGLOVES_ALPHA_ENCODING_JoyY)) / + OPENGLOVES_ENCODING_MAX_ANALOG_VALUE - + 1; + out->joysticks.main.pressed = input_map.find(OPENGLOVES_ALPHA_ENCODING_JoyBtn) != input_map.end(); + + } catch (std::invalid_argument &e) { + U_LOG_E("Error parsing input string: %s", e.what()); + } + + if (input_map.find(OPENGLOVES_ALPHA_ENCODING_TrgValue) != input_map.end()) + out->buttons.trigger.value = + std::stof(input_map.at(OPENGLOVES_ALPHA_ENCODING_TrgValue)) / OPENGLOVES_ENCODING_MAX_ANALOG_VALUE; + out->buttons.trigger.pressed = input_map.find(OPENGLOVES_ALPHA_ENCODING_BtnTrg) != input_map.end(); + + out->buttons.A.pressed = input_map.find(OPENGLOVES_ALPHA_ENCODING_BtnA) != input_map.end(); + out->buttons.B.pressed = input_map.find(OPENGLOVES_ALPHA_ENCODING_BtnB) != input_map.end(); + out->gestures.grab.activated = input_map.find(OPENGLOVES_ALPHA_ENCODING_GesGrab) != input_map.end(); + out->gestures.pinch.activated = input_map.find(OPENGLOVES_ALPHA_ENCODING_GesPinch) != input_map.end(); + out->buttons.menu.pressed = input_map.find(OPENGLOVES_ALPHA_ENCODING_BtnMenu) != input_map.end(); +} \ No newline at end of file diff --git a/src/xrt/drivers/opengloves/encoding/alpha_encoding.h b/src/xrt/drivers/opengloves/encoding/alpha_encoding.h new file mode 100644 index 000000000..1ca7a1dd8 --- /dev/null +++ b/src/xrt/drivers/opengloves/encoding/alpha_encoding.h @@ -0,0 +1,22 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves Alpha Encoding Decoding interface. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#pragma once +#include "encoding.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void +opengloves_alpha_encoding_decode(const char *data, struct opengloves_input *out_kv); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/opengloves/encoding/encoding.h b/src/xrt/drivers/opengloves/encoding/encoding.h new file mode 100644 index 000000000..682620199 --- /dev/null +++ b/src/xrt/drivers/opengloves/encoding/encoding.h @@ -0,0 +1,71 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Defines for OpenGloves internal inputs + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#pragma once +#include + +#define OPENGLOVES_ENCODING_MAX_ANALOG_VALUE 1023.0f +#define OPENGLOVES_ENCODING_MAX_PACKET_SIZE 150 + +#ifdef __cplusplus +extern "C" { +#endif + + +struct opengloves_input_button +{ + float value; + bool pressed; +}; + +struct opengloves_input_joystick +{ + float x; + float y; + bool pressed; +}; + +struct opengloves_input_gesture +{ + bool activated; +}; + +struct opengloves_input_buttons +{ + struct opengloves_input_button A; + struct opengloves_input_button B; + struct opengloves_input_button trigger; + struct opengloves_input_button menu; +}; + +struct opengloves_input_joysticks +{ + struct opengloves_input_joystick main; +}; + +struct opengloves_input_gestures +{ + struct opengloves_input_gesture grab; + struct opengloves_input_gesture pinch; +}; + + +struct opengloves_input +{ + float flexion[5][5]; + float splay[5]; + + struct opengloves_input_joysticks joysticks; + struct opengloves_input_buttons buttons; + struct opengloves_input_gestures gestures; +}; + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/xrt/drivers/opengloves/opengloves_device.c b/src/xrt/drivers/opengloves/opengloves_device.c new file mode 100644 index 000000000..c2833e81d --- /dev/null +++ b/src/xrt/drivers/opengloves/opengloves_device.c @@ -0,0 +1,290 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves device implementation. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#include + +#include "xrt/xrt_device.h" +#include "xrt/xrt_defines.h" + +#include "util/u_device.h" +#include "util/u_debug.h" +#include "util/u_hand_tracking.h" +#include "util/u_logging.h" +#include "util/u_misc.h" +#include "util/u_var.h" + +#include "opengloves_device.h" + +#include "communication/opengloves_communication.h" +#include "encoding/alpha_encoding.h" + +DEBUG_GET_ONCE_LOG_OPTION(opengloves_log, "OPENGLOVES_LOG", U_LOGGING_INFO) + + +#include "os/os_threading.h" + +#define OPENGLOVES_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) +#define OPENGLOVES_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) +#define OPENGLOVES_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) +#define OPENGLOVES_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) +#define OPENGLOVES_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) + +enum opengloves_input_index +{ + OPENGLOVES_INDEX_HAND_TRACKING, + + OPENGLOVES_INDEX_TRIGGER_CLICK, + OPENGLOVES_INDEX_TRIGGER_VALUE, + + OPENGLOVES_INDEX_A_CLICK, + OPENGLOVES_INDEX_B_CLICK, + + OPENGLOVES_INDEX_JOYSTICK_MAIN, + OPENGLOVES_INDEX_JOYSTICK_MAIN_CLICK, + + OPENGLOVES_INDEX_COUNT +}; + +/*! + * @implements xrt_device + */ +struct opengloves_device +{ + struct xrt_device base; + struct opengloves_communication_device *ocd; + + struct os_thread_helper oth; + struct os_mutex lock; + + struct opengloves_input *last_input; + + enum xrt_hand hand; + + struct u_hand_tracking hand_tracking; + + enum u_logging_level log_level; +}; + +static inline struct opengloves_device * +opengloves_device(struct xrt_device *xdev) +{ + return (struct opengloves_device *)xdev; +} + +static void +opengloves_device_get_hand_tracking(struct xrt_device *xdev, + enum xrt_input_name name, + uint64_t requested_timestamp_ns, + struct xrt_hand_joint_set *out_joint_set, + uint64_t *out_timestamp_ns) +{ + struct opengloves_device *od = opengloves_device(xdev); + + enum xrt_hand hand = od->hand; + + struct xrt_vec3 static_offset = {0, 0, 0}; + struct u_hand_tracking_values values = {.little = + { + .splay = od->last_input->splay[4], + .joint_count = 5, + }, + .ring = + { + .splay = od->last_input->splay[3], + .joint_count = 5, + }, + .middle = + { + .splay = od->last_input->splay[2], + .joint_count = 5, + }, + .index = + { + .splay = od->last_input->splay[1], + .joint_count = 5, + }, + .thumb = { + .splay = od->last_input->splay[0], + .joint_count = 4, + }}; + // copy in the curls + memcpy(values.little.joint_curls, od->last_input->flexion[4], sizeof(od->last_input->flexion[4])); + memcpy(values.ring.joint_curls, od->last_input->flexion[3], sizeof(od->last_input->flexion[3])); + memcpy(values.middle.joint_curls, od->last_input->flexion[2], sizeof(od->last_input->flexion[2])); + memcpy(values.index.joint_curls, od->last_input->flexion[1], sizeof(od->last_input->flexion[1])); + memcpy(values.thumb.joint_curls, od->last_input->flexion[0], sizeof(od->last_input->flexion[0])); + + u_hand_joints_update(&od->hand_tracking, hand, requested_timestamp_ns, &values); + + struct xrt_space_relation controller_relation = {.pose = {.orientation.w = 1.0f, .position = {0, 0, 0}}}; + controller_relation.relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_POSITION_VALID_BIT; + + struct xrt_pose hand_on_handle_pose; + u_hand_joints_offset_valve_index_controller(hand, &static_offset, &hand_on_handle_pose); + u_hand_joints_set_out_data(&od->hand_tracking, hand, &controller_relation, &hand_on_handle_pose, out_joint_set); + + + *out_timestamp_ns = requested_timestamp_ns; + out_joint_set->is_active = true; +} + +static void +opengloves_device_update_inputs(struct xrt_device *xdev) +{ + struct opengloves_device *od = opengloves_device(xdev); + + os_mutex_lock(&od->lock); + + od->base.inputs[OPENGLOVES_INDEX_A_CLICK].value.boolean = od->last_input->buttons.A.pressed; + od->base.inputs[OPENGLOVES_INDEX_B_CLICK].value.boolean = od->last_input->buttons.B.pressed; + + od->base.inputs[OPENGLOVES_INDEX_TRIGGER_CLICK].value.boolean = od->last_input->buttons.trigger.pressed; + od->base.inputs[OPENGLOVES_INDEX_TRIGGER_VALUE].value.vec1.x = od->last_input->buttons.trigger.value; + + od->base.inputs[OPENGLOVES_INDEX_JOYSTICK_MAIN].value.vec2.x = od->last_input->joysticks.main.x; + od->base.inputs[OPENGLOVES_INDEX_JOYSTICK_MAIN].value.vec2.y = od->last_input->joysticks.main.y; + od->base.inputs[OPENGLOVES_INDEX_JOYSTICK_MAIN_CLICK].value.boolean = od->last_input->joysticks.main.pressed; + + os_mutex_unlock(&od->lock); +} + +static void +opengloves_device_destroy(struct xrt_device *xdev) +{ + struct opengloves_device *od = opengloves_device(xdev); + + os_thread_helper_destroy(&od->oth); + + os_mutex_destroy(&od->lock); + + opengloves_communication_device_destory(od->ocd); + + free(od->last_input); + free(od); +} + + +/*! + * Reads the next packet from the device, finishing successfully when reaching a newline + * Returns true if finished at a newline, or false if there was an error + */ +static bool +opengloves_read_next_packet(struct opengloves_device *od, char *buffer, int buffer_len) +{ + os_thread_helper_lock(&od->oth); + + char next_char = 0; + int i = 0; + do { + // try read one byte + int ret = opengloves_communication_device_read(od->ocd, &next_char, 1); + if (ret < 0) { + OPENGLOVES_ERROR(od, "Failed to read from device! %s", strerror(ret)); + os_thread_helper_unlock(&od->oth); + return false; + } + + if (next_char == 0 || next_char == '\n') + continue; + + buffer[i++] = next_char; + } while (next_char != '\n' && i < buffer_len); + + // null terminate + buffer[i] = '\0'; + + OPENGLOVES_DEBUG(od, "%s -> len %i", buffer, i); + + os_thread_helper_unlock(&od->oth); + + return true; +} + +/*! + * Main thread for reading data from the device + */ +static void * +opengloves_run_thread(void *ptr) +{ + struct opengloves_device *od = (struct opengloves_device *)ptr; + + char buffer[OPENGLOVES_ENCODING_MAX_PACKET_SIZE]; + + while (opengloves_read_next_packet(od, buffer, OPENGLOVES_ENCODING_MAX_PACKET_SIZE) && + os_thread_helper_is_running(&od->oth)) { + os_mutex_lock(&od->lock); + opengloves_alpha_encoding_decode(buffer, od->last_input); + os_mutex_unlock(&od->lock); + } + + return 0; +} + +struct xrt_device * +opengloves_device_create(struct opengloves_communication_device *ocd, enum xrt_hand hand) +{ + enum u_device_alloc_flags flags = (enum u_device_alloc_flags)(U_DEVICE_ALLOC_TRACKING_NONE); + struct opengloves_device *od = U_DEVICE_ALLOCATE(struct opengloves_device, flags, 8, 0); + + od->base.name = XRT_DEVICE_HAND_TRACKER; + od->base.device_type = XRT_DEVICE_TYPE_HAND_TRACKER; + od->hand = hand; + + od->ocd = ocd; + od->base.destroy = opengloves_device_destroy; + os_mutex_init(&od->lock); + + // hand tracking + od->base.get_hand_tracking = opengloves_device_get_hand_tracking; + od->base.inputs[OPENGLOVES_INDEX_HAND_TRACKING].name = + od->hand == XRT_HAND_LEFT ? XRT_INPUT_GENERIC_HAND_TRACKING_LEFT : XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT; + + u_hand_joints_init_default_set(&od->hand_tracking, hand, XRT_HAND_TRACKING_MODEL_INTRINSIC, 1.0); + od->base.hand_tracking_supported = true; + + // inputs + od->base.update_inputs = opengloves_device_update_inputs; + od->last_input = U_TYPED_CALLOC(struct opengloves_input); + + + od->base.inputs[OPENGLOVES_INDEX_A_CLICK].name = XRT_INPUT_INDEX_A_CLICK; + od->base.inputs[OPENGLOVES_INDEX_B_CLICK].name = XRT_INPUT_INDEX_B_CLICK; + + od->base.inputs[OPENGLOVES_INDEX_TRIGGER_VALUE].name = XRT_INPUT_INDEX_TRIGGER_VALUE; + od->base.inputs[OPENGLOVES_INDEX_TRIGGER_CLICK].name = XRT_INPUT_INDEX_TRIGGER_CLICK; + + od->base.inputs[OPENGLOVES_INDEX_JOYSTICK_MAIN].name = XRT_INPUT_INDEX_THUMBSTICK; + od->base.inputs[OPENGLOVES_INDEX_JOYSTICK_MAIN_CLICK].name = XRT_INPUT_INDEX_THUMBSTICK_CLICK; + + // startup thread + int ret = os_thread_helper_init(&od->oth); + if (ret != 0) { + OPENGLOVES_ERROR(od, "Failed to initialise threading!"); + opengloves_device_destroy(&od->base); + return NULL; + } + + ret = os_thread_helper_start(&od->oth, opengloves_run_thread, od); + if (ret != 0) { + OPENGLOVES_ERROR(od, "Failed to start thread!"); + opengloves_device_destroy(&od->base); + + return 0; + } + + u_var_add_root(od, "OpenGloves VR glove device", true); + snprintf(od->base.serial, XRT_DEVICE_NAME_LEN, "OpenGloves %s", hand == XRT_HAND_LEFT ? "Left" : "Right"); + + od->log_level = debug_get_log_option_opengloves_log(); + + + return &od->base; +} diff --git a/src/xrt/drivers/opengloves/opengloves_device.h b/src/xrt/drivers/opengloves/opengloves_device.h new file mode 100644 index 000000000..cf225d357 --- /dev/null +++ b/src/xrt/drivers/opengloves/opengloves_device.h @@ -0,0 +1,24 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves device. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#pragma once +#include "util/u_logging.h" +#include "xrt/xrt_device.h" +#include "communication/opengloves_communication.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct xrt_device * +opengloves_device_create(struct opengloves_communication_device *ocd, enum xrt_hand hand); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/opengloves/opengloves_interface.h b/src/xrt/drivers/opengloves/opengloves_interface.h new file mode 100644 index 000000000..a82ba4289 --- /dev/null +++ b/src/xrt/drivers/opengloves/opengloves_interface.h @@ -0,0 +1,36 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves device interface. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +struct u_system_devices; + +/*! + * @defgroup drv_opengloves OpenGloves Driver for VR Gloves + * @ingroup drv + * + * @brief Driver for OpenGloves VR Gloves Devices + */ + +int +opengloves_create_devices(struct xrt_device **out_xdevs, const struct xrt_system_devices *sysdevs); + +/*! + * @dir drivers/opengloves + * + * @brief @ref drv_opengloves files. + */ + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/opengloves/opengloves_prober.c b/src/xrt/drivers/opengloves/opengloves_prober.c new file mode 100644 index 000000000..912794be8 --- /dev/null +++ b/src/xrt/drivers/opengloves/opengloves_prober.c @@ -0,0 +1,116 @@ +// Copyright 2019-2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief OpenGloves prober implementation. + * @author Daniel Willmott + * @ingroup drv_opengloves + */ + +#include "xrt/xrt_prober.h" +#include "xrt/xrt_defines.h" + +#include "util/u_config_json.h" +#include "util/u_debug.h" +#include "util/u_system_helpers.h" + + +#include "opengloves_interface.h" +#include "opengloves_device.h" + +#include "communication/serial/opengloves_prober_serial.h" +#include "communication/bluetooth/opengloves_prober_bt.h" + +#include "../multi_wrapper/multi.h" + +#define OPENGLOVES_PROBER_LOG_LEVEL U_LOGGING_TRACE + +#define OPENGLOVES_ERROR(...) U_LOG_IFL_E(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) +#define OPENGLOVES_INFO(...) U_LOG_IFL_I(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__) + +#define JSON_VEC3(a, b, c) u_json_get_vec3_array(u_json_get(a, b), c) +#define JSON_QUAT(a, b, c) u_json_get_quat(u_json_get(a, b), c) + + +static const cJSON * +opengloves_load_config_file(struct u_config_json config_json) +{ + u_config_json_open_or_create_main_file(&config_json); + if (!config_json.file_loaded) { + OPENGLOVES_ERROR("Failed to load config file"); + return NULL; + } + + const cJSON *out_config_json = u_json_get(config_json.root, "config_opengloves"); + if (out_config_json == NULL) { + return NULL; + } + + return out_config_json; +} + +int +opengloves_create_devices(struct xrt_device **out_xdevs, const struct xrt_system_devices *sysdevs) +{ + struct xrt_device *dev_left = NULL; + struct xrt_device *dev_right = NULL; + + // first check for serial devices + struct opengloves_communication_device *ocd_left = NULL; + struct opengloves_communication_device *ocd_right = NULL; + + // try to find serial devices + opengloves_get_serial_devices(LUCIDGLOVES_USB_VID, LUCIDGLOVES_USB_L_PID, &ocd_left); + opengloves_get_serial_devices(LUCIDGLOVES_USB_VID, LUCIDGLOVES_USB_R_PID, &ocd_right); + + + // if comm device is still null try search for bluetooth devices to fill it + if (ocd_left == NULL) + opengloves_get_bt_devices(LUCIDGLOVES_BT_L_NAME, &ocd_left); + if (ocd_right == NULL) + opengloves_get_bt_devices(LUCIDGLOVES_BT_R_NAME, &ocd_right); + + // now try to create the device if we've found a communication device + if (ocd_left != NULL) + dev_left = opengloves_device_create(ocd_left, XRT_HAND_LEFT); + if (ocd_right != NULL) + dev_right = opengloves_device_create(ocd_right, XRT_HAND_RIGHT); + + // load config + struct u_config_json config_json = {0}; + const cJSON *opengloves_config_json = opengloves_load_config_file(config_json); + + // set up tracking overrides + int cur_dev = 0; + if (dev_left != NULL && sysdevs->roles.left != NULL) { + struct xrt_quat rot = XRT_QUAT_IDENTITY; + struct xrt_vec3 pos = XRT_VEC3_ZERO; + JSON_QUAT(opengloves_config_json, "offset_rot_right", &rot); + JSON_VEC3(opengloves_config_json, "offset_pos_right", &pos); + + struct xrt_pose offset_pose = {.orientation = rot, .position = pos}; + + struct xrt_device *dev_wrap = + multi_create_tracking_override(XRT_TRACKING_OVERRIDE_DIRECT, dev_left, sysdevs->roles.left, + XRT_INPUT_GENERIC_TRACKER_POSE, &offset_pose); + + out_xdevs[cur_dev++] = dev_wrap; + } + + if (dev_right != NULL && sysdevs->roles.right != NULL) { + struct xrt_quat rot = XRT_QUAT_IDENTITY; + struct xrt_vec3 pos = XRT_VEC3_ZERO; + JSON_QUAT(opengloves_config_json, "offset_rot_right", &rot); + JSON_VEC3(opengloves_config_json, "offset_pos_right", &pos); + + struct xrt_pose offset_pose = {.orientation = rot, .position = pos}; + + struct xrt_device *dev_wrap = + multi_create_tracking_override(XRT_TRACKING_OVERRIDE_DIRECT, dev_left, sysdevs->roles.left, + XRT_INPUT_GENERIC_TRACKER_POSE, &offset_pose); + + out_xdevs[cur_dev++] = dev_wrap; + } + + return cur_dev; +}