d/wmr: Split out OG WMR controller handling

Move the original WMR controller specific handling
into a subclass of wmr_controller_base, and add
a stub placeholder for HP Reverb G2 controllers
This commit is contained in:
Jan Schmidt 2023-05-01 02:52:59 +10:00 committed by Jakob Bornecrantz
parent f33326e90b
commit ea53d274a5
12 changed files with 524 additions and 332 deletions

View file

@ -366,6 +366,10 @@ if(XRT_BUILD_DRIVER_WMR)
wmr/wmr_config.h
wmr/wmr_controller_base.c
wmr/wmr_controller_base.h
wmr/wmr_controller_og.c
wmr/wmr_controller_hp.c
wmr/wmr_controller.c
wmr/wmr_controller.h
wmr/wmr_bt_controller.c
wmr/wmr_bt_controller.h
wmr/wmr_hmd.c

View file

@ -16,6 +16,7 @@
#include "wmr_common.h"
#include "wmr_bt_controller.h"
#include "wmr_controller.h"
#include "wmr_config_key.h"
#include <stdio.h>
@ -148,6 +149,8 @@ wmr_bt_connection_destroy(struct wmr_controller_connection *base)
struct xrt_device *
wmr_bt_controller_create(struct os_hid_device *controller_hid,
enum xrt_device_type controller_type,
uint16_t vid,
uint16_t pid,
enum u_logging_level log_level)
{
DRV_TRACE_MARKER();
@ -179,7 +182,7 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
}
// Takes ownership of the connection
struct wmr_controller_base *wcb = wmr_controller_base_create(&conn->base, controller_type, log_level);
struct wmr_controller_base *wcb = wmr_controller_create(&conn->base, controller_type, vid, pid, log_level);
if (wcb == NULL) {
WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to create controller");
return NULL;

View file

@ -43,6 +43,8 @@ struct wmr_bt_connection
struct xrt_device *
wmr_bt_controller_create(struct os_hid_device *controller_hid,
enum xrt_device_type controller_type,
uint16_t vid,
uint16_t pid,
enum u_logging_level log_level);
#ifdef __cplusplus

View file

@ -0,0 +1,45 @@
// Copyright 2020-2021, N Madsen.
// Copyright 2020-2023, Collabora, Ltd.
// Copyright 2020-2023, Jan Schmidt
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Driver for WMR Controllers.
* @author Jan Schmidt <jan@centricular.com>
* @ingroup drv_wmr
*/
#include <assert.h>
#include "wmr_common.h"
#include "wmr_controller.h"
struct wmr_controller_base *
wmr_controller_og_create(struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
enum u_logging_level log_level);
struct wmr_controller_base *
wmr_controller_hp_create(struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
enum u_logging_level log_level);
struct wmr_controller_base *
wmr_controller_create(struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
uint16_t vid,
uint16_t pid,
enum u_logging_level log_level)
{
struct wmr_controller_base *ret = NULL;
assert(vid == MICROSOFT_VID); /* The only known controllers all use Microsoft VID right now */
switch (pid) {
case WMR_CONTROLLER_PID:
case ODYSSEY_CONTROLLER_PID: ret = wmr_controller_og_create(conn, controller_type, log_level); break;
case REVERB_G2_CONTROLLER_PID: ret = wmr_controller_hp_create(conn, controller_type, log_level); break;
default: break;
}
return ret;
}

View file

@ -0,0 +1,22 @@
// Copyright 2020-2021, N Madsen.
// Copyright 2020-2021, Collabora, Ltd.
// Copyright 2021-2023, Jan Schmidt
// SPDX-License-Identifier: BSL-1.0
//
/*!
* @file
* @brief Implementation of Original & HP WMR controllers
* @author Jan Schmidt <jan@centricular.com>
* @author Nis Madsen <nima_zero_one@protonmail.com>
* @ingroup drv_wmr
*/
#pragma once
#include "wmr_controller_base.h"
struct wmr_controller_base *
wmr_controller_create(struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
uint16_t vid,
uint16_t pid,
enum u_logging_level log_level);

View file

@ -35,29 +35,19 @@
#include <errno.h>
#define WMR_TRACE(wcb, ...) U_LOG_XDEV_IFL_T(&wcb->base, wcb->log_level, __VA_ARGS__)
#define WMR_TRACE_HEX(wcb, ...) U_LOG_XDEV_IFL_T_HEX(&wcb->base, wcb->log_level, __VA_ARGS__)
#define WMR_DEBUG(wcb, ...) U_LOG_XDEV_IFL_D(&wcb->base, wcb->log_level, __VA_ARGS__)
#define WMR_DEBUG_HEX(wcb, ...) U_LOG_XDEV_IFL_D_HEX(&wcb->base, wcb->log_level, __VA_ARGS__)
#define WMR_INFO(wcb, ...) U_LOG_XDEV_IFL_I(&wcb->base, wcb->log_level, __VA_ARGS__)
#define WMR_WARN(wcb, ...) U_LOG_XDEV_IFL_W(&wcb->base, wcb->log_level, __VA_ARGS__)
#define WMR_ERROR(wcb, ...) U_LOG_XDEV_IFL_E(&wcb->base, wcb->log_level, __VA_ARGS__)
/*!
* Indices in input list of each input.
*/
enum wmr_bt_input_index
{
WMR_INDEX_MENU_CLICK,
WMR_INDEX_SQUEEZE_CLICK,
WMR_INDEX_TRIGGER_VALUE,
WMR_INDEX_THUMBSTICK_CLICK,
WMR_INDEX_THUMBSTICK,
WMR_INDEX_TRACKPAD_CLICK,
WMR_INDEX_TRACKPAD_TOUCH,
WMR_INDEX_TRACKPAD,
WMR_INDEX_GRIP_POSE,
WMR_INDEX_AIM_POSE,
};
#define wmr_controller_hexdump_buffer(wcb, label, buf, length) \
do { \
WMR_DEBUG(wcb, "%s", label); \
WMR_DEBUG_HEX(wcb, buf, length); \
} while (0);
#define SET_INPUT(NAME) (wcb->base.inputs[WMR_INDEX_##NAME].name = XRT_INPUT_WMR_##NAME)
//! file path to store controller JSON configuration blocks that
//! read from the firmware.
@ -81,22 +71,16 @@ receive_bytes(struct wmr_controller_base *wcb, uint64_t time_ns, uint8_t *buffer
case WMR_MOTION_CONTROLLER_STATUS_MSG:
os_mutex_lock(&wcb->data_lock);
// Note: skipping msg type byte
bool b = wmr_controller_packet_parse(&buffer[1], (size_t)buf_size - 1, &wcb->input, wcb->log_level);
if (b) {
m_imu_3dof_update(&wcb->fusion,
wcb->input.imu.timestamp_ticks * WMR_MOTION_CONTROLLER_NS_PER_TICK,
&wcb->input.imu.acc, &wcb->input.imu.gyro);
bool b = wcb->handle_input_packet(wcb, time_ns, &buffer[1], (size_t)buf_size - 1);
os_mutex_unlock(&wcb->data_lock);
wcb->last_imu_timestamp_ns = time_ns;
wcb->last_angular_velocity = wcb->input.imu.gyro;
} else {
WMR_ERROR(wcb, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i",
buffer[0], buf_size);
os_mutex_unlock(&wcb->data_lock);
if (!b) {
WMR_ERROR(wcb, "WMR Controller: Failed handling message type: %02x, size: %i", buffer[0],
buf_size);
wmr_controller_hexdump_buffer(wcb, "Controller Message", buffer, buf_size);
return;
}
os_mutex_unlock(&wcb->data_lock);
break;
default:
WMR_DEBUG(wcb, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], buf_size);
@ -242,6 +226,7 @@ wmr_read_fw_block(struct wmr_controller_base *d, uint8_t blk_id, uint8_t **out_d
}
WMR_DEBUG(d, "Read %d-byte FW data block %d", data_size, blk_id);
wmr_controller_hexdump_buffer(d, "Data block", data, data_size);
*out_data = data;
*out_size = data_size;
@ -332,15 +317,6 @@ read_controller_config(struct wmr_controller_base *wcb)
return true;
}
static void
wmr_controller_base_set_output(struct xrt_device *xdev, enum xrt_output_name name, const union xrt_output_value *value)
{
DRV_TRACE_MARKER();
// struct wmr_controller_base *d = wmr_controller_base(xdev);
// Todo: implement
}
static void
wmr_controller_base_get_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
@ -386,38 +362,11 @@ wmr_controller_base_get_tracked_pose(struct xrt_device *xdev,
m_predict_relation(&relation, prediction_s, out_relation);
}
static void
wmr_controller_base_update_inputs(struct xrt_device *xdev)
void
wmr_controller_base_deinit(struct wmr_controller_base *wcb)
{
DRV_TRACE_MARKER();
struct wmr_controller_base *wcb = wmr_controller_base(xdev);
struct xrt_input *inputs = wcb->base.inputs;
os_mutex_lock(&wcb->data_lock);
inputs[WMR_INDEX_MENU_CLICK].value.boolean = wcb->input.menu;
inputs[WMR_INDEX_SQUEEZE_CLICK].value.boolean = wcb->input.squeeze;
inputs[WMR_INDEX_TRIGGER_VALUE].value.vec1.x = wcb->input.trigger;
inputs[WMR_INDEX_THUMBSTICK_CLICK].value.boolean = wcb->input.thumbstick.click;
inputs[WMR_INDEX_THUMBSTICK].value.vec2 = wcb->input.thumbstick.values;
inputs[WMR_INDEX_TRACKPAD_CLICK].value.boolean = wcb->input.trackpad.click;
inputs[WMR_INDEX_TRACKPAD_TOUCH].value.boolean = wcb->input.trackpad.touch;
inputs[WMR_INDEX_TRACKPAD].value.vec2 = wcb->input.trackpad.values;
os_mutex_unlock(&wcb->data_lock);
}
static void
wmr_controller_base_destroy(struct xrt_device *xdev)
{
DRV_TRACE_MARKER();
struct wmr_controller_base *wcb = wmr_controller_base(xdev);
// Remove the variable tracking.
u_var_remove_root(wcb);
@ -437,55 +386,22 @@ wmr_controller_base_destroy(struct xrt_device *xdev)
// Destroy the fusion.
m_imu_3dof_close(&wcb->fusion);
free(wcb);
}
/*
*
* Bindings
*
*/
static struct xrt_binding_input_pair simple_inputs[4] = {
{XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE},
{XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_WMR_MENU_CLICK},
{XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_WMR_GRIP_POSE},
{XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_WMR_AIM_POSE},
};
static struct xrt_binding_output_pair simple_outputs[1] = {
{XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_WMR_HAPTIC},
};
static struct xrt_binding_profile binding_profiles[1] = {
{
.name = XRT_DEVICE_SIMPLE_CONTROLLER,
.inputs = simple_inputs,
.input_count = ARRAY_SIZE(simple_inputs),
.outputs = simple_outputs,
.output_count = ARRAY_SIZE(simple_outputs),
},
};
/*
*
* 'Exported' functions.
*
*/
struct wmr_controller_base *
wmr_controller_base_create(struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
enum u_logging_level log_level)
bool
wmr_controller_base_init(struct wmr_controller_base *wcb,
struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
enum u_logging_level log_level)
{
DRV_TRACE_MARKER();
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
struct wmr_controller_base *wcb = U_DEVICE_ALLOCATE(struct wmr_controller_base, flags, 10, 1);
wcb->log_level = log_level;
wcb->wcc = conn;
wcb->receive_bytes = receive_bytes;
@ -496,30 +412,7 @@ wmr_controller_base_create(struct wmr_controller_connection *conn,
snprintf(wcb->base.str, ARRAY_SIZE(wcb->base.str), "WMR Right Controller");
}
wcb->base.destroy = wmr_controller_base_destroy;
wcb->base.get_tracked_pose = wmr_controller_base_get_tracked_pose;
wcb->base.set_output = wmr_controller_base_set_output;
wcb->base.update_inputs = wmr_controller_base_update_inputs;
SET_INPUT(MENU_CLICK);
SET_INPUT(SQUEEZE_CLICK);
SET_INPUT(TRIGGER_VALUE);
SET_INPUT(THUMBSTICK_CLICK);
SET_INPUT(THUMBSTICK);
SET_INPUT(TRACKPAD_CLICK);
SET_INPUT(TRACKPAD_TOUCH);
SET_INPUT(TRACKPAD);
SET_INPUT(GRIP_POSE);
SET_INPUT(AIM_POSE);
for (uint32_t i = 0; i < wcb->base.input_count; i++) {
wcb->base.inputs[0].active = true;
}
wcb->base.outputs[0].name = XRT_OUTPUT_NAME_WMR_HAPTIC;
wcb->base.binding_profiles = binding_profiles;
wcb->base.binding_profile_count = ARRAY_SIZE(binding_profiles);
wcb->base.name = XRT_DEVICE_WMR_CONTROLLER;
wcb->base.device_type = controller_type;
@ -527,39 +420,43 @@ wmr_controller_base_create(struct wmr_controller_connection *conn,
wcb->base.position_tracking_supported = false;
wcb->base.hand_tracking_supported = true;
wcb->input.imu.timestamp_ticks = 0;
m_imu_3dof_init(&wcb->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
if (os_mutex_init(&wcb->conn_lock) != 0 || os_mutex_init(&wcb->data_lock) != 0) {
WMR_ERROR(wcb, "WMR Controller: Failed to init mutex!");
wmr_controller_base_destroy(&wcb->base);
return NULL;
return false;
}
u_var_add_root(wcb, wcb->base.str, true);
/* Send init commands */
struct wmr_controller_fw_cmd fw_cmd = {
0,
};
struct wmr_controller_fw_cmd_response fw_cmd_response;
/* Zero command. Clears controller state? */
fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x0, 0, 0);
if (wmr_controller_send_fw_cmd(wcb, &fw_cmd, 0x06, &fw_cmd_response) < 0) {
return false;
}
/* Unknown what this one does. No obvious effect */
fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x04, 0xc1, 0x02);
if (wmr_controller_send_fw_cmd(wcb, &fw_cmd, 0x06, &fw_cmd_response) < 0) {
return false;
}
// Read config file from controller
if (!read_controller_config(wcb)) {
wmr_controller_base_destroy(&wcb->base);
return NULL;
return false;
}
u_var_add_root(wcb, wcb->base.str, true);
u_var_add_bool(wcb, &wcb->input.menu, "input.menu");
u_var_add_bool(wcb, &wcb->input.home, "input.home");
u_var_add_bool(wcb, &wcb->input.bt_pairing, "input.bt_pairing");
u_var_add_bool(wcb, &wcb->input.squeeze, "input.squeeze");
u_var_add_f32(wcb, &wcb->input.trigger, "input.trigger");
u_var_add_u8(wcb, &wcb->input.battery, "input.battery");
u_var_add_bool(wcb, &wcb->input.thumbstick.click, "input.thumbstick.click");
u_var_add_f32(wcb, &wcb->input.thumbstick.values.x, "input.thumbstick.values.y");
u_var_add_f32(wcb, &wcb->input.thumbstick.values.y, "input.thumbstick.values.x");
u_var_add_bool(wcb, &wcb->input.trackpad.click, "input.trackpad.click");
u_var_add_bool(wcb, &wcb->input.trackpad.touch, "input.trackpad.touch");
u_var_add_f32(wcb, &wcb->input.trackpad.values.x, "input.trackpad.values.x");
u_var_add_f32(wcb, &wcb->input.trackpad.values.y, "input.trackpad.values.y");
u_var_add_ro_vec3_f32(wcb, &wcb->input.imu.acc, "imu.acc");
u_var_add_ro_vec3_f32(wcb, &wcb->input.imu.gyro, "imu.gyro");
u_var_add_i32(wcb, &wcb->input.imu.temperature, "imu.temperature");
/* Enable the status reports, IMU and touchpad */
const unsigned char wmr_controller_status_enable_cmd[64] = {0x06, 0x03, 0x01, 0x00, 0x02};
wmr_controller_send_bytes(wcb, wmr_controller_status_enable_cmd, sizeof(wmr_controller_status_enable_cmd));
const unsigned char wmr_controller_imu_on_cmd[64] = {0x06, 0x03, 0x02, 0xe1, 0x02};
wmr_controller_send_bytes(wcb, wmr_controller_imu_on_cmd, sizeof(wmr_controller_imu_on_cmd));
return wcb;
return true;
}

View file

@ -101,11 +101,16 @@ struct wmr_controller_base
//! Mutex protects shared data used from OpenXR callbacks
struct os_mutex data_lock;
//! Callback to parse a controller update packet and update the input / imu info. Called with the
// data lock held.
bool (*handle_input_packet)(struct wmr_controller_base *wcb,
uint64_t time_ns,
uint8_t *buffer,
uint32_t buf_size);
/* firmware configuration block */
struct wmr_controller_config config;
//! The last decoded package of IMU and button data
struct wmr_controller_input input;
//! Time of last IMU sample, in CPU time.
uint64_t last_imu_timestamp_ns;
//! Main fusion calculator.
@ -114,11 +119,14 @@ struct wmr_controller_base
struct xrt_vec3 last_angular_velocity;
};
struct wmr_controller_base *
wmr_controller_base_create(struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
enum u_logging_level log_level);
bool
wmr_controller_base_init(struct wmr_controller_base *wcb,
struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
enum u_logging_level log_level);
void
wmr_controller_base_deinit(struct wmr_controller_base *wcb);
static inline void
wmr_controller_connection_receive_bytes(struct wmr_controller_connection *wcc,

View file

@ -0,0 +1,20 @@
// Copyright 2020-2021, N Madsen.
// Copyright 2020-2023, Collabora, Ltd.
// Copyright 2020-2023, Jan Schmidt
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Driver for WMR Controllers.
* @author Jan Schmidt <jan@centricular.com>
* @ingroup drv_wmr
*/
#include "wmr_controller.h"
struct wmr_controller_base *
wmr_controller_hp_create(struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
enum u_logging_level log_level)
{
return NULL;
}

View file

@ -0,0 +1,361 @@
// Copyright 2020-2021, N Madsen.
// Copyright 2020-2023, Collabora, Ltd.
// Copyright 2020-2023, Jan Schmidt
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Driver for WMR Controllers.
* @author Jan Schmidt <jan@centricular.com>
* @ingroup drv_wmr
*/
#include "util/u_device.h"
#include "util/u_trace_marker.h"
#include "util/u_var.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <errno.h>
#include "wmr_controller.h"
#ifdef XRT_DOXYGEN
#define WMR_PACKED
#else
#define WMR_PACKED __attribute__((packed))
#endif
/*!
* Indices in input list of each input.
*/
enum wmr_controller_og_input_index
{
WMR_CONTROLLER_INDEX_MENU_CLICK,
WMR_CONTROLLER_INDEX_SQUEEZE_CLICK,
WMR_CONTROLLER_INDEX_TRIGGER_VALUE,
WMR_CONTROLLER_INDEX_THUMBSTICK_CLICK,
WMR_CONTROLLER_INDEX_THUMBSTICK,
WMR_CONTROLLER_INDEX_TRACKPAD_CLICK,
WMR_CONTROLLER_INDEX_TRACKPAD_TOUCH,
WMR_CONTROLLER_INDEX_TRACKPAD,
WMR_CONTROLLER_INDEX_GRIP_POSE,
WMR_CONTROLLER_INDEX_AIM_POSE,
};
#define SET_INPUT(wcb, NAME) (wcb->base.inputs[WMR_CONTROLLER_INDEX_##NAME].name = XRT_INPUT_WMR_##NAME)
/*
*
* Bindings
*
*/
static struct xrt_binding_input_pair simple_inputs[4] = {
{XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE},
{XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_WMR_MENU_CLICK},
{XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_WMR_GRIP_POSE},
{XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_WMR_AIM_POSE},
};
static struct xrt_binding_output_pair simple_outputs[1] = {
{XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_WMR_HAPTIC},
};
static struct xrt_binding_profile binding_profiles[1] = {
{
.name = XRT_DEVICE_SIMPLE_CONTROLLER,
.inputs = simple_inputs,
.input_count = ARRAY_SIZE(simple_inputs),
.outputs = simple_outputs,
.output_count = ARRAY_SIZE(simple_outputs),
},
};
/* OG WMR Controller inputs struct */
struct wmr_controller_og_input
{
// buttons clicked
bool menu;
bool home;
bool bt_pairing;
bool squeeze; // Actually a "squeeze" click
float trigger;
struct
{
bool click;
struct xrt_vec2 values;
} thumbstick;
struct
{
bool click;
bool touch;
struct xrt_vec2 values;
} trackpad;
uint8_t battery;
struct
{
uint64_t timestamp_ticks;
struct xrt_vec3 acc;
struct xrt_vec3 gyro;
int32_t temperature;
} imu;
};
#undef WMR_PACKED
/* OG WMR Controller device struct */
struct wmr_controller_og
{
struct wmr_controller_base base;
//! The last decoded package of IMU and button data
struct wmr_controller_og_input last_inputs;
};
/*
*
* WMR Motion Controller protocol helpers
*
*/
static inline void
vec3_from_wmr_controller_accel(const int32_t sample[3], struct xrt_vec3 *out_vec)
{
// Reverb G1 observation: 1g is approximately 490,000.
out_vec->x = (float)sample[0] / (98000 / 2);
out_vec->y = (float)sample[1] / (98000 / 2);
out_vec->z = (float)sample[2] / (98000 / 2);
}
static inline void
vec3_from_wmr_controller_gyro(const int32_t sample[3], struct xrt_vec3 *out_vec)
{
out_vec->x = (float)sample[0] * 0.00001f;
out_vec->y = (float)sample[1] * 0.00001f;
out_vec->z = (float)sample[2] * 0.00001f;
}
static bool
wmr_controller_og_packet_parse(struct wmr_controller_og *ctrl, const unsigned char *buffer, size_t len)
{
struct wmr_controller_og_input *last_input = &ctrl->last_inputs;
struct wmr_controller_base *wcb = (struct wmr_controller_base *)(ctrl);
if (len != 44) {
U_LOG_IFL_E(wcb->log_level, "WMR Controller: unexpected message length: %zd", len);
return false;
}
const unsigned char *p = buffer;
// Read buttons
uint8_t buttons = read8(&p);
last_input->thumbstick.click = buttons & 0x01;
last_input->home = buttons & 0x02;
last_input->menu = buttons & 0x04;
last_input->squeeze = buttons & 0x08; // squeeze-click
last_input->trackpad.click = buttons & 0x10;
last_input->bt_pairing = buttons & 0x20;
last_input->trackpad.touch = buttons & 0x40;
// Read thumbstick coordinates (12 bit resolution)
int16_t stick_x = read8(&p);
uint8_t nibbles = read8(&p);
stick_x += ((nibbles & 0x0F) << 8);
int16_t stick_y = (nibbles >> 4);
stick_y += (read8(&p) << 4);
last_input->thumbstick.values.x = (float)(stick_x - 0x07FF) / 0x07FF;
if (last_input->thumbstick.values.x > 1.0f) {
last_input->thumbstick.values.x = 1.0f;
}
last_input->thumbstick.values.y = (float)(stick_y - 0x07FF) / 0x07FF;
if (last_input->thumbstick.values.y > 1.0f) {
last_input->thumbstick.values.y = 1.0f;
}
// Read trigger value (0x00 - 0xFF)
last_input->trigger = (float)read8(&p) / 0xFF;
// Read trackpad coordinates (0x00 - 0x64. Both are 0xFF when untouched)
uint8_t trackpad_x = read8(&p);
uint8_t trackpad_y = read8(&p);
last_input->trackpad.values.x = (trackpad_x == 0xFF) ? 0.0f : (float)(trackpad_x - 0x32) / 0x32;
last_input->trackpad.values.y = (trackpad_y == 0xFF) ? 0.0f : (float)(trackpad_y - 0x32) / 0x32;
last_input->battery = read8(&p);
int32_t acc[3];
acc[0] = read24(&p); // x
acc[1] = read24(&p); // y
acc[2] = read24(&p); // z
vec3_from_wmr_controller_accel(acc, &last_input->imu.acc);
U_LOG_IFL_T(wcb->log_level, "Accel [m/s^2] : %f",
sqrtf(last_input->imu.acc.x * last_input->imu.acc.x +
last_input->imu.acc.y * last_input->imu.acc.y +
last_input->imu.acc.z * last_input->imu.acc.z));
last_input->imu.temperature = read16(&p);
int32_t gyro[3];
gyro[0] = read24(&p);
gyro[1] = read24(&p);
gyro[2] = read24(&p);
vec3_from_wmr_controller_gyro(gyro, &last_input->imu.gyro);
uint32_t prev_ticks = last_input->imu.timestamp_ticks & UINT32_C(0xFFFFFFFF);
// Write the new ticks value into the lower half of timestamp_ticks
last_input->imu.timestamp_ticks &= (UINT64_C(0xFFFFFFFF) << 32u);
last_input->imu.timestamp_ticks += (uint32_t)read32(&p);
if ((last_input->imu.timestamp_ticks & UINT64_C(0xFFFFFFFF)) < prev_ticks) {
// Timer overflow, so increment the upper half of timestamp_ticks
last_input->imu.timestamp_ticks += (UINT64_C(0x1) << 32u);
}
/* Todo: More decoding here
read16(&p); // Unknown. Seems to depend on controller orientation (probably mag)
read32(&p); // Unknown.
read16(&p); // Unknown. Device state, etc.
read16(&p);
read16(&p);
*/
return true;
}
static bool
handle_input_packet(struct wmr_controller_base *wcb, uint64_t time_ns, uint8_t *buffer, uint32_t buf_size)
{
struct wmr_controller_og *ctrl = (struct wmr_controller_og *)(wcb);
bool b = wmr_controller_og_packet_parse(ctrl, buffer, buf_size);
if (b) {
m_imu_3dof_update(&wcb->fusion,
ctrl->last_inputs.imu.timestamp_ticks * WMR_MOTION_CONTROLLER_NS_PER_TICK,
&ctrl->last_inputs.imu.acc, &ctrl->last_inputs.imu.gyro);
wcb->last_imu_timestamp_ns = time_ns;
wcb->last_angular_velocity = ctrl->last_inputs.imu.gyro;
}
return b;
}
static void
wmr_controller_og_update_xrt_inputs(struct xrt_device *xdev)
{
DRV_TRACE_MARKER();
struct wmr_controller_og *ctrl = (struct wmr_controller_og *)(xdev);
struct wmr_controller_base *wcb = (struct wmr_controller_base *)(xdev);
os_mutex_lock(&wcb->data_lock);
struct xrt_input *xrt_inputs = xdev->inputs;
struct wmr_controller_og_input *cur_inputs = &ctrl->last_inputs;
xrt_inputs[WMR_CONTROLLER_INDEX_MENU_CLICK].value.boolean = cur_inputs->menu;
xrt_inputs[WMR_CONTROLLER_INDEX_SQUEEZE_CLICK].value.boolean = cur_inputs->squeeze;
xrt_inputs[WMR_CONTROLLER_INDEX_TRIGGER_VALUE].value.vec1.x = cur_inputs->trigger;
xrt_inputs[WMR_CONTROLLER_INDEX_THUMBSTICK_CLICK].value.boolean = cur_inputs->thumbstick.click;
xrt_inputs[WMR_CONTROLLER_INDEX_THUMBSTICK].value.vec2 = cur_inputs->thumbstick.values;
xrt_inputs[WMR_CONTROLLER_INDEX_TRACKPAD_CLICK].value.boolean = cur_inputs->trackpad.click;
xrt_inputs[WMR_CONTROLLER_INDEX_TRACKPAD_TOUCH].value.boolean = cur_inputs->trackpad.touch;
xrt_inputs[WMR_CONTROLLER_INDEX_TRACKPAD].value.vec2 = cur_inputs->trackpad.values;
os_mutex_unlock(&wcb->data_lock);
}
static void
wmr_controller_og_set_output(struct xrt_device *xdev, enum xrt_output_name name, const union xrt_output_value *value)
{
DRV_TRACE_MARKER();
// struct wmr_controller_base *d = wmr_controller_base(xdev);
// Todo: implement
}
static void
wmr_controller_og_destroy(struct xrt_device *xdev)
{
struct wmr_controller_base *wcb = (struct wmr_controller_base *)(xdev);
wmr_controller_base_deinit(wcb);
free(wcb);
}
struct wmr_controller_base *
wmr_controller_og_create(struct wmr_controller_connection *conn,
enum xrt_device_type controller_type,
enum u_logging_level log_level)
{
DRV_TRACE_MARKER();
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
struct wmr_controller_og *ctrl = U_DEVICE_ALLOCATE(struct wmr_controller_og, flags, 10, 1);
struct wmr_controller_base *wcb = (struct wmr_controller_base *)(ctrl);
if (!wmr_controller_base_init(wcb, conn, controller_type, log_level)) {
wmr_controller_og_destroy(&wcb->base);
return NULL;
}
wcb->handle_input_packet = handle_input_packet;
wcb->base.destroy = wmr_controller_og_destroy;
wcb->base.update_inputs = wmr_controller_og_update_xrt_inputs;
wcb->base.set_output = wmr_controller_og_set_output;
SET_INPUT(wcb, MENU_CLICK);
SET_INPUT(wcb, SQUEEZE_CLICK);
SET_INPUT(wcb, TRIGGER_VALUE);
SET_INPUT(wcb, THUMBSTICK_CLICK);
SET_INPUT(wcb, THUMBSTICK);
SET_INPUT(wcb, TRACKPAD_CLICK);
SET_INPUT(wcb, TRACKPAD_TOUCH);
SET_INPUT(wcb, TRACKPAD);
SET_INPUT(wcb, GRIP_POSE);
SET_INPUT(wcb, AIM_POSE);
for (uint32_t i = 0; i < wcb->base.input_count; i++) {
wcb->base.inputs[0].active = true;
}
ctrl->last_inputs.imu.timestamp_ticks = 0;
wcb->base.outputs[0].name = XRT_OUTPUT_NAME_WMR_HAPTIC;
wcb->base.binding_profiles = binding_profiles;
wcb->base.binding_profile_count = ARRAY_SIZE(binding_profiles);
u_var_add_bool(wcb, &ctrl->last_inputs.menu, "input.menu");
u_var_add_bool(wcb, &ctrl->last_inputs.home, "input.home");
u_var_add_bool(wcb, &ctrl->last_inputs.bt_pairing, "input.bt_pairing");
u_var_add_bool(wcb, &ctrl->last_inputs.squeeze, "input.squeeze");
u_var_add_f32(wcb, &ctrl->last_inputs.trigger, "input.trigger");
u_var_add_u8(wcb, &ctrl->last_inputs.battery, "input.battery");
u_var_add_bool(wcb, &ctrl->last_inputs.thumbstick.click, "input.thumbstick.click");
u_var_add_f32(wcb, &ctrl->last_inputs.thumbstick.values.x, "input.thumbstick.values.y");
u_var_add_f32(wcb, &ctrl->last_inputs.thumbstick.values.y, "input.thumbstick.values.x");
u_var_add_bool(wcb, &ctrl->last_inputs.trackpad.click, "input.trackpad.click");
u_var_add_bool(wcb, &ctrl->last_inputs.trackpad.touch, "input.trackpad.touch");
u_var_add_f32(wcb, &ctrl->last_inputs.trackpad.values.x, "input.trackpad.values.x");
u_var_add_f32(wcb, &ctrl->last_inputs.trackpad.values.y, "input.trackpad.values.y");
u_var_add_ro_vec3_f32(wcb, &ctrl->last_inputs.imu.acc, "imu.acc");
u_var_add_ro_vec3_f32(wcb, &ctrl->last_inputs.imu.gyro, "imu.gyro");
u_var_add_i32(wcb, &ctrl->last_inputs.imu.temperature, "imu.temperature");
return wcb;
}

View file

@ -19,121 +19,3 @@
* WMR Motion Controller protocol helpers
*
*/
static inline void
vec3_from_wmr_controller_accel(const int32_t sample[3], struct xrt_vec3 *out_vec)
{
// Reverb G1 observation: 1g is approximately 490,000.
out_vec->x = (float)sample[0] / (98000 / 2);
out_vec->y = (float)sample[1] / (98000 / 2);
out_vec->z = (float)sample[2] / (98000 / 2);
}
static inline void
vec3_from_wmr_controller_gyro(const int32_t sample[3], struct xrt_vec3 *out_vec)
{
out_vec->x = (float)sample[0] * 0.00001f;
out_vec->y = (float)sample[1] * 0.00001f;
out_vec->z = (float)sample[2] * 0.00001f;
}
bool
wmr_controller_packet_parse(const unsigned char *buffer,
size_t len,
struct wmr_controller_input *decoded_input,
enum u_logging_level log_level)
{
if (len != 44) {
U_LOG_IFL_E(log_level, "WMR Controller: unexpected message length: %zd", len);
return false;
}
const unsigned char *p = buffer;
// Read buttons
uint8_t buttons = read8(&p);
decoded_input->thumbstick.click = buttons & 0x01;
decoded_input->home = buttons & 0x02;
decoded_input->menu = buttons & 0x04;
decoded_input->squeeze = buttons & 0x08; // squeeze-click
decoded_input->trackpad.click = buttons & 0x10;
decoded_input->bt_pairing = buttons & 0x20;
decoded_input->trackpad.touch = buttons & 0x40;
// Read thumbstick coordinates (12 bit resolution)
int16_t stick_x = read8(&p);
uint8_t nibbles = read8(&p);
stick_x += ((nibbles & 0x0F) << 8);
int16_t stick_y = (nibbles >> 4);
stick_y += (read8(&p) << 4);
decoded_input->thumbstick.values.x = (float)(stick_x - 0x07FF) / 0x07FF;
if (decoded_input->thumbstick.values.x > 1.0f) {
decoded_input->thumbstick.values.x = 1.0f;
}
decoded_input->thumbstick.values.y = (float)(stick_y - 0x07FF) / 0x07FF;
if (decoded_input->thumbstick.values.y > 1.0f) {
decoded_input->thumbstick.values.y = 1.0f;
}
// Read trigger value (0x00 - 0xFF)
decoded_input->trigger = (float)read8(&p) / 0xFF;
// Read trackpad coordinates (0x00 - 0x64. Both are 0xFF when untouched)
uint8_t trackpad_x = read8(&p);
uint8_t trackpad_y = read8(&p);
decoded_input->trackpad.values.x = (trackpad_x == 0xFF) ? 0.0f : (float)(trackpad_x - 0x32) / 0x32;
decoded_input->trackpad.values.y = (trackpad_y == 0xFF) ? 0.0f : (float)(trackpad_y - 0x32) / 0x32;
decoded_input->battery = read8(&p);
int32_t acc[3];
acc[0] = read24(&p); // x
acc[1] = read24(&p); // y
acc[2] = read24(&p); // z
vec3_from_wmr_controller_accel(acc, &decoded_input->imu.acc);
U_LOG_IFL_T(log_level, "Accel [m/s^2] : %f",
sqrtf(decoded_input->imu.acc.x * decoded_input->imu.acc.x +
decoded_input->imu.acc.y * decoded_input->imu.acc.y +
decoded_input->imu.acc.z * decoded_input->imu.acc.z));
decoded_input->imu.temperature = read16(&p);
int32_t gyro[3];
gyro[0] = read24(&p);
gyro[1] = read24(&p);
gyro[2] = read24(&p);
vec3_from_wmr_controller_gyro(gyro, &decoded_input->imu.gyro);
uint32_t prev_ticks = decoded_input->imu.timestamp_ticks & UINT32_C(0xFFFFFFFF);
// Write the new ticks value into the lower half of timestamp_ticks
decoded_input->imu.timestamp_ticks &= (UINT64_C(0xFFFFFFFF) << 32u);
decoded_input->imu.timestamp_ticks += (uint32_t)read32(&p);
if ((decoded_input->imu.timestamp_ticks & UINT64_C(0xFFFFFFFF)) < prev_ticks) {
// Timer overflow, so increment the upper half of timestamp_ticks
decoded_input->imu.timestamp_ticks += (UINT64_C(0x1) << 32u);
}
/* Todo: More decoding here
read16(&p); // Unknown. Seems to depend on controller orientation.
read32(&p); // Unknown.
read16(&p); // Unknown. Device state, etc.
read16(&p);
read16(&p);
*/
return true;
}

View file

@ -40,40 +40,6 @@ extern "C" {
// Messages types for WMR motion controllers
#define WMR_MOTION_CONTROLLER_STATUS_MSG 0x01
struct wmr_controller_input
{
// buttons clicked
bool menu;
bool home;
bool bt_pairing;
bool squeeze; // Actually a "squeeze" click
float trigger;
struct
{
bool click;
struct xrt_vec2 values;
} thumbstick;
struct
{
bool click;
bool touch;
struct xrt_vec2 values;
} trackpad;
uint8_t battery;
struct
{
uint64_t timestamp_ticks;
struct xrt_vec3 acc;
struct xrt_vec3 gyro;
int32_t temperature;
} imu;
};
struct wmr_controller_fw_cmd
{
union {
@ -119,25 +85,6 @@ struct wmr_controller_fw_cmd_response
#undef WMR_PACKED
/*!
* WMR Motion Controller protocol helpers
*
* @addtogroup drv_wmr
* @{
*/
bool
wmr_controller_packet_parse(const unsigned char *buffer,
size_t len,
struct wmr_controller_input *decoded_input,
enum u_logging_level log_level);
/*!
* @}
*/
#ifdef __cplusplus
}
#endif

View file

@ -417,7 +417,8 @@ wmr_create_bt_controller(struct xrt_prober *xp,
}
// Takes ownership of the hid_controller, even on failure
struct xrt_device *xdev = wmr_bt_controller_create(hid_controller, controller_type, log_level);
struct xrt_device *xdev =
wmr_bt_controller_create(hid_controller, controller_type, xpdev->vendor_id, xpdev->product_id, log_level);
if (xdev == NULL) {
U_LOG_IFL_E(log_level, "Failed to create WMR controller (Bluetooth)");
return XRT_ERROR_DEVICE_CREATION_FAILED;