mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-15 10:10:07 +00:00
d/wmr: Split the connection from controller
Factor out the bluetooth connection behaviour from the WMR controller, so the connection can be provided from the headset as well.
This commit is contained in:
parent
35f72696fb
commit
d326ff9890
|
@ -364,6 +364,8 @@ if(XRT_BUILD_DRIVER_WMR)
|
|||
wmr/wmr_common.h
|
||||
wmr/wmr_config.c
|
||||
wmr/wmr_config.h
|
||||
wmr/wmr_controller_base.c
|
||||
wmr/wmr_controller_base.h
|
||||
wmr/wmr_bt_controller.c
|
||||
wmr/wmr_bt_controller.h
|
||||
wmr/wmr_hmd.c
|
||||
|
|
|
@ -1,5 +1,6 @@
|
|||
// Copyright 2020-2021, N Madsen.
|
||||
// Copyright 2020-2021, Collabora, Ltd.
|
||||
// Copyright 2020-2023, Jan Schmidt
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
/*!
|
||||
* @file
|
||||
|
@ -11,16 +12,6 @@
|
|||
#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 "util/u_trace_marker.h"
|
||||
|
||||
#include "wmr_common.h"
|
||||
|
@ -32,433 +23,128 @@
|
|||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <errno.h>
|
||||
#ifndef XRT_OS_WINDOWS
|
||||
#include <unistd.h> // for sleep()
|
||||
#endif
|
||||
|
||||
#define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__)
|
||||
#define WMR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__)
|
||||
#define WMR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__)
|
||||
#define WMR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__)
|
||||
#define WMR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__)
|
||||
#define WMR_TRACE(c, ...) U_LOG_IFL_T(c->log_level, __VA_ARGS__)
|
||||
#define WMR_DEBUG(c, ...) U_LOG_IFL_D(c->log_level, __VA_ARGS__)
|
||||
#define WMR_INFO(c, ...) U_LOG_IFL_I(c->log_level, __VA_ARGS__)
|
||||
#define WMR_WARN(c, ...) U_LOG_IFL_W(c->log_level, __VA_ARGS__)
|
||||
#define WMR_ERROR(c, ...) U_LOG_IFL_E(c->log_level, __VA_ARGS__)
|
||||
|
||||
#define SET_INPUT(NAME) (d->base.inputs[WMR_INDEX_##NAME].name = XRT_INPUT_WMR_##NAME)
|
||||
|
||||
//! file path to store controller JSON configuration blocks that
|
||||
//! read from the firmware.
|
||||
DEBUG_GET_ONCE_OPTION(wmr_ctrl_config_path, "WMR_CONFIG_DUMP", NULL)
|
||||
|
||||
static inline struct wmr_bt_controller *
|
||||
wmr_bt_controller(struct xrt_device *p)
|
||||
static inline struct wmr_bt_connection *
|
||||
wmr_bt_connection(struct wmr_controller_connection *p)
|
||||
{
|
||||
return (struct wmr_bt_controller *)p;
|
||||
return (struct wmr_bt_connection *)p;
|
||||
}
|
||||
|
||||
static bool
|
||||
read_packets(struct wmr_bt_controller *d)
|
||||
read_packets(struct wmr_bt_connection *conn)
|
||||
{
|
||||
DRV_TRACE_MARKER();
|
||||
|
||||
unsigned char buffer[WMR_MOTION_CONTROLLER_MSG_BUFFER_SIZE];
|
||||
|
||||
// Better cpu efficiency with blocking reads instead of multiple reads.
|
||||
int size = os_hid_read(d->controller_hid, buffer, sizeof(buffer), 500);
|
||||
os_mutex_lock(&conn->hid_lock);
|
||||
int size = os_hid_read(conn->controller_hid, buffer, sizeof(buffer), 500);
|
||||
|
||||
// Get the timing as close to reading packet as possible.
|
||||
uint64_t now_ns = os_monotonic_get_ns();
|
||||
os_mutex_unlock(&conn->hid_lock);
|
||||
|
||||
DRV_TRACE_IDENT(read_packets_got);
|
||||
|
||||
if (size < 0) {
|
||||
WMR_ERROR(d, "WMR Controller (Bluetooth): Error reading from device");
|
||||
WMR_ERROR(conn, "WMR Controller (Bluetooth): Error reading from device");
|
||||
return false;
|
||||
}
|
||||
if (size == 0) {
|
||||
WMR_TRACE(d, "WMR Controller (Bluetooth): No data to read from device");
|
||||
WMR_TRACE(conn, "WMR Controller (Bluetooth): No data to read from device");
|
||||
return true; // No more messages, return.
|
||||
}
|
||||
|
||||
WMR_TRACE(d, "WMR Controller (Bluetooth): Read %u bytes from device", size);
|
||||
WMR_TRACE(conn, "WMR Controller (Bluetooth): Read %u bytes from device", size);
|
||||
|
||||
switch (buffer[0]) {
|
||||
case WMR_BT_MOTION_CONTROLLER_MSG:
|
||||
os_mutex_lock(&d->lock);
|
||||
// Note: skipping msg type byte
|
||||
bool b = wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->input, d->log_level);
|
||||
if (b) {
|
||||
m_imu_3dof_update(&d->fusion, d->input.imu.timestamp_ticks * WMR_MOTION_CONTROLLER_NS_PER_TICK,
|
||||
&d->input.imu.acc, &d->input.imu.gyro);
|
||||
|
||||
d->last_imu_timestamp_ns = now_ns;
|
||||
d->last_angular_velocity = d->input.imu.gyro;
|
||||
|
||||
} else {
|
||||
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i",
|
||||
buffer[0], size);
|
||||
os_mutex_unlock(&d->lock);
|
||||
return false;
|
||||
}
|
||||
os_mutex_unlock(&d->lock);
|
||||
break;
|
||||
default:
|
||||
WMR_DEBUG(d, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], size);
|
||||
break;
|
||||
}
|
||||
struct wmr_controller_connection *wcc = (struct wmr_controller_connection *)conn;
|
||||
wmr_controller_connection_receive_bytes(wcc, now_ns, buffer, size);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* Config functions.
|
||||
*
|
||||
*/
|
||||
|
||||
static int
|
||||
wmr_controller_send_fw_cmd(struct wmr_bt_controller *d,
|
||||
const struct wmr_controller_fw_cmd *fw_cmd,
|
||||
unsigned char response_code,
|
||||
struct wmr_controller_fw_cmd_response *response)
|
||||
{
|
||||
// comms timeout. Replies are usually in 10ms or so but the first can take longer
|
||||
const int timeout_ms = 250;
|
||||
const int timeout_ns = timeout_ms * U_TIME_1MS_IN_NS;
|
||||
uint64_t timeout_start = os_monotonic_get_ns();
|
||||
uint64_t timeout_end_ns = timeout_start + timeout_ns;
|
||||
struct os_hid_device *hid = d->controller_hid;
|
||||
|
||||
os_hid_write(hid, fw_cmd->buf, sizeof(fw_cmd->buf));
|
||||
|
||||
do {
|
||||
int size = os_hid_read(hid, response->buf, sizeof(response->buf), timeout_ms);
|
||||
if (size == -1) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (size < 1) {
|
||||
// Ignore 0-byte reads (timeout) and try again
|
||||
continue;
|
||||
}
|
||||
|
||||
WMR_TRACE(d, "Controller fw read returned %d bytes", size);
|
||||
if (response->buf[0] == response_code) {
|
||||
if (size != sizeof(response->buf) || (response->response.cmd_id_echo != fw_cmd->cmd.cmd_id)) {
|
||||
WMR_DEBUG(
|
||||
d, "Unexpected fw response - size %d (expected %zu), cmd_id_echo %u != cmd_id %u",
|
||||
size, sizeof(response->buf), response->response.cmd_id_echo, fw_cmd->cmd.cmd_id);
|
||||
return -1;
|
||||
}
|
||||
|
||||
response->response.blk_remain = __le32_to_cpu(response->response.blk_remain);
|
||||
return size;
|
||||
}
|
||||
} while (os_monotonic_get_ns() < timeout_end_ns);
|
||||
|
||||
WMR_WARN(d, "Controller fw read timed out after %u ms",
|
||||
(unsigned int)((os_monotonic_get_ns() - timeout_start) / U_TIME_1MS_IN_NS));
|
||||
return -ETIMEDOUT;
|
||||
}
|
||||
|
||||
XRT_MAYBE_UNUSED static int
|
||||
wmr_read_fw_block(struct wmr_bt_controller *d, uint8_t blk_id, uint8_t **out_data, size_t *out_size)
|
||||
{
|
||||
struct wmr_controller_fw_cmd_response fw_cmd_response;
|
||||
|
||||
uint8_t *data;
|
||||
uint8_t *data_pos;
|
||||
uint8_t *data_end;
|
||||
uint32_t data_size;
|
||||
uint32_t remain;
|
||||
|
||||
struct wmr_controller_fw_cmd fw_cmd;
|
||||
memset(&fw_cmd, 0, sizeof(fw_cmd));
|
||||
|
||||
fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, 0xffffffff);
|
||||
if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) {
|
||||
WMR_WARN(d, "Failed to read fw - cmd 0x02 failed to read header");
|
||||
return -1;
|
||||
}
|
||||
|
||||
data_size = fw_cmd_response.response.blk_remain + fw_cmd_response.response.len;
|
||||
WMR_DEBUG(d, "FW header %d bytes, %u bytes in block", fw_cmd_response.response.len, data_size);
|
||||
|
||||
data = calloc(1, data_size + 1);
|
||||
if (!data) {
|
||||
return -1;
|
||||
}
|
||||
data[data_size] = '\0';
|
||||
|
||||
remain = data_size;
|
||||
data_pos = data;
|
||||
data_end = data + data_size;
|
||||
|
||||
uint8_t to_copy = fw_cmd_response.response.len;
|
||||
|
||||
memcpy(data_pos, fw_cmd_response.response.data, to_copy);
|
||||
data_pos += to_copy;
|
||||
remain -= to_copy;
|
||||
|
||||
while (remain > 0) {
|
||||
fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, remain);
|
||||
|
||||
os_nanosleep(U_TIME_1MS_IN_NS * 10); // Sleep 10ms
|
||||
if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) {
|
||||
WMR_WARN(d, "Failed to read fw - cmd 0x02 failed @ offset %zu", data_pos - data);
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t to_copy = fw_cmd_response.response.len;
|
||||
if (data_pos + to_copy > data_end)
|
||||
to_copy = data_end - data_pos;
|
||||
|
||||
WMR_DEBUG(d, "Read %d bytes @ offset %zu / %d", to_copy, data_pos - data, data_size);
|
||||
memcpy(data_pos, fw_cmd_response.response.data, to_copy);
|
||||
data_pos += to_copy;
|
||||
remain -= to_copy;
|
||||
}
|
||||
|
||||
WMR_DEBUG(d, "Read %d-byte FW data block %d", data_size, blk_id);
|
||||
|
||||
*out_data = data;
|
||||
*out_size = data_size;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static bool
|
||||
read_controller_config(struct wmr_bt_controller *d)
|
||||
send_bytes(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size)
|
||||
{
|
||||
unsigned char *data = NULL;
|
||||
unsigned char *config_json_block;
|
||||
size_t data_size;
|
||||
int ret;
|
||||
struct wmr_bt_connection *conn = (struct wmr_bt_connection *)(wcc);
|
||||
|
||||
#if 0
|
||||
// There are extra firmware blocks that can be read from
|
||||
// the controllers, like these. Serial numbers and
|
||||
// USB PID/VID are visible in them, but it's not clear
|
||||
// what the layout is and we don't use them currently,
|
||||
// so this if 0 code is just exemplary.
|
||||
os_mutex_lock(&conn->hid_lock);
|
||||
int ret = os_hid_write(conn->controller_hid, buffer, buf_size);
|
||||
os_mutex_unlock(&conn->hid_lock);
|
||||
|
||||
// Read serials
|
||||
ret = wmr_read_fw_block(d, 0x03, &data, &data_size);
|
||||
if (ret < 0 || data == NULL)
|
||||
return false;
|
||||
free(data);
|
||||
data = NULL;
|
||||
|
||||
// Read block 0x14
|
||||
ret = wmr_read_fw_block(d, 0x14, &data, &data_size);
|
||||
if (ret < 0 || data == NULL)
|
||||
return false;
|
||||
free(data);
|
||||
data = NULL;
|
||||
#endif
|
||||
|
||||
// Read config block
|
||||
ret = wmr_read_fw_block(d, 0x02, &data, &data_size);
|
||||
if (ret < 0 || data == NULL)
|
||||
return false;
|
||||
|
||||
/* De-obfuscate the JSON config */
|
||||
config_json_block = data + sizeof(uint16_t);
|
||||
for (unsigned int i = 0; i < data_size - sizeof(uint16_t); i++) {
|
||||
config_json_block[i] ^= wmr_config_key[i % sizeof(wmr_config_key)];
|
||||
}
|
||||
|
||||
#if 1
|
||||
// Option to dump config block to a path. Later, these will be
|
||||
// stored in a cache to save time on future startup
|
||||
const char *dump_dir = debug_get_option_wmr_ctrl_config_path();
|
||||
if (dump_dir != NULL) {
|
||||
char fname[256];
|
||||
|
||||
int device_id = (d->base.device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) ? 0 : 1;
|
||||
|
||||
sprintf(fname, "%s/controller-%d-fw.txt", dump_dir, device_id);
|
||||
WMR_INFO(d, "Storing controller config JSON to %s", fname);
|
||||
|
||||
FILE *f = fopen(fname, "w");
|
||||
fwrite(config_json_block, data_size - 2, 1, f);
|
||||
fclose(f);
|
||||
}
|
||||
#endif
|
||||
|
||||
if (!wmr_controller_config_parse(&d->config, (char *)config_json_block, d->log_level)) {
|
||||
free(data);
|
||||
return false;
|
||||
}
|
||||
|
||||
WMR_DEBUG(d, "Parsed %d LED entries from controller calibration", d->config.led_count);
|
||||
|
||||
free(data);
|
||||
return true;
|
||||
return ret != -1 && (uint32_t)(ret) == buf_size;
|
||||
}
|
||||
|
||||
static void
|
||||
wmr_bt_controller_set_output(struct xrt_device *xdev, enum xrt_output_name name, const union xrt_output_value *value)
|
||||
/* Synchronously read a buffer from the HID connection.
|
||||
* This is only used for reading firmware during startup,
|
||||
* before the hid reading loop is running */
|
||||
static int
|
||||
read_sync(struct wmr_controller_connection *wcc, uint8_t *buffer, uint32_t buf_size, int timeout_ms)
|
||||
{
|
||||
DRV_TRACE_MARKER();
|
||||
struct wmr_bt_connection *conn = (struct wmr_bt_connection *)(wcc);
|
||||
|
||||
// struct wmr_bt_controller *d = wmr_bt_controller(xdev);
|
||||
// Todo: implement
|
||||
}
|
||||
os_mutex_lock(&conn->hid_lock);
|
||||
int res = os_hid_read(conn->controller_hid, buffer, buf_size, timeout_ms);
|
||||
os_mutex_unlock(&conn->hid_lock);
|
||||
|
||||
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)
|
||||
{
|
||||
DRV_TRACE_MARKER();
|
||||
|
||||
struct wmr_bt_controller *d = wmr_bt_controller(xdev);
|
||||
|
||||
// Variables needed for prediction.
|
||||
uint64_t last_imu_timestamp_ns = 0;
|
||||
struct xrt_space_relation relation = {0};
|
||||
relation.relation_flags = (enum xrt_space_relation_flags)(
|
||||
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
|
||||
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
|
||||
|
||||
|
||||
struct xrt_pose pose = {{0, 0, 0, 1}, {0, 1.2, -0.5}};
|
||||
if (xdev->device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
|
||||
pose.position.x = -0.2;
|
||||
} else {
|
||||
pose.position.x = 0.2;
|
||||
}
|
||||
relation.pose = pose;
|
||||
|
||||
// Copy data while holding the lock.
|
||||
os_mutex_lock(&d->lock);
|
||||
relation.pose.orientation = d->fusion.rot;
|
||||
relation.angular_velocity = d->last_angular_velocity;
|
||||
last_imu_timestamp_ns = d->last_imu_timestamp_ns;
|
||||
os_mutex_unlock(&d->lock);
|
||||
|
||||
// No prediction needed.
|
||||
if (at_timestamp_ns < last_imu_timestamp_ns) {
|
||||
*out_relation = relation;
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns;
|
||||
double prediction_s = time_ns_to_s(prediction_ns);
|
||||
|
||||
m_predict_relation(&relation, prediction_s, out_relation);
|
||||
}
|
||||
|
||||
|
||||
|
||||
static void
|
||||
wmr_bt_controller_update_inputs(struct xrt_device *xdev)
|
||||
{
|
||||
DRV_TRACE_MARKER();
|
||||
|
||||
struct wmr_bt_controller *d = wmr_bt_controller(xdev);
|
||||
|
||||
struct xrt_input *inputs = d->base.inputs;
|
||||
|
||||
os_mutex_lock(&d->lock);
|
||||
|
||||
inputs[WMR_INDEX_MENU_CLICK].value.boolean = d->input.menu;
|
||||
inputs[WMR_INDEX_SQUEEZE_CLICK].value.boolean = d->input.squeeze;
|
||||
inputs[WMR_INDEX_TRIGGER_VALUE].value.vec1.x = d->input.trigger;
|
||||
inputs[WMR_INDEX_THUMBSTICK_CLICK].value.boolean = d->input.thumbstick.click;
|
||||
inputs[WMR_INDEX_THUMBSTICK].value.vec2 = d->input.thumbstick.values;
|
||||
inputs[WMR_INDEX_TRACKPAD_CLICK].value.boolean = d->input.trackpad.click;
|
||||
inputs[WMR_INDEX_TRACKPAD_TOUCH].value.boolean = d->input.trackpad.touch;
|
||||
inputs[WMR_INDEX_TRACKPAD].value.vec2 = d->input.trackpad.values;
|
||||
|
||||
os_mutex_unlock(&d->lock);
|
||||
return res;
|
||||
}
|
||||
|
||||
static void *
|
||||
wmr_bt_controller_run_thread(void *ptr)
|
||||
wmr_bt_connection_run_thread(void *ptr)
|
||||
{
|
||||
U_TRACE_SET_THREAD_NAME("WMR: BT-Controller");
|
||||
|
||||
struct wmr_bt_controller *d = wmr_bt_controller(ptr);
|
||||
struct wmr_bt_connection *conn = wmr_bt_connection(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);
|
||||
os_thread_helper_lock(&conn->controller_thread);
|
||||
while (os_thread_helper_is_running_locked(&conn->controller_thread)) {
|
||||
os_thread_helper_unlock(&conn->controller_thread);
|
||||
|
||||
// Does not block.
|
||||
if (!read_packets(d)) {
|
||||
if (!read_packets(conn)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
WMR_DEBUG(d, "WMR Controller (Bluetooth): Exiting reading thread.");
|
||||
WMR_DEBUG(conn, "WMR Controller (Bluetooth): Exiting reading thread.");
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
wmr_bt_controller_destroy(struct xrt_device *xdev)
|
||||
wmr_bt_connection_destroy(struct wmr_controller_connection *base)
|
||||
{
|
||||
struct wmr_bt_connection *conn = (struct wmr_bt_connection *)base;
|
||||
|
||||
DRV_TRACE_MARKER();
|
||||
|
||||
struct wmr_bt_controller *d = wmr_bt_controller(xdev);
|
||||
|
||||
// Remove the variable tracking.
|
||||
u_var_remove_root(d);
|
||||
|
||||
// Destroy the thread object.
|
||||
os_thread_helper_destroy(&d->controller_thread);
|
||||
os_thread_helper_destroy(&conn->controller_thread);
|
||||
|
||||
if (d->controller_hid != NULL) {
|
||||
os_hid_destroy(d->controller_hid);
|
||||
d->controller_hid = NULL;
|
||||
if (conn->controller_hid != NULL) {
|
||||
os_hid_destroy(conn->controller_hid);
|
||||
conn->controller_hid = NULL;
|
||||
}
|
||||
|
||||
os_mutex_destroy(&d->lock);
|
||||
os_mutex_destroy(&conn->hid_lock);
|
||||
|
||||
// Destroy the fusion.
|
||||
m_imu_3dof_close(&d->fusion);
|
||||
|
||||
free(d);
|
||||
free(conn);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
* 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 xrt_device *
|
||||
wmr_bt_controller_create(struct os_hid_device *controller_hid,
|
||||
enum xrt_device_type controller_type,
|
||||
|
@ -466,107 +152,52 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
|
|||
{
|
||||
DRV_TRACE_MARKER();
|
||||
|
||||
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
|
||||
struct wmr_bt_controller *d = U_DEVICE_ALLOCATE(struct wmr_bt_controller, flags, 10, 1);
|
||||
|
||||
d->log_level = log_level;
|
||||
d->controller_hid = controller_hid;
|
||||
|
||||
if (controller_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
|
||||
snprintf(d->base.str, ARRAY_SIZE(d->base.str), "WMR Left Controller");
|
||||
} else {
|
||||
snprintf(d->base.str, ARRAY_SIZE(d->base.str), "WMR Right Controller");
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
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 < d->base.input_count; i++) {
|
||||
d->base.inputs[0].active = true;
|
||||
}
|
||||
|
||||
d->base.outputs[0].name = XRT_OUTPUT_NAME_WMR_HAPTIC;
|
||||
|
||||
d->base.binding_profiles = binding_profiles;
|
||||
d->base.binding_profile_count = ARRAY_SIZE(binding_profiles);
|
||||
|
||||
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;
|
||||
|
||||
|
||||
d->input.imu.timestamp_ticks = 0;
|
||||
m_imu_3dof_init(&d->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
|
||||
struct wmr_bt_connection *conn = calloc(1, sizeof(struct wmr_bt_connection));
|
||||
|
||||
conn->log_level = log_level;
|
||||
conn->controller_hid = controller_hid;
|
||||
|
||||
conn->base.send_bytes = send_bytes;
|
||||
conn->base.read_sync = read_sync;
|
||||
conn->base.disconnect = wmr_bt_connection_destroy;
|
||||
|
||||
int ret = 0;
|
||||
|
||||
ret = os_mutex_init(&d->lock);
|
||||
ret = os_mutex_init(&conn->hid_lock);
|
||||
if (ret != 0) {
|
||||
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to init mutex!");
|
||||
wmr_bt_controller_destroy(&d->base);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Read config file from controller
|
||||
if (!read_controller_config(d)) {
|
||||
wmr_bt_controller_destroy(&d->base);
|
||||
WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to init mutex!");
|
||||
wmr_bt_connection_destroy(&conn->base);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Thread and other state.
|
||||
ret = os_thread_helper_init(&d->controller_thread);
|
||||
ret = os_thread_helper_init(&conn->controller_thread);
|
||||
if (ret != 0) {
|
||||
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to init controller threading!");
|
||||
wmr_bt_controller_destroy(&d->base);
|
||||
d = NULL;
|
||||
WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to init controller threading!");
|
||||
wmr_bt_connection_destroy(&conn->base);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Takes ownership of the connection
|
||||
struct wmr_controller_base *wcb = wmr_controller_base_create(&conn->base, controller_type, log_level);
|
||||
if (wcb == NULL) {
|
||||
WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to create controller");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// If the controller device was created, the connection belongs to
|
||||
// it now and will be cleaned up when it calls disconnect().
|
||||
conn->base.wcb = wcb;
|
||||
|
||||
struct xrt_device *xdev = &wcb->base;
|
||||
|
||||
// Hand over controller device to reading thread.
|
||||
ret = os_thread_helper_start(&d->controller_thread, wmr_bt_controller_run_thread, d);
|
||||
ret = os_thread_helper_start(&conn->controller_thread, wmr_bt_connection_run_thread, conn);
|
||||
if (ret != 0) {
|
||||
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to start controller thread!");
|
||||
wmr_bt_controller_destroy(&d->base);
|
||||
d = NULL;
|
||||
WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to start controller thread!");
|
||||
xrt_device_destroy(&xdev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
u_var_add_root(d, d->base.str, true);
|
||||
u_var_add_bool(d, &d->input.menu, "input.menu");
|
||||
u_var_add_bool(d, &d->input.home, "input.home");
|
||||
u_var_add_bool(d, &d->input.bt_pairing, "input.bt_pairing");
|
||||
u_var_add_bool(d, &d->input.squeeze, "input.squeeze");
|
||||
u_var_add_f32(d, &d->input.trigger, "input.trigger");
|
||||
u_var_add_u8(d, &d->input.battery, "input.battery");
|
||||
u_var_add_bool(d, &d->input.thumbstick.click, "input.thumbstick.click");
|
||||
u_var_add_f32(d, &d->input.thumbstick.values.x, "input.thumbstick.values.y");
|
||||
u_var_add_f32(d, &d->input.thumbstick.values.y, "input.thumbstick.values.x");
|
||||
u_var_add_bool(d, &d->input.trackpad.click, "input.trackpad.click");
|
||||
u_var_add_bool(d, &d->input.trackpad.touch, "input.trackpad.touch");
|
||||
u_var_add_f32(d, &d->input.trackpad.values.x, "input.trackpad.values.x");
|
||||
u_var_add_f32(d, &d->input.trackpad.values.y, "input.trackpad.values.y");
|
||||
u_var_add_ro_vec3_f32(d, &d->input.imu.acc, "imu.acc");
|
||||
u_var_add_ro_vec3_f32(d, &d->input.imu.gyro, "imu.gyro");
|
||||
u_var_add_i32(d, &d->input.imu.temperature, "imu.temperature");
|
||||
|
||||
|
||||
return &d->base;
|
||||
return xdev;
|
||||
}
|
||||
|
|
|
@ -1,23 +1,19 @@
|
|||
// Copyright 2020-2021, N Madsen.
|
||||
// Copyright 2020-2021, Collabora, Ltd.
|
||||
// Copyright 2020-2023, Jan Schmidt
|
||||
// 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
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "os/os_threading.h"
|
||||
#include "math/m_imu_3dof.h"
|
||||
#include "util/u_logging.h"
|
||||
#include "xrt/xrt_device.h"
|
||||
|
||||
#include "wmr_controller_protocol.h"
|
||||
#include "wmr_config.h"
|
||||
#include "wmr_controller_base.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
|
@ -25,59 +21,30 @@ extern "C" {
|
|||
|
||||
|
||||
/*!
|
||||
* 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,
|
||||
};
|
||||
|
||||
/*!
|
||||
* A Bluetooth connected WMR Controller device, representing just a single controller.
|
||||
* A connection to a Bluetooth connected WMR Controller device
|
||||
*
|
||||
* @ingroup drv_wmr
|
||||
* @implements xrt_device
|
||||
* @implements wmr_controller_connection
|
||||
*/
|
||||
struct wmr_bt_controller
|
||||
struct wmr_bt_connection
|
||||
{
|
||||
struct xrt_device base;
|
||||
struct wmr_controller_connection base;
|
||||
|
||||
enum u_logging_level log_level;
|
||||
|
||||
struct os_hid_device *controller_hid;
|
||||
struct os_thread_helper controller_thread;
|
||||
|
||||
/* firmware configuration block */
|
||||
struct wmr_controller_config config;
|
||||
struct os_mutex hid_lock;
|
||||
|
||||
struct os_mutex lock;
|
||||
|
||||
//! 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.
|
||||
struct m_imu_3dof fusion;
|
||||
//! The last angular velocity from the IMU, for prediction.
|
||||
struct xrt_vec3 last_angular_velocity;
|
||||
|
||||
enum u_logging_level log_level;
|
||||
struct wmr_controller_base *wcb;
|
||||
};
|
||||
|
||||
|
||||
struct xrt_device *
|
||||
wmr_bt_controller_create(struct os_hid_device *controller_hid,
|
||||
enum xrt_device_type controller_type,
|
||||
enum u_logging_level log_level);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
565
src/xrt/drivers/wmr/wmr_controller_base.c
Normal file
565
src/xrt/drivers/wmr/wmr_controller_base.c
Normal file
|
@ -0,0 +1,565 @@
|
|||
// 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 Controller.
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @ingroup drv_wmr
|
||||
*/
|
||||
|
||||
#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 "util/u_trace_marker.h"
|
||||
|
||||
#include "wmr_common.h"
|
||||
#include "wmr_controller_base.h"
|
||||
#include "wmr_config_key.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <errno.h>
|
||||
|
||||
#define WMR_TRACE(wcb, ...) U_LOG_XDEV_IFL_T(&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_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 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.
|
||||
DEBUG_GET_ONCE_OPTION(wmr_ctrl_config_path, "WMR_CONFIG_DUMP", NULL)
|
||||
|
||||
static inline struct wmr_controller_base *
|
||||
wmr_controller_base(struct xrt_device *p)
|
||||
{
|
||||
return (struct wmr_controller_base *)p;
|
||||
}
|
||||
|
||||
static void
|
||||
receive_bytes(struct wmr_controller_base *wcb, uint64_t time_ns, uint8_t *buffer, uint32_t buf_size)
|
||||
{
|
||||
if (buf_size < 1) {
|
||||
WMR_ERROR(wcb, "WMR Controller: Error receiving short packet");
|
||||
return;
|
||||
}
|
||||
|
||||
switch (buffer[0]) {
|
||||
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);
|
||||
|
||||
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);
|
||||
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);
|
||||
break;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
static bool
|
||||
wmr_controller_send_bytes(struct wmr_controller_base *wcb, const uint8_t *buffer, uint32_t buf_size)
|
||||
{
|
||||
bool res = false;
|
||||
|
||||
os_mutex_lock(&wcb->conn_lock);
|
||||
struct wmr_controller_connection *conn = wcb->wcc;
|
||||
if (conn != NULL) {
|
||||
res = wmr_controller_connection_send_bytes(conn, buffer, buf_size);
|
||||
}
|
||||
os_mutex_unlock(&wcb->conn_lock);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
static int
|
||||
wmr_controller_read_sync(struct wmr_controller_base *wcb, uint8_t *buffer, uint32_t buf_size, int timeout_ms)
|
||||
{
|
||||
int res = -1;
|
||||
os_mutex_lock(&wcb->conn_lock);
|
||||
struct wmr_controller_connection *conn = wcb->wcc;
|
||||
if (conn != NULL) {
|
||||
res = wmr_controller_connection_read_sync(conn, buffer, buf_size, timeout_ms);
|
||||
}
|
||||
os_mutex_unlock(&wcb->conn_lock);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
static int
|
||||
wmr_controller_send_fw_cmd(struct wmr_controller_base *wcb,
|
||||
const struct wmr_controller_fw_cmd *fw_cmd,
|
||||
unsigned char response_code,
|
||||
struct wmr_controller_fw_cmd_response *response)
|
||||
{
|
||||
// comms timeout. Replies are usually in 10ms or so but the first can take longer
|
||||
const int timeout_ms = 250;
|
||||
const int timeout_ns = timeout_ms * U_TIME_1MS_IN_NS;
|
||||
uint64_t timeout_start = os_monotonic_get_ns();
|
||||
uint64_t timeout_end_ns = timeout_start + timeout_ns;
|
||||
|
||||
if (!wmr_controller_send_bytes(wcb, fw_cmd->buf, sizeof(fw_cmd->buf))) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
do {
|
||||
int size = wmr_controller_read_sync(wcb, response->buf, sizeof(response->buf), timeout_ms);
|
||||
if (size == -1) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (size < 1) {
|
||||
// Ignore 0-byte reads (timeout) and try again
|
||||
continue;
|
||||
}
|
||||
|
||||
WMR_TRACE(wcb, "Controller fw read returned %d bytes", size);
|
||||
if (response->buf[0] == response_code) {
|
||||
if (size != sizeof(response->buf) || (response->response.cmd_id_echo != fw_cmd->cmd.cmd_id)) {
|
||||
WMR_DEBUG(
|
||||
wcb, "Unexpected fw response - size %d (expected %zu), cmd_id_echo %u != cmd_id %u",
|
||||
size, sizeof(response->buf), response->response.cmd_id_echo, fw_cmd->cmd.cmd_id);
|
||||
return -1;
|
||||
}
|
||||
|
||||
response->response.blk_remain = __le32_to_cpu(response->response.blk_remain);
|
||||
return size;
|
||||
}
|
||||
} while (os_monotonic_get_ns() < timeout_end_ns);
|
||||
|
||||
WMR_WARN(wcb, "Controller fw read timed out after %u ms",
|
||||
(unsigned int)((os_monotonic_get_ns() - timeout_start) / U_TIME_1MS_IN_NS));
|
||||
return -ETIMEDOUT;
|
||||
}
|
||||
|
||||
XRT_MAYBE_UNUSED static int
|
||||
wmr_read_fw_block(struct wmr_controller_base *d, uint8_t blk_id, uint8_t **out_data, size_t *out_size)
|
||||
{
|
||||
struct wmr_controller_fw_cmd_response fw_cmd_response;
|
||||
|
||||
uint8_t *data;
|
||||
uint8_t *data_pos;
|
||||
uint8_t *data_end;
|
||||
uint32_t data_size;
|
||||
uint32_t remain;
|
||||
|
||||
struct wmr_controller_fw_cmd fw_cmd;
|
||||
memset(&fw_cmd, 0, sizeof(fw_cmd));
|
||||
|
||||
fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, 0xffffffff);
|
||||
if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) {
|
||||
WMR_WARN(d, "Failed to read fw - cmd 0x02 failed to read header for block %d", blk_id);
|
||||
return -1;
|
||||
}
|
||||
|
||||
data_size = fw_cmd_response.response.blk_remain + fw_cmd_response.response.len;
|
||||
WMR_DEBUG(d, "FW header %d bytes, %u bytes in block", fw_cmd_response.response.len, data_size);
|
||||
if (data_size == 0)
|
||||
return -1;
|
||||
|
||||
data = calloc(1, data_size + 1);
|
||||
if (!data) {
|
||||
return -1;
|
||||
}
|
||||
data[data_size] = '\0';
|
||||
|
||||
remain = data_size;
|
||||
data_pos = data;
|
||||
data_end = data + data_size;
|
||||
|
||||
uint8_t to_copy = fw_cmd_response.response.len;
|
||||
|
||||
memcpy(data_pos, fw_cmd_response.response.data, to_copy);
|
||||
data_pos += to_copy;
|
||||
remain -= to_copy;
|
||||
|
||||
while (remain > 0) {
|
||||
fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, remain);
|
||||
|
||||
os_nanosleep(U_TIME_1MS_IN_NS * 10); // Sleep 10ms
|
||||
if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) {
|
||||
WMR_WARN(d, "Failed to read fw - cmd 0x02 failed @ offset %zu", data_pos - data);
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint8_t to_copy = fw_cmd_response.response.len;
|
||||
if (data_pos + to_copy > data_end)
|
||||
to_copy = data_end - data_pos;
|
||||
|
||||
WMR_DEBUG(d, "Read %d bytes @ offset %zu / %d", to_copy, data_pos - data, data_size);
|
||||
memcpy(data_pos, fw_cmd_response.response.data, to_copy);
|
||||
data_pos += to_copy;
|
||||
remain -= to_copy;
|
||||
}
|
||||
|
||||
WMR_DEBUG(d, "Read %d-byte FW data block %d", data_size, blk_id);
|
||||
|
||||
*out_data = data;
|
||||
*out_size = data_size;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* Config functions.
|
||||
*
|
||||
*/
|
||||
|
||||
static bool
|
||||
read_controller_config(struct wmr_controller_base *wcb)
|
||||
{
|
||||
unsigned char *data = NULL;
|
||||
unsigned char *config_json_block;
|
||||
size_t data_size;
|
||||
int ret;
|
||||
|
||||
#if 1
|
||||
// There are extra firmware blocks that can be read from
|
||||
// the controllers, like these. Serial numbers and
|
||||
// USB PID/VID are visible in them, but it's not clear
|
||||
// what the layout is and we don't use them currently,
|
||||
// so this if 0 code is just exemplary.
|
||||
|
||||
// Read 0x00 block
|
||||
ret = wmr_read_fw_block(wcb, 0x0, &data, &data_size);
|
||||
if (ret < 0 || data == NULL)
|
||||
return false;
|
||||
free(data);
|
||||
data = NULL;
|
||||
|
||||
// Read serials
|
||||
ret = wmr_read_fw_block(wcb, 0x03, &data, &data_size);
|
||||
if (ret < 0 || data == NULL)
|
||||
return false;
|
||||
free(data);
|
||||
data = NULL;
|
||||
|
||||
// Read block 0x14
|
||||
ret = wmr_read_fw_block(wcb, 0x14, &data, &data_size);
|
||||
if (ret < 0 || data == NULL)
|
||||
return false;
|
||||
free(data);
|
||||
data = NULL;
|
||||
#endif
|
||||
|
||||
// Read config block
|
||||
ret = wmr_read_fw_block(wcb, 0x02, &data, &data_size);
|
||||
if (ret < 0 || data == NULL)
|
||||
return false;
|
||||
|
||||
/* De-obfuscate the JSON config */
|
||||
config_json_block = data + sizeof(uint16_t);
|
||||
for (unsigned int i = 0; i < data_size - sizeof(uint16_t); i++) {
|
||||
config_json_block[i] ^= wmr_config_key[i % sizeof(wmr_config_key)];
|
||||
}
|
||||
|
||||
#if 1
|
||||
// Option to dump config block to a path. Later, these will be
|
||||
// stored in a cache to save time on future startup
|
||||
const char *dump_dir = debug_get_option_wmr_ctrl_config_path();
|
||||
if (dump_dir != NULL) {
|
||||
char fname[256];
|
||||
|
||||
int device_id = (wcb->base.device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) ? 0 : 1;
|
||||
|
||||
sprintf(fname, "%s/controller-%d-fw.txt", dump_dir, device_id);
|
||||
WMR_INFO(wcb, "Storing controller config JSON to %s", fname);
|
||||
|
||||
FILE *f = fopen(fname, "w");
|
||||
fwrite(config_json_block, data_size - 2, 1, f);
|
||||
fclose(f);
|
||||
}
|
||||
#endif
|
||||
|
||||
if (!wmr_controller_config_parse(&wcb->config, (char *)config_json_block, wcb->log_level)) {
|
||||
free(data);
|
||||
return false;
|
||||
}
|
||||
|
||||
WMR_DEBUG(wcb, "Parsed %d LED entries from controller calibration", wcb->config.led_count);
|
||||
|
||||
free(data);
|
||||
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,
|
||||
uint64_t at_timestamp_ns,
|
||||
struct xrt_space_relation *out_relation)
|
||||
{
|
||||
DRV_TRACE_MARKER();
|
||||
|
||||
struct wmr_controller_base *wcb = wmr_controller_base(xdev);
|
||||
|
||||
// Variables needed for prediction.
|
||||
uint64_t last_imu_timestamp_ns = 0;
|
||||
struct xrt_space_relation relation = {0};
|
||||
relation.relation_flags = (enum xrt_space_relation_flags)(
|
||||
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
|
||||
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
|
||||
|
||||
|
||||
struct xrt_pose pose = {{0, 0, 0, 1}, {0, 1.2, -0.5}};
|
||||
if (xdev->device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
|
||||
pose.position.x = -0.2;
|
||||
} else {
|
||||
pose.position.x = 0.2;
|
||||
}
|
||||
relation.pose = pose;
|
||||
|
||||
// Copy data while holding the lock.
|
||||
os_mutex_lock(&wcb->data_lock);
|
||||
relation.pose.orientation = wcb->fusion.rot;
|
||||
relation.angular_velocity = wcb->last_angular_velocity;
|
||||
last_imu_timestamp_ns = wcb->last_imu_timestamp_ns;
|
||||
os_mutex_unlock(&wcb->data_lock);
|
||||
|
||||
// No prediction needed.
|
||||
if (at_timestamp_ns < last_imu_timestamp_ns) {
|
||||
*out_relation = relation;
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns;
|
||||
double prediction_s = time_ns_to_s(prediction_ns);
|
||||
|
||||
m_predict_relation(&relation, prediction_s, out_relation);
|
||||
}
|
||||
|
||||
|
||||
|
||||
static void
|
||||
wmr_controller_base_update_inputs(struct xrt_device *xdev)
|
||||
{
|
||||
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);
|
||||
|
||||
// Disconnect from the connection so we don't
|
||||
// receive any more callbacks
|
||||
os_mutex_lock(&wcb->conn_lock);
|
||||
struct wmr_controller_connection *conn = wcb->wcc;
|
||||
wcb->wcc = NULL;
|
||||
os_mutex_unlock(&wcb->conn_lock);
|
||||
|
||||
if (conn != NULL) {
|
||||
wmr_controller_connection_disconnect(conn);
|
||||
}
|
||||
|
||||
os_mutex_destroy(&wcb->conn_lock);
|
||||
os_mutex_destroy(&wcb->data_lock);
|
||||
|
||||
// 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)
|
||||
{
|
||||
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;
|
||||
|
||||
if (controller_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
|
||||
snprintf(wcb->base.str, ARRAY_SIZE(wcb->base.str), "WMR Left Controller");
|
||||
} else {
|
||||
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;
|
||||
wcb->base.orientation_tracking_supported = true;
|
||||
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;
|
||||
}
|
||||
|
||||
// Read config file from controller
|
||||
if (!read_controller_config(wcb)) {
|
||||
wmr_controller_base_destroy(&wcb->base);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
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");
|
||||
|
||||
return wcb;
|
||||
}
|
136
src/xrt/drivers/wmr/wmr_controller_base.h
Normal file
136
src/xrt/drivers/wmr/wmr_controller_base.h
Normal file
|
@ -0,0 +1,136 @@
|
|||
// Copyright 2020-2021, N Madsen.
|
||||
// Copyright 2020-2021, Collabora, Ltd.
|
||||
// Copyright 2021-2023, Jan Schmidt
|
||||
// SPDX-License-Identifier: BSL-1.0
|
||||
//
|
||||
/*!
|
||||
* @file
|
||||
* @brief Common implementation for WMR controllers, handling
|
||||
* shared behaviour such as communication, configuration reading,
|
||||
* IMU integration.
|
||||
* @author Jan Schmidt <jan@centricular.com>
|
||||
* @author Nis Madsen <nima_zero_one@protonmail.com>
|
||||
* @ingroup drv_wmr
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "os/os_threading.h"
|
||||
#include "math/m_imu_3dof.h"
|
||||
#include "util/u_logging.h"
|
||||
#include "xrt/xrt_device.h"
|
||||
|
||||
#include "wmr_controller_protocol.h"
|
||||
#include "wmr_config.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
struct wmr_controller_base;
|
||||
|
||||
/*!
|
||||
* A connection for communicating with the controller.
|
||||
* The mechanism is implementation specific, so there are
|
||||
* two variants for either communicating directly with a
|
||||
* controller via bluetooth, and another for talking
|
||||
* to a controller through a headset tunnelled mapping.
|
||||
*
|
||||
* The controller implementation doesn't need to care how
|
||||
* the communication is implemented.
|
||||
*
|
||||
* The connection is reference counted and mutex protected,
|
||||
* as both the controller and the connection implementation
|
||||
* may need to hold a reference to it, and to detach
|
||||
* safely when shutting down.
|
||||
*/
|
||||
struct wmr_controller_connection
|
||||
{
|
||||
//! The controller this connection is talking to.
|
||||
struct wmr_controller_base *wcb;
|
||||
|
||||
bool (*send_bytes)(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size);
|
||||
int (*read_sync)(struct wmr_controller_connection *wcc, uint8_t *buffer, uint32_t buf_size, int timeout_ms);
|
||||
|
||||
void (*disconnect)(struct wmr_controller_connection *wcc);
|
||||
};
|
||||
|
||||
static inline bool
|
||||
wmr_controller_connection_send_bytes(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size)
|
||||
{
|
||||
assert(wcc->send_bytes != NULL);
|
||||
return wcc->send_bytes(wcc, buffer, buf_size);
|
||||
}
|
||||
|
||||
static inline int
|
||||
wmr_controller_connection_read_sync(struct wmr_controller_connection *wcc,
|
||||
uint8_t *buffer,
|
||||
uint32_t buf_size,
|
||||
int timeout_ms)
|
||||
{
|
||||
return wcc->read_sync(wcc, buffer, buf_size, timeout_ms);
|
||||
}
|
||||
|
||||
static inline void
|
||||
wmr_controller_connection_disconnect(struct wmr_controller_connection *wcc)
|
||||
{
|
||||
wcc->disconnect(wcc);
|
||||
}
|
||||
|
||||
/*!
|
||||
* Common base for all WMR controllers.
|
||||
*
|
||||
* @ingroup drv_wmr
|
||||
* @implements xrt_device
|
||||
*/
|
||||
struct wmr_controller_base
|
||||
{
|
||||
//! Base struct.
|
||||
struct xrt_device base;
|
||||
|
||||
//! Mutex protects the controller connection
|
||||
struct os_mutex conn_lock;
|
||||
|
||||
//! The connection for this controller.
|
||||
struct wmr_controller_connection *wcc;
|
||||
|
||||
//! Callback from the connection when a packet has been received.
|
||||
void (*receive_bytes)(struct wmr_controller_base *wcb, uint64_t time_ns, uint8_t *buffer, uint32_t buf_size);
|
||||
|
||||
enum u_logging_level log_level;
|
||||
|
||||
//! Mutex protects shared data used from OpenXR callbacks
|
||||
struct os_mutex data_lock;
|
||||
|
||||
/* 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.
|
||||
struct m_imu_3dof fusion;
|
||||
//! The last angular velocity from the IMU, for prediction.
|
||||
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);
|
||||
|
||||
|
||||
static inline void
|
||||
wmr_controller_connection_receive_bytes(struct wmr_controller_connection *wcc,
|
||||
uint64_t time_ns,
|
||||
uint8_t *buffer,
|
||||
uint32_t buf_size)
|
||||
{
|
||||
struct wmr_controller_base *wcb = wcc->wcb;
|
||||
|
||||
wcb->receive_bytes(wcb, time_ns, buffer, buf_size);
|
||||
}
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
|
@ -37,8 +37,8 @@ extern "C" {
|
|||
#define WMR_MOTION_CONTROLLER_NS_PER_TICK 100
|
||||
|
||||
|
||||
// Messages types specific to Bluetooth connected WMR motion controllers
|
||||
#define WMR_BT_MOTION_CONTROLLER_MSG 0x01
|
||||
// Messages types for WMR motion controllers
|
||||
#define WMR_MOTION_CONTROLLER_STATUS_MSG 0x01
|
||||
|
||||
|
||||
struct wmr_controller_input
|
||||
|
|
|
@ -416,10 +416,10 @@ wmr_create_bt_controller(struct xrt_prober *xp,
|
|||
return XRT_ERROR_DEVICE_CREATION_FAILED;
|
||||
}
|
||||
|
||||
// Takes ownership of the hid_controller, even on failure
|
||||
struct xrt_device *xdev = wmr_bt_controller_create(hid_controller, controller_type, log_level);
|
||||
if (xdev == NULL) {
|
||||
U_LOG_IFL_E(log_level, "Failed to create WMR controller (Bluetooth)");
|
||||
os_hid_destroy(hid_controller);
|
||||
return XRT_ERROR_DEVICE_CREATION_FAILED;
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in a new issue