mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-29 11:06:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/wmr: Add basic Reverb (G1, Bluetooth) motion controller support.

Browse source
This commit is contained in:
Nima01 2021-11-13 03:35:13 +01:00 committed by Jakob Bornecrantz
parent acd8a0986e
commit c4db3dfccc
12 changed files with 473 additions and 21 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/xrt/drivers/CMakeLists.txt
View file

@ -300,6 +300,8 @@ if(XRT_BUILD_DRIVER_WMR)
wmr/wmr_common.h
wmr/wmr_config.c
wmr/wmr_config.h
wmr/wmr_bt_controller.c
wmr/wmr_bt_controller.h
wmr/wmr_hmd.c
wmr/wmr_hmd.h
wmr/wmr_interface.h

2
src/xrt/drivers/meson.build
View file

@ -295,6 +295,8 @@ lib_drv_wmr = static_library(
files(
'wmr/wmr_common.h',
'wmr/wmr_config.c',
'wmr/wmr_bt_controller.c',
'wmr/wmr_bt_controller.h',
'wmr/wmr_hmd.c',
'wmr/wmr_hmd.h',
'wmr/wmr_interface.h',

271
src/xrt/drivers/wmr/wmr_bt_controller.c Normal file
View file

@ -0,0 +1,271 @@
// Copyright 2020-2021, N Madsen.
// Copyright 2020-2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Driver for Bluetooth based WMR Controller.
* @author Nis Madsen <nima_zero_one@protonmail.com>
* @ingroup drv_wmr
*/
#include "xrt/xrt_config_os.h"
#include "xrt/xrt_device.h"
#include "os/os_time.h"
#include "os/os_hid.h"
#include "math/m_mathinclude.h"
#include "math/m_api.h"
#include "math/m_vec2.h"
#include "math/m_predict.h"
#include "util/u_var.h"
#include "util/u_misc.h"
#include "util/u_time.h"
#include "util/u_debug.h"
#include "util/u_device.h"
#include "wmr_bt_controller.h"
#include "wmr_common.h"
#include "wmr_protocol.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#ifndef XRT_OS_WINDOWS
#include <unistd.h> // for sleep()
#endif
#define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->ll, __VA_ARGS__)
#define WMR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->ll, __VA_ARGS__)
#define WMR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->ll, __VA_ARGS__)
#define WMR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->ll, __VA_ARGS__)
#define WMR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->ll, __VA_ARGS__)
static inline struct wmr_bt_controller *
wmr_bt_controller(struct xrt_device *p)
{
return (struct wmr_bt_controller *)p;
}
static bool
control_read_packets(struct wmr_bt_controller *d)
{
unsigned char buffer[WMR_FEATURE_BUFFER_SIZE];
// Do not block
int size = os_hid_read(d->controller_hid, buffer, sizeof(buffer), 0);
if (size < 0) {
WMR_ERROR(d, "Error reading from controller device");
return false;
} else if (size == 0) {
WMR_TRACE(d, "No more data to read from controller device");
return true; // No more messages, return.
} else {
WMR_DEBUG(d, "Read %u bytes from controller device", size);
}
switch (buffer[0]) {
case WMR_MS_HOLOLENS_MSG_SENSORS: //
if (size != 45) {
WMR_ERROR(d, "WMR Controller unexpected message size: %d", size);
return false;
}
WMR_DEBUG(d,
"%02x | " // msg type
"%02x %02x %02x %02x %02x %02x %02x %02x | " // buttons and inputs, battery
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // accel
"%02x %02x | " // temp
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // gyro
"%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | " // timestamp and more?
"%02x %02x %02x %02x %02x %02x", // device run state, status and more?
buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7],
buffer[8], buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15],
buffer[16], buffer[17], buffer[18], buffer[19], buffer[20], buffer[21], buffer[22],
buffer[23], buffer[24], buffer[25], buffer[26], buffer[27], buffer[28], buffer[29],
buffer[30], buffer[31], buffer[32], buffer[33], buffer[34], buffer[35], buffer[36],
buffer[37], buffer[38], buffer[39], buffer[40], buffer[41], buffer[42], buffer[43],
buffer[44]);
const unsigned char *p = (unsigned char *)&buffer[1];
// HP Reverb G1 button mask:
// Stick_pressed: 0x01
// Windows button: 0x02
// Menu button: 0x04
// Side button: 0x08
// Touch-pad pressed: 0x10
// BT pairing button: 0x20
// Touch-pad touched: 0x40
uint8_t buttons = read8(&p);
// Todo: interpret analog stick data
uint8_t stick_1 = read8(&p);
uint8_t stick_2 = read8(&p);
uint8_t stick_3 = read8(&p);
uint8_t trigger = read8(&p); // pressure: 0x00 - 0xFF
// Touchpad coords range: 0x00 - 0x64. Both are 0xFF when untouched.
uint8_t pad_x = read8(&p);
uint8_t pad_y = read8(&p);
uint8_t battery = read8(&p);
int32_t accel_x = read24(&p);
int32_t accel_y = read24(&p);
int32_t accel_z = read24(&p);
int32_t temp = read16(&p);
int32_t gyro_x = read24(&p);
int32_t gyro_y = read24(&p);
int32_t gyro_z = read24(&p);
uint64_t timestamp = read32(&p); // Maybe only part of timestamp.
read16(&p); // Unknown. Seems to depend on controller orientation.
read32(&p); // Unknown.
read16(&p); // Unknown. Device state, etc.
read16(&p);
read16(&p);
WMR_DEBUG(d, "timestamp %lu\ttemp %d\taccel x: %f\ty: %f\tz: %f\t\tgyro x: %f\tgyro y: %f\tgyro z: %f",
timestamp, temp, accel_x * 0.001f, accel_y * 0.001f, accel_z * 0.001f, gyro_x * 2e-6,
gyro_y * 2e-6, gyro_z * 2e-6);
break;
default: //
WMR_DEBUG(d, "Unknown message type: %02x, size: %i from controller device", buffer[0], size);
break;
}
return true;
}
static void
wmr_bt_controller_set_output(struct xrt_device *xdev, enum xrt_output_name name, union xrt_output_value *value)
{
// struct wmr_bt_controller *d = wmr_bt_controller(xdev);
// Todo: implement
}
static void
wmr_bt_controller_get_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
uint64_t at_timestamp_ns,
struct xrt_space_relation *out_relation)
{
// struct wmr_bt_controller *d = wmr_bt_controller(xdev);
// Todo: implement
}
static void
wmr_bt_controller_update_inputs(struct xrt_device *xdev)
{
// struct wmr_bt_controller *d = wmr_bt_controller(xdev);
// Todo: implement
}
static void *
wmr_bt_controller_run_thread(void *ptr)
{
struct wmr_bt_controller *d = wmr_bt_controller(ptr);
os_thread_helper_lock(&d->controller_thread);
while (os_thread_helper_is_running_locked(&d->controller_thread)) {
os_thread_helper_unlock(&d->controller_thread);
// Does not block.
if (!control_read_packets(d)) {
break;
}
}
WMR_DEBUG(d, "Exiting reading thread.");
return NULL;
}
static void
wmr_bt_controller_destroy(struct xrt_device *xdev)
{
struct wmr_bt_controller *d = wmr_bt_controller(xdev);
// Destroy the thread object.
os_thread_helper_destroy(&d->controller_thread);
if (d->controller_hid != NULL) {
/* Do any deinit if we have a deinit function */
// if (d->hmd_desc && d->hmd_desc->deinit_func) {
// d->hmd_desc->deinit_func(d);
// }
os_hid_destroy(d->controller_hid);
d->controller_hid = NULL;
}
// Destroy the fusion.
m_imu_3dof_close(&d->fusion);
free(d);
}
struct xrt_device *
wmr_bt_controller_create(struct os_hid_device *controller_hid,
enum xrt_device_type controller_type,
enum u_logging_level ll)
{
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
struct wmr_bt_controller *d = U_DEVICE_ALLOCATE(struct wmr_bt_controller, flags, 1, 01);
d->ll = ll;
d->controller_hid = controller_hid;
d->base.destroy = wmr_bt_controller_destroy;
d->base.get_tracked_pose = wmr_bt_controller_get_tracked_pose;
d->base.set_output = wmr_bt_controller_set_output;
d->base.update_inputs = wmr_bt_controller_update_inputs;
d->base.inputs[0].name = XRT_INPUT_GENERIC_HAND_TRACKING_LEFT;
d->base.inputs[1].name = XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT;
d->base.name = XRT_DEVICE_WMR_CONTROLLER;
d->base.device_type = controller_type;
d->base.orientation_tracking_supported = true;
d->base.position_tracking_supported = false;
d->base.hand_tracking_supported = true;
m_imu_3dof_init(&d->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
int ret = 0;
// Todo: Read config file from controller
// Thread and other state.
ret = os_thread_helper_init(&d->controller_thread);
if (ret != 0) {
WMR_ERROR(d, "Failed to init WMR controller threading!");
wmr_bt_controller_destroy(&d->base);
d = NULL;
return NULL;
}
// Hand over controller device to reading thread.
ret = os_thread_helper_start(&d->controller_thread, wmr_bt_controller_run_thread, d);
if (ret != 0) {
WMR_ERROR(d, "Failed to start WMR controller thread!");
wmr_bt_controller_destroy(&d->base);
d = NULL;
return NULL;
}
return &d->base;
}

81
src/xrt/drivers/wmr/wmr_bt_controller.h Normal file
View file

@ -0,0 +1,81 @@
// Copyright 2020-2021, N Madsen.
// Copyright 2020-2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Driver interface for Bluetooth based WMR motion controllers.
* Note: Only tested with HP Reverb (G1) controllers that are manually
* paired to a non hmd-integrated, generic BT usb adapter.
* @author Nis Madsen <nima_zero_one@protonmail.com>
* @ingroup drv_wmr
*/
#include "os/os_threading.h"
#include "xrt/xrt_prober.h"
#include "math/m_imu_3dof.h"
#include "util/u_logging.h"
#include "xrt/xrt_device.h"
#include "xrt/xrt_prober.h"
#include "wmr_protocol.h"
#include "wmr_config.h"
#ifdef __cplusplus
extern "C" {
#endif
/*!
* A Bluetooth connected WMR Controller device, representing just a single controller.
*
* @ingroup drv_wmr
* @implements xrt_device
*/
struct wmr_bt_controller
{
struct xrt_device base;
struct os_hid_device *controller_hid;
struct os_thread_helper controller_thread;
struct os_mutex lock;
struct
{
uint64_t time_ns;
uint32_t last_sample_time_raw;
timepoint_ns ts_received_ns;
} imu;
struct m_imu_3dof fusion;
struct
{
struct xrt_vec3 acc;
struct xrt_vec3 gyro;
} last;
struct xrt_quat rot_filtered;
enum u_logging_level ll;
uint32_t last_ticks;
// firmware configuration block, with device names etc
// struct wmr_config_header config_hdr;
// Config data parsed from the firmware JSON
// wmr_bt_controller_config config;
};
struct xrt_device *
wmr_bt_controller_create(struct os_hid_device *controller_hid,
enum xrt_device_type controller_type,
enum u_logging_level ll);
#ifdef __cplusplus
}
#endif

6
src/xrt/drivers/wmr/wmr_common.h
View file

@ -4,7 +4,7 @@
/*!
* @file
* @brief Defines and constants related to WMR driver code.
* @author nima01 <nima_zero_one@protonmail.com>
* @author Nis Madsen <nima_zero_one@protonmail.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_wmr
*/
@ -29,11 +29,15 @@ extern "C" {
#define MICROSOFT_VID 0x045e
#define HOLOLENS_SENSORS_PID 0x0659
#define WMR_CONTROLLER_PID 0x065b
#define WMR_CONTROLLER_LEFT_PRODUCT_STRING "Motion controller - Left"
#define WMR_CONTROLLER_RIGHT_PRODUCT_STRING "Motion controller - Right"
#define HP_VID 0x03f0
#define REVERB_G1_PID 0x0c6a
#define REVERB_G2_PID 0x0580
#define LENOVO_VID 0x17ef
#define EXPLORER_PID 0xb801

13
src/xrt/drivers/wmr/wmr_config.h
View file

@ -66,6 +66,19 @@ struct wmr_hmd_config
bool
wmr_config_parse(struct wmr_hmd_config *c, char *json_string, enum u_logging_level ll);
struct wmr_bt_controller_config
{
/* Todo: still work in progress */
struct xrt_pose accel_pose;
struct xrt_pose gyro_pose;
struct xrt_pose mag_pose;
};
/* Todo: Extract and parse motion controller config. */
#ifdef __cplusplus
}
#endif

2
src/xrt/drivers/wmr/wmr_hmd.c
View file

@ -198,6 +198,8 @@ hololens_sensors_read_packets(struct wmr_hmd *wh)
hololens_unknown_17_decode_packet(wh, buffer, size);
break;
case WMR_MS_HOLOLENS_MSG_CONTROL:
WMR_DEBUG(wh, "WMR_MS_HOLOLENS_MSG_CONTROL: %02x, (%i)", buffer[0], size);
break;
case WMR_MS_HOLOLENS_MSG_DEBUG: //
break;
default: //

15
src/xrt/drivers/wmr/wmr_hmd.h
View file

@ -27,21 +27,6 @@
extern "C" {
#endif
enum rvb_g1_status_bits
{
// clang-format off
REVERB_G1_STATUS_BIT_UNKNOWN_BIT_0 = (1 << 0),
REVERB_G1_STATUS_BIT_UNKNOWN_BIT_1 = (1 << 1),
REVERB_G1_STATUS_BIT_UNKNOWN_BIT_2 = (1 << 2),
REVERB_G1_STATUS_BIT_UNKNOWN_BIT_3 = (1 << 3),
REVERB_G1_STATUS_BIT_UNKNOWN_BIT_4 = (1 << 4),
REVERB_G1_STATUS_BIT_UNKNOWN_BIT_5 = (1 << 5),
REVERB_G1_STATUS_BIT_UNKNOWN_BIT_6 = (1 << 6),
REVERB_G1_STATUS_BIT_UNKNOWN_BIT_7 = (1 << 7),
// clang-format on
};
enum wmr_headset_type
{
WMR_HEADSET_GENERIC,

15
src/xrt/drivers/wmr/wmr_interface.h
View file

@ -37,6 +37,21 @@ wmr_found(struct xrt_prober *xp,
cJSON *attached_data,
struct xrt_device **out_xdev);
/*!
* Probing function for Bluetooth WMR motion controllers.
*
* @ingroup drv_wmr
*/
int
wmr_bt_controller_found(struct xrt_prober *xp,
struct xrt_prober_device **devices,
size_t device_count,
size_t index,
cJSON *attached_data,
struct xrt_device **out_xdev);
/*!
* @dir drivers/wmr
*

75
src/xrt/drivers/wmr/wmr_prober.c
View file

@ -4,7 +4,7 @@
/*!
* @file
* @brief WMR prober code.
* @author nima01 <nima_zero_one@protonmail.com>
* @author Nis Madsen <nima_zero_one@protonmail.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_wmr
*/
@ -17,6 +17,7 @@
#include "wmr_interface.h"
#include "wmr_hmd.h"
#include "wmr_bt_controller.h"
#include "wmr_common.h"
#include <stdio.h>
@ -171,12 +172,12 @@ wmr_found(struct xrt_prober *xp,
return -1;
}
U_LOG_IFL_D(ll, "Found HoloLens Sensors HMD device '%s' '%s' (vid %04X, pid %04X)", MS_HOLOLENS_MANUFACTURER_STRING,
MS_HOLOLENS_PRODUCT_STRING, dev_holo->vendor_id, dev_holo->product_id);
U_LOG_IFL_D(ll, "Found HoloLens Sensors HMD device '%s' '%s' (vid %04X, pid %04X)",
MS_HOLOLENS_MANUFACTURER_STRING, MS_HOLOLENS_PRODUCT_STRING, dev_holo->vendor_id,
dev_holo->product_id);
if (!find_companion_device(xp, devices, device_count, ll, &hmd_type, &dev_companion, &interface_companion)) {
U_LOG_IFL_E(ll,
"Did not find HoloLens Sensors' companion device");
U_LOG_IFL_E(ll, "Did not find HoloLens Sensors' companion device");
return -1;
}
@ -203,3 +204,67 @@ wmr_found(struct xrt_prober *xp,
*out_xdev = p;
return 1;
}
int
wmr_bt_controller_found(struct xrt_prober *xp,
struct xrt_prober_device **devices,
size_t num_devices,
size_t index,
cJSON *attached_data,
struct xrt_device **out_xdev)
{
enum u_logging_level ll = debug_get_log_option_wmr_log();
struct os_hid_device *hid_controller = NULL;
// Only handle Bluetooth connected controllers here.
if (devices[index]->bus != XRT_BUS_TYPE_BLUETOOTH) {
return 0;
}
unsigned char product_name[XRT_DEVICE_PRODUCT_NAME_LEN] = {0};
int ret = xrt_prober_get_string_descriptor(xp, devices[index], XRT_PROBER_STRING_PRODUCT, product_name,
sizeof(product_name));
enum xrt_device_type controller_type = XRT_DEVICE_TYPE_UNKNOWN;
int interface_controller = -1;
switch (devices[index]->product_id) {
case WMR_CONTROLLER_PID:
if (strncmp((char *)product_name, WMR_CONTROLLER_LEFT_PRODUCT_STRING, sizeof(product_name)) == 0) {
controller_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER;
interface_controller = 0;
break;
} else if (strncmp((char *)product_name, WMR_CONTROLLER_RIGHT_PRODUCT_STRING, sizeof(product_name)) ==
0) {
controller_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER;
interface_controller = 0;
break;
}
// else fall through
default:
U_LOG_IFL_D(ll,
"Unsupported controller device (Bluetooth): vid: 0x%04X, pid: 0x%04X, Product Name: '%s'",
devices[index]->vendor_id, devices[index]->product_id, product_name);
return -1;
}
ret = xrt_prober_open_hid_interface(xp, devices[index], interface_controller, &hid_controller);
if (ret != 0) {
U_LOG_IFL_E(ll, "Failed to open WMR Bluetooth controller's HID interface");
return -1;
}
struct xrt_device *p = wmr_bt_controller_create(hid_controller, controller_type, ll);
if (!p) {
U_LOG_IFL_E(ll, "Failed to create WMR controller (Bluetooth)");
return -1;
}
*out_xdev = p;
return 1;
}

11
src/xrt/drivers/wmr/wmr_protocol.h
View file

@ -106,6 +106,17 @@ read16(const unsigned char **buffer)
return ret;
}
static inline int32_t
read24(const unsigned char **buffer)
{
// Note: Preserve sign by shifting up to write MSB
int32_t ret = (*(*buffer + 0) << 8) | (*(*buffer + 1) << 16) | (*(*buffer + 2) << 24);
*buffer += 3;
// restore 24 bit scale again
return ret >> 8;
}
static inline int32_t
read32(const unsigned char **buffer)
{

1
src/xrt/targets/common/target_lists.c
View file

@ -129,6 +129,7 @@ struct xrt_prober_entry target_entry_list[] = {
#ifdef XRT_BUILD_DRIVER_WMR
{MICROSOFT_VID, HOLOLENS_SENSORS_PID, wmr_found, "Microsoft HoloLens Sensors", "wmr"},
{MICROSOFT_VID, WMR_CONTROLLER_PID, wmr_bt_controller_found, "WMR Bluetooth controller", "wmr"},
#endif // XRT_BUILD_DRIVER_WMR
{0x0000, 0x0000, NULL, NULL, NULL}, // Terminate