mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-15 10:10:07 +00:00
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:
parent
f33326e90b
commit
ea53d274a5
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
45
src/xrt/drivers/wmr/wmr_controller.c
Normal file
45
src/xrt/drivers/wmr/wmr_controller.c
Normal 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;
|
||||
}
|
22
src/xrt/drivers/wmr/wmr_controller.h
Normal file
22
src/xrt/drivers/wmr/wmr_controller.h
Normal 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);
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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,
|
||||
|
|
20
src/xrt/drivers/wmr/wmr_controller_hp.c
Normal file
20
src/xrt/drivers/wmr/wmr_controller_hp.c
Normal 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;
|
||||
}
|
361
src/xrt/drivers/wmr/wmr_controller_og.c
Normal file
361
src/xrt/drivers/wmr/wmr_controller_og.c
Normal 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;
|
||||
}
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in a new issue