mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2024-12-28 18:46:18 +00:00
drivers: add all of opengloves driver
This commit is contained in:
danwillm
parent
0943ce10be
commit
fd61dd08b2
|
|
@ -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}")
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -0,0 +1,91 @@
|
|||
// Copyright 2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief OpenGloves bluetooth serial implementation
|
||||
* @author Daniel Willmott <web@dan-w.com>
|
||||
* @ingroup drv_opengloves
|
||||
*/
|
||||
|
||||
#include <unistd.h>
|
||||
#include <errno.h>
|
||||
#include <bluetooth/bluetooth.h>
|
||||
#include <bluetooth/rfcomm.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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 <web@dan-w.com>
|
||||
* @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
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
// Copyright 2019-2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief OpenGloves bluetooth prober implementation.
|
||||
* @author Daniel Willmott <web@dan-w.com>
|
||||
* @ingroup drv_opengloves
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <bluetooth/bluetooth.h>
|
||||
#include <bluetooth/hci.h>
|
||||
#include <bluetooth/hci_lib.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
// Copyright 2019-2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief OpenGloves bluetooth prober.
|
||||
* @author Daniel Willmott <web@dan-w.com>
|
||||
* @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
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
// Copyright 2019-2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief Communication structures for OpenGloves
|
||||
* @author Daniel Willmott <web@dan-w.com>
|
||||
* @ingroup drv_opengloves
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#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
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
// Copyright 2019-2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief OpenGloves serial prober implementation.
|
||||
* @author Daniel Willmott <web@dan-w.com>
|
||||
* @ingroup drv_opengloves
|
||||
*/
|
||||
|
||||
#include <libudev.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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 <web@dan-w.com>
|
||||
* @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
|
||||
|
|
@ -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 <web@dan-w.com>
|
||||
* @ingroup drv_opengloves
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <termios.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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 <web@dan-w.com>
|
||||
* @ingroup drv_opengloves
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#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
|
||||
250
src/xrt/drivers/opengloves/encoding/alpha_encoding.cpp
Normal file
250
src/xrt/drivers/opengloves/encoding/alpha_encoding.cpp
Normal file
|
|
@ -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 <web@dan-w.com>
|
||||
* @ingroup drv_opengloves
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <map>
|
||||
#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<std::string, int> 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<int, std::string>
|
||||
opengloves_alpha_encoding_parse_to_map(const std::string &str)
|
||||
{
|
||||
std::map<int, std::string> 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<int, std::string> 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();
|
||||
}
|
||||
22
src/xrt/drivers/opengloves/encoding/alpha_encoding.h
Normal file
22
src/xrt/drivers/opengloves/encoding/alpha_encoding.h
Normal file
|
|
@ -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 <web@dan-w.com>
|
||||
* @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
|
||||
71
src/xrt/drivers/opengloves/encoding/encoding.h
Normal file
71
src/xrt/drivers/opengloves/encoding/encoding.h
Normal file
|
|
@ -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 <web@dan-w.com>
|
||||
* @ingroup drv_opengloves
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <stdbool.h>
|
||||
|
||||
#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
|
||||
290
src/xrt/drivers/opengloves/opengloves_device.c
Normal file
290
src/xrt/drivers/opengloves/opengloves_device.c
Normal file
|
|
@ -0,0 +1,290 @@
|
|||
// Copyright 2019-2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief OpenGloves device implementation.
|
||||
* @author Daniel Willmott <web@dan-w.com>
|
||||
* @ingroup drv_opengloves
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
24
src/xrt/drivers/opengloves/opengloves_device.h
Normal file
24
src/xrt/drivers/opengloves/opengloves_device.h
Normal file
|
|
@ -0,0 +1,24 @@
|
|||
// Copyright 2019-2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief OpenGloves device.
|
||||
* @author Daniel Willmott <web@dan-w.com>
|
||||
* @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
|
||||
36
src/xrt/drivers/opengloves/opengloves_interface.h
Normal file
36
src/xrt/drivers/opengloves/opengloves_interface.h
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
// Copyright 2019-2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief OpenGloves device interface.
|
||||
* @author Daniel Willmott <web@dan-w.com>
|
||||
* @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
|
||||
116
src/xrt/drivers/opengloves/opengloves_prober.c
Normal file
116
src/xrt/drivers/opengloves/opengloves_prober.c
Normal file
|
|
@ -0,0 +1,116 @@
|
|||
// Copyright 2019-2022, Collabora, Ltd.
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
* @brief OpenGloves prober implementation.
|
||||
* @author Daniel Willmott <web@dan-w.com>
|
||||
* @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;
|
||||
}
|
||||
Loading…
Reference in a new issue