matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2021, N Madsen.
2// Copyright 2020-2023, Collabora, Ltd.
3// Copyright 2020-2023, Jan Schmidt
4// SPDX-License-Identifier: BSL-1.0
5/*!
6 * @file
7 * @brief Driver for WMR Controllers.
8 * @author Jan Schmidt <jan@centricular.com>
9 * @ingroup drv_wmr
10 */
11#include "math/m_api.h"
12
13#include "util/u_device.h"
14#include "util/u_trace_marker.h"
15#include "util/u_var.h"
16
17#include <stdio.h>
18#include <stdlib.h>
19#include <string.h>
20#include <assert.h>
21#include <errno.h>
22
23#include "wmr_controller.h"
24
25#define WMR_TRACE(ctrl, ...) U_LOG_XDEV_IFL_T(&ctrl->base.base, ctrl->base.log_level, __VA_ARGS__)
26#define WMR_TRACE_HEX(ctrl, ...) U_LOG_XDEV_IFL_T_HEX(&ctrl->base.base, ctrl->base.log_level, __VA_ARGS__)
27#define WMR_DEBUG(ctrl, ...) U_LOG_XDEV_IFL_D(&ctrl->base.base, ctrl->base.log_level, __VA_ARGS__)
28#define WMR_DEBUG_HEX(ctrl, ...) U_LOG_XDEV_IFL_D_HEX(&ctrl->base.base, ctrl->base.log_level, __VA_ARGS__)
29#define WMR_INFO(ctrl, ...) U_LOG_XDEV_IFL_I(&ctrl->base.base, ctrl->base.log_level, __VA_ARGS__)
30#define WMR_WARN(ctrl, ...) U_LOG_XDEV_IFL_W(&ctrl->base.base, ctrl->base.log_level, __VA_ARGS__)
31#define WMR_ERROR(ctrl, ...) U_LOG_XDEV_IFL_E(&ctrl->base.base, ctrl->base.log_level, __VA_ARGS__)
32
33#ifdef XRT_DOXYGEN
34#define WMR_PACKED
35#else
36#define WMR_PACKED __attribute__((packed))
37#endif
38
39/*!
40 * Indices in input list of each input.
41 */
42enum wmr_controller_hp_input_index
43{
44 WMR_CONTROLLER_INDEX_MENU_CLICK,
45 WMR_CONTROLLER_INDEX_HOME_CLICK,
46 WMR_CONTROLLER_INDEX_SQUEEZE_CLICK,
47 WMR_CONTROLLER_INDEX_SQUEEZE_VALUE,
48 WMR_CONTROLLER_INDEX_TRIGGER_VALUE,
49 WMR_CONTROLLER_INDEX_THUMBSTICK_CLICK,
50 WMR_CONTROLLER_INDEX_THUMBSTICK,
51 WMR_CONTROLLER_INDEX_GRIP_POSE,
52 WMR_CONTROLLER_INDEX_AIM_POSE,
53 WMR_CONTROLLER_INDEX_X_A_CLICK,
54 WMR_CONTROLLER_INDEX_Y_B_CLICK,
55 /* keep as last: */
56 WMR_CONTROLLER_INDEX_COUNT
57};
58
59#define SET_INPUT(wcb, INDEX, NAME) \
60 (wcb->base.inputs[WMR_CONTROLLER_INDEX_##INDEX].name = XRT_INPUT_G2_CONTROLLER_##NAME)
61
62/*
63 *
64 * Bindings
65 *
66 */
67
68static struct xrt_binding_input_pair touch_inputs[19] = {
69 {XRT_INPUT_TOUCH_X_CLICK, XRT_INPUT_G2_CONTROLLER_X_CLICK},
70 {XRT_INPUT_TOUCH_X_TOUCH, XRT_INPUT_G2_CONTROLLER_X_CLICK},
71 {XRT_INPUT_TOUCH_Y_CLICK, XRT_INPUT_G2_CONTROLLER_Y_CLICK},
72 {XRT_INPUT_TOUCH_Y_TOUCH, XRT_INPUT_G2_CONTROLLER_Y_CLICK},
73 {XRT_INPUT_TOUCH_MENU_CLICK, XRT_INPUT_G2_CONTROLLER_MENU_CLICK},
74 {XRT_INPUT_TOUCH_MENU_CLICK, XRT_INPUT_G2_CONTROLLER_HOME_CLICK},
75 {XRT_INPUT_TOUCH_A_CLICK, XRT_INPUT_G2_CONTROLLER_A_CLICK},
76 {XRT_INPUT_TOUCH_A_TOUCH, XRT_INPUT_G2_CONTROLLER_A_CLICK},
77 {XRT_INPUT_TOUCH_B_CLICK, XRT_INPUT_G2_CONTROLLER_B_CLICK},
78 {XRT_INPUT_TOUCH_B_TOUCH, XRT_INPUT_G2_CONTROLLER_B_CLICK},
79 {XRT_INPUT_TOUCH_SYSTEM_CLICK, XRT_INPUT_G2_CONTROLLER_MENU_CLICK},
80 {XRT_INPUT_TOUCH_SYSTEM_CLICK, XRT_INPUT_G2_CONTROLLER_HOME_CLICK},
81 {XRT_INPUT_TOUCH_SQUEEZE_VALUE, XRT_INPUT_G2_CONTROLLER_SQUEEZE_VALUE},
82 {XRT_INPUT_TOUCH_TRIGGER_TOUCH, XRT_INPUT_G2_CONTROLLER_TRIGGER_VALUE},
83 {XRT_INPUT_TOUCH_TRIGGER_VALUE, XRT_INPUT_G2_CONTROLLER_TRIGGER_VALUE},
84 {XRT_INPUT_TOUCH_THUMBSTICK_CLICK, XRT_INPUT_G2_CONTROLLER_THUMBSTICK_CLICK},
85 {XRT_INPUT_TOUCH_THUMBSTICK, XRT_INPUT_G2_CONTROLLER_THUMBSTICK},
86 {XRT_INPUT_TOUCH_GRIP_POSE, XRT_INPUT_G2_CONTROLLER_GRIP_POSE},
87 {XRT_INPUT_TOUCH_AIM_POSE, XRT_INPUT_G2_CONTROLLER_AIM_POSE},
88};
89
90static struct xrt_binding_output_pair touch_outputs[1] = {
91 {XRT_OUTPUT_NAME_TOUCH_HAPTIC, XRT_OUTPUT_NAME_G2_CONTROLLER_HAPTIC},
92};
93
94static struct xrt_binding_input_pair simple_inputs[4] = {
95 {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_G2_CONTROLLER_TRIGGER_VALUE},
96 {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_G2_CONTROLLER_MENU_CLICK},
97 {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_G2_CONTROLLER_GRIP_POSE},
98 {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_G2_CONTROLLER_AIM_POSE},
99};
100
101static struct xrt_binding_output_pair simple_outputs[1] = {
102 {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_G2_CONTROLLER_HAPTIC},
103};
104
105static struct xrt_binding_profile binding_profiles[2] = {
106 {
107 .name = XRT_DEVICE_TOUCH_CONTROLLER,
108 .inputs = touch_inputs,
109 .input_count = ARRAY_SIZE(touch_inputs),
110 .outputs = touch_outputs,
111 .output_count = ARRAY_SIZE(touch_outputs),
112 },
113 {
114 .name = XRT_DEVICE_SIMPLE_CONTROLLER,
115 .inputs = simple_inputs,
116 .input_count = ARRAY_SIZE(simple_inputs),
117 .outputs = simple_outputs,
118 .output_count = ARRAY_SIZE(simple_outputs),
119 },
120};
121
122/* OG WMR Controller inputs struct */
123struct wmr_controller_hp_input
124{
125 // buttons clicked
126 bool menu;
127 bool home;
128 bool bt_pairing;
129 bool squeeze_click; // Squeeze click reported on full squeeze
130
131 // X/Y/A/B buttons
132 bool x_a;
133 bool y_b;
134
135 float trigger;
136 float squeeze;
137
138 struct
139 {
140 bool click;
141 struct xrt_vec2 values;
142 } thumbstick;
143
144 uint8_t battery;
145
146 struct
147 {
148 uint64_t timestamp_ticks;
149 struct xrt_vec3 acc;
150 struct xrt_vec3 gyro;
151 int32_t temperature;
152 } imu;
153};
154#undef WMR_PACKED
155
156/* HP WMR Controller device struct */
157struct wmr_controller_hp
158{
159 struct wmr_controller_base base;
160
161 //! The last decoded package of IMU and button data
162 struct wmr_controller_hp_input last_inputs;
163};
164
165/*
166 *
167 * WMR Motion Controller protocol helpers
168 *
169 */
170
171static inline void
172vec3_from_wmr_controller_accel(const int32_t sample[3], struct xrt_vec3 *out_vec)
173{
174 // Reverb G1 observation: 1g is approximately 490,000.
175 // @todo: Confirm the scale is correct
176
177 out_vec->x = (float)sample[0] / (98000 / 2);
178 out_vec->y = (float)sample[1] / (98000 / 2);
179 out_vec->z = (float)sample[2] / (98000 / 2);
180}
181
182
183static inline void
184vec3_from_wmr_controller_gyro(const int32_t sample[3], struct xrt_vec3 *out_vec)
185{
186 // @todo: Confirm the scale is correct
187 out_vec->x = (float)sample[0] * 0.00001f;
188 out_vec->y = (float)sample[1] * 0.00001f;
189 out_vec->z = (float)sample[2] * 0.00001f;
190}
191
192static bool
193wmr_controller_hp_packet_parse(struct wmr_controller_hp *ctrl, const unsigned char *buffer, size_t len)
194{
195 struct wmr_controller_hp_input *last_input = &ctrl->last_inputs;
196 struct wmr_controller_base *wcb = (struct wmr_controller_base *)(ctrl);
197
198 if (len != 44) {
199 U_LOG_IFL_E(wcb->log_level, "WMR Controller: unexpected message length: %zd", len);
200 return false;
201 }
202
203 const unsigned char *p = buffer;
204
205 // Read buttons
206 uint8_t buttons = read8(&p);
207 last_input->thumbstick.click = buttons & 0x01;
208 last_input->home = buttons & 0x02;
209 last_input->menu = buttons & 0x04;
210 last_input->squeeze_click = buttons & 0x08; // squeeze-click
211 last_input->bt_pairing = buttons & 0x20;
212
213 // Read thumbstick coordinates (12 bit resolution)
214 int16_t stick_x = read8(&p);
215 uint8_t nibbles = read8(&p);
216 stick_x += ((nibbles & 0x0F) << 8);
217 int16_t stick_y = (nibbles >> 4);
218 stick_y += (read8(&p) << 4);
219
220 last_input->thumbstick.values.x = (float)(stick_x - 0x07FF) / 0x07FF;
221 if (last_input->thumbstick.values.x > 1.0f) {
222 last_input->thumbstick.values.x = 1.0f;
223 }
224
225 last_input->thumbstick.values.y = (float)(stick_y - 0x07FF) / 0x07FF;
226 if (last_input->thumbstick.values.y > 1.0f) {
227 last_input->thumbstick.values.y = 1.0f;
228 }
229
230 // Read trigger value (0x00 - 0xFF)
231 last_input->trigger = (float)read8(&p) / 0xFF;
232
233 /* On OG these are touchpad values, but on HP it's
234 * squeeze value and A_X/B_Y click */
235 last_input->squeeze = (float)read8(&p) / 0xFF;
236
237 buttons = read8(&p);
238 last_input->x_a = buttons & 0x02;
239 last_input->y_b = buttons & 0x01;
240
241 last_input->battery = read8(&p);
242
243 int32_t acc[3];
244 acc[0] = read24(&p); // x
245 acc[1] = read24(&p); // y
246 acc[2] = read24(&p); // z
247 vec3_from_wmr_controller_accel(acc, &last_input->imu.acc);
248 math_matrix_3x3_transform_vec3(&wcb->config.sensors.accel.mix_matrix, &last_input->imu.acc,
249 &last_input->imu.acc);
250 math_vec3_accum(&wcb->config.sensors.accel.bias_offsets, &last_input->imu.acc);
251 math_quat_rotate_vec3(&wcb->config.sensors.transforms.P_oxr_acc.orientation, &last_input->imu.acc,
252 &last_input->imu.acc);
253
254 U_LOG_IFL_T(wcb->log_level, "Accel [m/s^2] : %f",
255 sqrtf(last_input->imu.acc.x * last_input->imu.acc.x +
256 last_input->imu.acc.y * last_input->imu.acc.y +
257 last_input->imu.acc.z * last_input->imu.acc.z));
258
259
260 last_input->imu.temperature = read16(&p);
261
262 int32_t gyro[3];
263 gyro[0] = read24(&p);
264 gyro[1] = read24(&p);
265 gyro[2] = read24(&p);
266 vec3_from_wmr_controller_gyro(gyro, &last_input->imu.gyro);
267 math_matrix_3x3_transform_vec3(&wcb->config.sensors.gyro.mix_matrix, &last_input->imu.gyro,
268 &last_input->imu.gyro);
269 math_vec3_accum(&wcb->config.sensors.gyro.bias_offsets, &last_input->imu.gyro);
270 math_quat_rotate_vec3(&wcb->config.sensors.transforms.P_oxr_gyr.orientation, &last_input->imu.gyro,
271 &last_input->imu.gyro);
272
273
274 uint32_t prev_ticks = last_input->imu.timestamp_ticks & UINT32_C(0xFFFFFFFF);
275
276 // Write the new ticks value into the lower half of timestamp_ticks
277 last_input->imu.timestamp_ticks &= (UINT64_C(0xFFFFFFFF) << 32u);
278 last_input->imu.timestamp_ticks += (uint32_t)read32(&p);
279
280 if ((last_input->imu.timestamp_ticks & UINT64_C(0xFFFFFFFF)) < prev_ticks) {
281 // Timer overflow, so increment the upper half of timestamp_ticks
282 last_input->imu.timestamp_ticks += (UINT64_C(0x1) << 32u);
283 }
284
285 /* Todo: More decoding here
286 read16(&p); // Unknown. Seems to depend on controller orientation (probably mag)
287 read32(&p); // Unknown.
288 read16(&p); // Unknown. Device state, etc.
289 read16(&p);
290 read16(&p);
291 */
292
293 return true;
294}
295
296static bool
297handle_input_packet(struct wmr_controller_base *wcb, uint64_t time_ns, uint8_t *buffer, uint32_t buf_size)
298{
299 struct wmr_controller_hp *ctrl = (struct wmr_controller_hp *)(wcb);
300
301 bool b = wmr_controller_hp_packet_parse(ctrl, buffer, buf_size);
302 if (b) {
303 m_imu_3dof_update(&wcb->fusion,
304 ctrl->last_inputs.imu.timestamp_ticks * WMR_MOTION_CONTROLLER_NS_PER_TICK,
305 &ctrl->last_inputs.imu.acc, &ctrl->last_inputs.imu.gyro);
306
307 wcb->last_imu_timestamp_ns = time_ns;
308 wcb->last_angular_velocity = ctrl->last_inputs.imu.gyro;
309 }
310
311 return b;
312}
313
314static xrt_result_t
315wmr_controller_hp_update_inputs(struct xrt_device *xdev)
316{
317 DRV_TRACE_MARKER();
318
319 struct wmr_controller_hp *ctrl = (struct wmr_controller_hp *)(xdev);
320 struct wmr_controller_base *wcb = (struct wmr_controller_base *)(xdev);
321
322 os_mutex_lock(&wcb->data_lock);
323
324 struct xrt_input *xrt_inputs = xdev->inputs;
325 struct wmr_controller_hp_input *cur_inputs = &ctrl->last_inputs;
326
327 xrt_inputs[WMR_CONTROLLER_INDEX_MENU_CLICK].value.boolean = cur_inputs->menu;
328 xrt_inputs[WMR_CONTROLLER_INDEX_HOME_CLICK].value.boolean = cur_inputs->home;
329 xrt_inputs[WMR_CONTROLLER_INDEX_X_A_CLICK].value.boolean = cur_inputs->x_a;
330 xrt_inputs[WMR_CONTROLLER_INDEX_Y_B_CLICK].value.boolean = cur_inputs->y_b;
331 xrt_inputs[WMR_CONTROLLER_INDEX_SQUEEZE_CLICK].value.boolean = cur_inputs->squeeze;
332 xrt_inputs[WMR_CONTROLLER_INDEX_SQUEEZE_VALUE].value.vec1.x = cur_inputs->squeeze;
333 xrt_inputs[WMR_CONTROLLER_INDEX_TRIGGER_VALUE].value.vec1.x = cur_inputs->trigger;
334 xrt_inputs[WMR_CONTROLLER_INDEX_THUMBSTICK_CLICK].value.boolean = cur_inputs->thumbstick.click;
335 xrt_inputs[WMR_CONTROLLER_INDEX_THUMBSTICK].value.vec2 = cur_inputs->thumbstick.values;
336
337 os_mutex_unlock(&wcb->data_lock);
338
339 return XRT_SUCCESS;
340}
341
342static void
343wmr_controller_hp_destroy(struct xrt_device *xdev)
344{
345 struct wmr_controller_base *wcb = (struct wmr_controller_base *)(xdev);
346
347 wmr_controller_base_deinit(wcb);
348 free(wcb);
349}
350
351struct wmr_controller_base *
352wmr_controller_hp_create(struct wmr_controller_connection *conn,
353 enum xrt_device_type controller_type,
354 enum u_logging_level log_level)
355{
356 DRV_TRACE_MARKER();
357
358 enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE;
359 struct wmr_controller_hp *ctrl =
360 U_DEVICE_ALLOCATE(struct wmr_controller_hp, flags, WMR_CONTROLLER_INDEX_COUNT, 1);
361 struct wmr_controller_base *wcb = (struct wmr_controller_base *)(ctrl);
362
363 if (!wmr_controller_base_init(wcb, conn, controller_type, log_level, wmr_controller_hp_destroy)) {
364 wmr_controller_hp_destroy(&wcb->base);
365 return NULL;
366 }
367
368 wcb->handle_input_packet = handle_input_packet;
369
370 // Only set those we want to overwrite.
371 wcb->base.update_inputs = wmr_controller_hp_update_inputs;
372 wcb->base.name = XRT_DEVICE_HP_REVERB_G2_CONTROLLER;
373
374 if (controller_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
375 snprintf(wcb->base.str, ARRAY_SIZE(wcb->base.str), "HP Reverb G2 Left Controller");
376 } else {
377 snprintf(wcb->base.str, ARRAY_SIZE(wcb->base.str), "HP Reverb G2 Right Controller");
378 }
379
380 SET_INPUT(wcb, MENU_CLICK, MENU_CLICK);
381 SET_INPUT(wcb, HOME_CLICK, HOME_CLICK);
382 SET_INPUT(wcb, SQUEEZE_CLICK, SQUEEZE_CLICK);
383 SET_INPUT(wcb, SQUEEZE_VALUE, SQUEEZE_VALUE);
384 SET_INPUT(wcb, TRIGGER_VALUE, TRIGGER_VALUE);
385 SET_INPUT(wcb, THUMBSTICK_CLICK, THUMBSTICK_CLICK);
386 SET_INPUT(wcb, THUMBSTICK, THUMBSTICK);
387 SET_INPUT(wcb, GRIP_POSE, GRIP_POSE);
388 SET_INPUT(wcb, AIM_POSE, AIM_POSE);
389 if (controller_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
390 SET_INPUT(wcb, X_A_CLICK, X_CLICK);
391 SET_INPUT(wcb, Y_B_CLICK, Y_CLICK);
392 } else {
393 SET_INPUT(wcb, X_A_CLICK, A_CLICK);
394 SET_INPUT(wcb, Y_B_CLICK, B_CLICK);
395 }
396
397 for (uint32_t i = 0; i < wcb->base.input_count; i++) {
398 wcb->base.inputs[0].active = true;
399 }
400
401 ctrl->last_inputs.imu.timestamp_ticks = 0;
402
403 wcb->base.outputs[0].name = XRT_OUTPUT_NAME_WMR_HAPTIC;
404
405 wcb->base.binding_profiles = binding_profiles;
406 wcb->base.binding_profile_count = ARRAY_SIZE(binding_profiles);
407
408 u_var_add_bool(wcb, &ctrl->last_inputs.menu, "input.menu");
409 u_var_add_bool(wcb, &ctrl->last_inputs.home, "input.home");
410 u_var_add_bool(wcb, &ctrl->last_inputs.bt_pairing, "input.bt_pairing");
411 u_var_add_bool(wcb, &ctrl->last_inputs.squeeze_click, "input.squeeze.click");
412 u_var_add_f32(wcb, &ctrl->last_inputs.squeeze, "input.squeeze.value");
413 u_var_add_f32(wcb, &ctrl->last_inputs.trigger, "input.trigger");
414 u_var_add_u8(wcb, &ctrl->last_inputs.battery, "input.battery");
415 u_var_add_bool(wcb, &ctrl->last_inputs.thumbstick.click, "input.thumbstick.click");
416 u_var_add_f32(wcb, &ctrl->last_inputs.thumbstick.values.x, "input.thumbstick.values.y");
417 u_var_add_f32(wcb, &ctrl->last_inputs.thumbstick.values.y, "input.thumbstick.values.x");
418 if (controller_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
419 u_var_add_bool(wcb, &ctrl->last_inputs.x_a, "input.x");
420 u_var_add_bool(wcb, &ctrl->last_inputs.y_b, "input.y");
421 } else {
422 u_var_add_bool(wcb, &ctrl->last_inputs.x_a, "input.a");
423 u_var_add_bool(wcb, &ctrl->last_inputs.y_b, "input.b");
424 }
425
426 u_var_add_ro_vec3_f32(wcb, &ctrl->last_inputs.imu.acc, "imu.acc");
427 u_var_add_ro_vec3_f32(wcb, &ctrl->last_inputs.imu.gyro, "imu.gyro");
428 u_var_add_i32(wcb, &ctrl->last_inputs.imu.temperature, "imu.temperature");
429
430 return wcb;
431}