d/wmr: Split out OG WMR controller handling · matrixfurry.com/monado@ea53d27

+4

src/xrt/drivers/CMakeLists.txt

··· 366 366 wmr/wmr_config.h 367 367 wmr/wmr_controller_base.c 368 368 wmr/wmr_controller_base.h 369 + wmr/wmr_controller_og.c 370 + wmr/wmr_controller_hp.c 371 + wmr/wmr_controller.c 372 + wmr/wmr_controller.h 369 373 wmr/wmr_bt_controller.c 370 374 wmr/wmr_bt_controller.h 371 375 wmr/wmr_hmd.c

+4 -1

src/xrt/drivers/wmr/wmr_bt_controller.c

··· 16 16 17 17 #include "wmr_common.h" 18 18 #include "wmr_bt_controller.h" 19 + #include "wmr_controller.h" 19 20 #include "wmr_config_key.h" 20 21 21 22 #include <stdio.h> ··· 148 149 struct xrt_device * 149 150 wmr_bt_controller_create(struct os_hid_device *controller_hid, 150 151 enum xrt_device_type controller_type, 152 + uint16_t vid, 153 + uint16_t pid, 151 154 enum u_logging_level log_level) 152 155 { 153 156 DRV_TRACE_MARKER(); ··· 179 182 } 180 183 181 184 // Takes ownership of the connection 182 - struct wmr_controller_base *wcb = wmr_controller_base_create(&conn->base, controller_type, log_level); 185 + struct wmr_controller_base *wcb = wmr_controller_create(&conn->base, controller_type, vid, pid, log_level); 183 186 if (wcb == NULL) { 184 187 WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to create controller"); 185 188 return NULL;

+2

src/xrt/drivers/wmr/wmr_bt_controller.h

··· 43 43 struct xrt_device * 44 44 wmr_bt_controller_create(struct os_hid_device *controller_hid, 45 45 enum xrt_device_type controller_type, 46 + uint16_t vid, 47 + uint16_t pid, 46 48 enum u_logging_level log_level); 47 49 48 50 #ifdef __cplusplus

+45

src/xrt/drivers/wmr/wmr_controller.c

··· 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 <assert.h> 12 + 13 + #include "wmr_common.h" 14 + #include "wmr_controller.h" 15 + 16 + struct wmr_controller_base * 17 + wmr_controller_og_create(struct wmr_controller_connection *conn, 18 + enum xrt_device_type controller_type, 19 + enum u_logging_level log_level); 20 + 21 + struct wmr_controller_base * 22 + wmr_controller_hp_create(struct wmr_controller_connection *conn, 23 + enum xrt_device_type controller_type, 24 + enum u_logging_level log_level); 25 + 26 + struct wmr_controller_base * 27 + wmr_controller_create(struct wmr_controller_connection *conn, 28 + enum xrt_device_type controller_type, 29 + uint16_t vid, 30 + uint16_t pid, 31 + enum u_logging_level log_level) 32 + { 33 + struct wmr_controller_base *ret = NULL; 34 + 35 + assert(vid == MICROSOFT_VID); /* The only known controllers all use Microsoft VID right now */ 36 + 37 + switch (pid) { 38 + case WMR_CONTROLLER_PID: 39 + case ODYSSEY_CONTROLLER_PID: ret = wmr_controller_og_create(conn, controller_type, log_level); break; 40 + case REVERB_G2_CONTROLLER_PID: ret = wmr_controller_hp_create(conn, controller_type, log_level); break; 41 + default: break; 42 + } 43 + 44 + return ret; 45 + }

+22

src/xrt/drivers/wmr/wmr_controller.h

··· 1 + // Copyright 2020-2021, N Madsen. 2 + // Copyright 2020-2021, Collabora, Ltd. 3 + // Copyright 2021-2023, Jan Schmidt 4 + // SPDX-License-Identifier: BSL-1.0 5 + // 6 + /*! 7 + * @file 8 + * @brief Implementation of Original & HP WMR controllers 9 + * @author Jan Schmidt <jan@centricular.com> 10 + * @author Nis Madsen <nima_zero_one@protonmail.com> 11 + * @ingroup drv_wmr 12 + */ 13 + #pragma once 14 + 15 + #include "wmr_controller_base.h" 16 + 17 + struct wmr_controller_base * 18 + wmr_controller_create(struct wmr_controller_connection *conn, 19 + enum xrt_device_type controller_type, 20 + uint16_t vid, 21 + uint16_t pid, 22 + enum u_logging_level log_level);

+50 -153

src/xrt/drivers/wmr/wmr_controller_base.c

··· 35 35 #include <errno.h> 36 36 37 37 #define WMR_TRACE(wcb, ...) U_LOG_XDEV_IFL_T(&wcb->base, wcb->log_level, __VA_ARGS__) 38 + #define WMR_TRACE_HEX(wcb, ...) U_LOG_XDEV_IFL_T_HEX(&wcb->base, wcb->log_level, __VA_ARGS__) 38 39 #define WMR_DEBUG(wcb, ...) U_LOG_XDEV_IFL_D(&wcb->base, wcb->log_level, __VA_ARGS__) 40 + #define WMR_DEBUG_HEX(wcb, ...) U_LOG_XDEV_IFL_D_HEX(&wcb->base, wcb->log_level, __VA_ARGS__) 39 41 #define WMR_INFO(wcb, ...) U_LOG_XDEV_IFL_I(&wcb->base, wcb->log_level, __VA_ARGS__) 40 42 #define WMR_WARN(wcb, ...) U_LOG_XDEV_IFL_W(&wcb->base, wcb->log_level, __VA_ARGS__) 41 43 #define WMR_ERROR(wcb, ...) U_LOG_XDEV_IFL_E(&wcb->base, wcb->log_level, __VA_ARGS__) 42 44 43 - /*! 44 - * Indices in input list of each input. 45 - */ 46 - enum wmr_bt_input_index 47 - { 48 - WMR_INDEX_MENU_CLICK, 49 - WMR_INDEX_SQUEEZE_CLICK, 50 - WMR_INDEX_TRIGGER_VALUE, 51 - WMR_INDEX_THUMBSTICK_CLICK, 52 - WMR_INDEX_THUMBSTICK, 53 - WMR_INDEX_TRACKPAD_CLICK, 54 - WMR_INDEX_TRACKPAD_TOUCH, 55 - WMR_INDEX_TRACKPAD, 56 - WMR_INDEX_GRIP_POSE, 57 - WMR_INDEX_AIM_POSE, 58 - }; 45 + #define wmr_controller_hexdump_buffer(wcb, label, buf, length) \ 46 + do { \ 47 + WMR_DEBUG(wcb, "%s", label); \ 48 + WMR_DEBUG_HEX(wcb, buf, length); \ 49 + } while (0); 59 50 60 - #define SET_INPUT(NAME) (wcb->base.inputs[WMR_INDEX_##NAME].name = XRT_INPUT_WMR_##NAME) 61 51 62 52 //! file path to store controller JSON configuration blocks that 63 53 //! read from the firmware. ··· 81 71 case WMR_MOTION_CONTROLLER_STATUS_MSG: 82 72 os_mutex_lock(&wcb->data_lock); 83 73 // Note: skipping msg type byte 84 - bool b = wmr_controller_packet_parse(&buffer[1], (size_t)buf_size - 1, &wcb->input, wcb->log_level); 85 - if (b) { 86 - m_imu_3dof_update(&wcb->fusion, 87 - wcb->input.imu.timestamp_ticks * WMR_MOTION_CONTROLLER_NS_PER_TICK, 88 - &wcb->input.imu.acc, &wcb->input.imu.gyro); 74 + bool b = wcb->handle_input_packet(wcb, time_ns, &buffer[1], (size_t)buf_size - 1); 75 + os_mutex_unlock(&wcb->data_lock); 89 76 90 - wcb->last_imu_timestamp_ns = time_ns; 91 - wcb->last_angular_velocity = wcb->input.imu.gyro; 92 - 93 - } else { 94 - WMR_ERROR(wcb, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i", 95 - buffer[0], buf_size); 96 - os_mutex_unlock(&wcb->data_lock); 77 + if (!b) { 78 + WMR_ERROR(wcb, "WMR Controller: Failed handling message type: %02x, size: %i", buffer[0], 79 + buf_size); 80 + wmr_controller_hexdump_buffer(wcb, "Controller Message", buffer, buf_size); 97 81 return; 98 82 } 99 - os_mutex_unlock(&wcb->data_lock); 83 + 100 84 break; 101 85 default: 102 86 WMR_DEBUG(wcb, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], buf_size); ··· 242 226 } 243 227 244 228 WMR_DEBUG(d, "Read %d-byte FW data block %d", data_size, blk_id); 229 + wmr_controller_hexdump_buffer(d, "Data block", data, data_size); 245 230 246 231 *out_data = data; 247 232 *out_size = data_size; ··· 333 318 } 334 319 335 320 static void 336 - wmr_controller_base_set_output(struct xrt_device *xdev, enum xrt_output_name name, const union xrt_output_value *value) 337 - { 338 - DRV_TRACE_MARKER(); 339 - 340 - // struct wmr_controller_base *d = wmr_controller_base(xdev); 341 - // Todo: implement 342 - } 343 - 344 - static void 345 321 wmr_controller_base_get_tracked_pose(struct xrt_device *xdev, 346 322 enum xrt_input_name name, 347 323 uint64_t at_timestamp_ns, ··· 386 362 m_predict_relation(&relation, prediction_s, out_relation); 387 363 } 388 364 389 - 390 - 391 - static void 392 - wmr_controller_base_update_inputs(struct xrt_device *xdev) 365 + void 366 + wmr_controller_base_deinit(struct wmr_controller_base *wcb) 393 367 { 394 368 DRV_TRACE_MARKER(); 395 369 396 - struct wmr_controller_base *wcb = wmr_controller_base(xdev); 397 - 398 - struct xrt_input *inputs = wcb->base.inputs; 399 - 400 - os_mutex_lock(&wcb->data_lock); 401 - 402 - inputs[WMR_INDEX_MENU_CLICK].value.boolean = wcb->input.menu; 403 - inputs[WMR_INDEX_SQUEEZE_CLICK].value.boolean = wcb->input.squeeze; 404 - inputs[WMR_INDEX_TRIGGER_VALUE].value.vec1.x = wcb->input.trigger; 405 - inputs[WMR_INDEX_THUMBSTICK_CLICK].value.boolean = wcb->input.thumbstick.click; 406 - inputs[WMR_INDEX_THUMBSTICK].value.vec2 = wcb->input.thumbstick.values; 407 - inputs[WMR_INDEX_TRACKPAD_CLICK].value.boolean = wcb->input.trackpad.click; 408 - inputs[WMR_INDEX_TRACKPAD_TOUCH].value.boolean = wcb->input.trackpad.touch; 409 - inputs[WMR_INDEX_TRACKPAD].value.vec2 = wcb->input.trackpad.values; 410 - 411 - os_mutex_unlock(&wcb->data_lock); 412 - } 413 - 414 - static void 415 - wmr_controller_base_destroy(struct xrt_device *xdev) 416 - { 417 - DRV_TRACE_MARKER(); 418 - 419 - struct wmr_controller_base *wcb = wmr_controller_base(xdev); 420 - 421 370 // Remove the variable tracking. 422 371 u_var_remove_root(wcb); 423 372 ··· 437 386 438 387 // Destroy the fusion. 439 388 m_imu_3dof_close(&wcb->fusion); 440 - 441 - free(wcb); 442 389 } 443 - 444 - 445 - /* 446 - * 447 - * Bindings 448 - * 449 - */ 450 - 451 - static struct xrt_binding_input_pair simple_inputs[4] = { 452 - {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE}, 453 - {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_WMR_MENU_CLICK}, 454 - {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_WMR_GRIP_POSE}, 455 - {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_WMR_AIM_POSE}, 456 - }; 457 - 458 - static struct xrt_binding_output_pair simple_outputs[1] = { 459 - {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_WMR_HAPTIC}, 460 - }; 461 - 462 - static struct xrt_binding_profile binding_profiles[1] = { 463 - { 464 - .name = XRT_DEVICE_SIMPLE_CONTROLLER, 465 - .inputs = simple_inputs, 466 - .input_count = ARRAY_SIZE(simple_inputs), 467 - .outputs = simple_outputs, 468 - .output_count = ARRAY_SIZE(simple_outputs), 469 - }, 470 - }; 471 - 472 390 473 391 /* 474 392 * ··· 476 394 * 477 395 */ 478 396 479 - struct wmr_controller_base * 480 - wmr_controller_base_create(struct wmr_controller_connection *conn, 481 - enum xrt_device_type controller_type, 482 - enum u_logging_level log_level) 397 + bool 398 + wmr_controller_base_init(struct wmr_controller_base *wcb, 399 + struct wmr_controller_connection *conn, 400 + enum xrt_device_type controller_type, 401 + enum u_logging_level log_level) 483 402 { 484 403 DRV_TRACE_MARKER(); 485 - 486 - enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE; 487 - struct wmr_controller_base *wcb = U_DEVICE_ALLOCATE(struct wmr_controller_base, flags, 10, 1); 488 404 489 405 wcb->log_level = log_level; 490 406 wcb->wcc = conn; ··· 496 412 snprintf(wcb->base.str, ARRAY_SIZE(wcb->base.str), "WMR Right Controller"); 497 413 } 498 414 499 - wcb->base.destroy = wmr_controller_base_destroy; 500 415 wcb->base.get_tracked_pose = wmr_controller_base_get_tracked_pose; 501 - wcb->base.set_output = wmr_controller_base_set_output; 502 - wcb->base.update_inputs = wmr_controller_base_update_inputs; 503 - 504 - SET_INPUT(MENU_CLICK); 505 - SET_INPUT(SQUEEZE_CLICK); 506 - SET_INPUT(TRIGGER_VALUE); 507 - SET_INPUT(THUMBSTICK_CLICK); 508 - SET_INPUT(THUMBSTICK); 509 - SET_INPUT(TRACKPAD_CLICK); 510 - SET_INPUT(TRACKPAD_TOUCH); 511 - SET_INPUT(TRACKPAD); 512 - SET_INPUT(GRIP_POSE); 513 - SET_INPUT(AIM_POSE); 514 - 515 - for (uint32_t i = 0; i < wcb->base.input_count; i++) { 516 - wcb->base.inputs[0].active = true; 517 - } 518 - 519 - wcb->base.outputs[0].name = XRT_OUTPUT_NAME_WMR_HAPTIC; 520 - 521 - wcb->base.binding_profiles = binding_profiles; 522 - wcb->base.binding_profile_count = ARRAY_SIZE(binding_profiles); 523 416 524 417 wcb->base.name = XRT_DEVICE_WMR_CONTROLLER; 525 418 wcb->base.device_type = controller_type; ··· 527 420 wcb->base.position_tracking_supported = false; 528 421 wcb->base.hand_tracking_supported = true; 529 422 530 - 531 - wcb->input.imu.timestamp_ticks = 0; 532 423 m_imu_3dof_init(&wcb->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); 533 424 534 425 if (os_mutex_init(&wcb->conn_lock) != 0 || os_mutex_init(&wcb->data_lock) != 0) { 535 426 WMR_ERROR(wcb, "WMR Controller: Failed to init mutex!"); 536 - wmr_controller_base_destroy(&wcb->base); 537 - return NULL; 427 + return false; 428 + } 429 + 430 + u_var_add_root(wcb, wcb->base.str, true); 431 + 432 + /* Send init commands */ 433 + struct wmr_controller_fw_cmd fw_cmd = { 434 + 0, 435 + }; 436 + struct wmr_controller_fw_cmd_response fw_cmd_response; 437 + 438 + /* Zero command. Clears controller state? */ 439 + fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x0, 0, 0); 440 + if (wmr_controller_send_fw_cmd(wcb, &fw_cmd, 0x06, &fw_cmd_response) < 0) { 441 + return false; 442 + } 443 + 444 + /* Unknown what this one does. No obvious effect */ 445 + fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x04, 0xc1, 0x02); 446 + if (wmr_controller_send_fw_cmd(wcb, &fw_cmd, 0x06, &fw_cmd_response) < 0) { 447 + return false; 538 448 } 539 449 540 450 // Read config file from controller 541 451 if (!read_controller_config(wcb)) { 542 - wmr_controller_base_destroy(&wcb->base); 543 - return NULL; 452 + return false; 544 453 } 545 454 546 - u_var_add_root(wcb, wcb->base.str, true); 547 - u_var_add_bool(wcb, &wcb->input.menu, "input.menu"); 548 - u_var_add_bool(wcb, &wcb->input.home, "input.home"); 549 - u_var_add_bool(wcb, &wcb->input.bt_pairing, "input.bt_pairing"); 550 - u_var_add_bool(wcb, &wcb->input.squeeze, "input.squeeze"); 551 - u_var_add_f32(wcb, &wcb->input.trigger, "input.trigger"); 552 - u_var_add_u8(wcb, &wcb->input.battery, "input.battery"); 553 - u_var_add_bool(wcb, &wcb->input.thumbstick.click, "input.thumbstick.click"); 554 - u_var_add_f32(wcb, &wcb->input.thumbstick.values.x, "input.thumbstick.values.y"); 555 - u_var_add_f32(wcb, &wcb->input.thumbstick.values.y, "input.thumbstick.values.x"); 556 - u_var_add_bool(wcb, &wcb->input.trackpad.click, "input.trackpad.click"); 557 - u_var_add_bool(wcb, &wcb->input.trackpad.touch, "input.trackpad.touch"); 558 - u_var_add_f32(wcb, &wcb->input.trackpad.values.x, "input.trackpad.values.x"); 559 - u_var_add_f32(wcb, &wcb->input.trackpad.values.y, "input.trackpad.values.y"); 560 - u_var_add_ro_vec3_f32(wcb, &wcb->input.imu.acc, "imu.acc"); 561 - u_var_add_ro_vec3_f32(wcb, &wcb->input.imu.gyro, "imu.gyro"); 562 - u_var_add_i32(wcb, &wcb->input.imu.temperature, "imu.temperature"); 455 + /* Enable the status reports, IMU and touchpad */ 456 + const unsigned char wmr_controller_status_enable_cmd[64] = {0x06, 0x03, 0x01, 0x00, 0x02}; 457 + wmr_controller_send_bytes(wcb, wmr_controller_status_enable_cmd, sizeof(wmr_controller_status_enable_cmd)); 458 + const unsigned char wmr_controller_imu_on_cmd[64] = {0x06, 0x03, 0x02, 0xe1, 0x02}; 459 + wmr_controller_send_bytes(wcb, wmr_controller_imu_on_cmd, sizeof(wmr_controller_imu_on_cmd)); 563 460 564 - return wcb; 461 + return true; 565 462 }

+14 -6

src/xrt/drivers/wmr/wmr_controller_base.h

··· 101 101 //! Mutex protects shared data used from OpenXR callbacks 102 102 struct os_mutex data_lock; 103 103 104 + //! Callback to parse a controller update packet and update the input / imu info. Called with the 105 + // data lock held. 106 + bool (*handle_input_packet)(struct wmr_controller_base *wcb, 107 + uint64_t time_ns, 108 + uint8_t *buffer, 109 + uint32_t buf_size); 110 + 104 111 /* firmware configuration block */ 105 112 struct wmr_controller_config config; 106 113 107 - //! The last decoded package of IMU and button data 108 - struct wmr_controller_input input; 109 114 //! Time of last IMU sample, in CPU time. 110 115 uint64_t last_imu_timestamp_ns; 111 116 //! Main fusion calculator. ··· 114 119 struct xrt_vec3 last_angular_velocity; 115 120 }; 116 121 117 - struct wmr_controller_base * 118 - wmr_controller_base_create(struct wmr_controller_connection *conn, 119 - enum xrt_device_type controller_type, 120 - enum u_logging_level log_level); 122 + bool 123 + wmr_controller_base_init(struct wmr_controller_base *wcb, 124 + struct wmr_controller_connection *conn, 125 + enum xrt_device_type controller_type, 126 + enum u_logging_level log_level); 121 127 128 + void 129 + wmr_controller_base_deinit(struct wmr_controller_base *wcb); 122 130 123 131 static inline void 124 132 wmr_controller_connection_receive_bytes(struct wmr_controller_connection *wcc,

+20

src/xrt/drivers/wmr/wmr_controller_hp.c

··· 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 + 12 + #include "wmr_controller.h" 13 + 14 + struct wmr_controller_base * 15 + wmr_controller_hp_create(struct wmr_controller_connection *conn, 16 + enum xrt_device_type controller_type, 17 + enum u_logging_level log_level) 18 + { 19 + return NULL; 20 + }

+361

src/xrt/drivers/wmr/wmr_controller_og.c

··· 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 "util/u_device.h" 12 + #include "util/u_trace_marker.h" 13 + #include "util/u_var.h" 14 + 15 + #include <stdio.h> 16 + #include <stdlib.h> 17 + #include <string.h> 18 + #include <assert.h> 19 + #include <errno.h> 20 + 21 + #include "wmr_controller.h" 22 + 23 + #ifdef XRT_DOXYGEN 24 + #define WMR_PACKED 25 + #else 26 + #define WMR_PACKED __attribute__((packed)) 27 + #endif 28 + 29 + /*! 30 + * Indices in input list of each input. 31 + */ 32 + enum wmr_controller_og_input_index 33 + { 34 + WMR_CONTROLLER_INDEX_MENU_CLICK, 35 + WMR_CONTROLLER_INDEX_SQUEEZE_CLICK, 36 + WMR_CONTROLLER_INDEX_TRIGGER_VALUE, 37 + WMR_CONTROLLER_INDEX_THUMBSTICK_CLICK, 38 + WMR_CONTROLLER_INDEX_THUMBSTICK, 39 + WMR_CONTROLLER_INDEX_TRACKPAD_CLICK, 40 + WMR_CONTROLLER_INDEX_TRACKPAD_TOUCH, 41 + WMR_CONTROLLER_INDEX_TRACKPAD, 42 + WMR_CONTROLLER_INDEX_GRIP_POSE, 43 + WMR_CONTROLLER_INDEX_AIM_POSE, 44 + }; 45 + 46 + #define SET_INPUT(wcb, NAME) (wcb->base.inputs[WMR_CONTROLLER_INDEX_##NAME].name = XRT_INPUT_WMR_##NAME) 47 + 48 + /* 49 + * 50 + * Bindings 51 + * 52 + */ 53 + 54 + static struct xrt_binding_input_pair simple_inputs[4] = { 55 + {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE}, 56 + {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_WMR_MENU_CLICK}, 57 + {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_WMR_GRIP_POSE}, 58 + {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_WMR_AIM_POSE}, 59 + }; 60 + 61 + static struct xrt_binding_output_pair simple_outputs[1] = { 62 + {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_WMR_HAPTIC}, 63 + }; 64 + 65 + static struct xrt_binding_profile binding_profiles[1] = { 66 + { 67 + .name = XRT_DEVICE_SIMPLE_CONTROLLER, 68 + .inputs = simple_inputs, 69 + .input_count = ARRAY_SIZE(simple_inputs), 70 + .outputs = simple_outputs, 71 + .output_count = ARRAY_SIZE(simple_outputs), 72 + }, 73 + }; 74 + 75 + /* OG WMR Controller inputs struct */ 76 + struct wmr_controller_og_input 77 + { 78 + // buttons clicked 79 + bool menu; 80 + bool home; 81 + bool bt_pairing; 82 + bool squeeze; // Actually a "squeeze" click 83 + 84 + float trigger; 85 + 86 + struct 87 + { 88 + bool click; 89 + struct xrt_vec2 values; 90 + } thumbstick; 91 + struct 92 + { 93 + bool click; 94 + bool touch; 95 + struct xrt_vec2 values; 96 + } trackpad; 97 + 98 + uint8_t battery; 99 + 100 + struct 101 + { 102 + uint64_t timestamp_ticks; 103 + struct xrt_vec3 acc; 104 + struct xrt_vec3 gyro; 105 + int32_t temperature; 106 + } imu; 107 + }; 108 + #undef WMR_PACKED 109 + 110 + /* OG WMR Controller device struct */ 111 + struct wmr_controller_og 112 + { 113 + struct wmr_controller_base base; 114 + 115 + //! The last decoded package of IMU and button data 116 + struct wmr_controller_og_input last_inputs; 117 + }; 118 + 119 + /* 120 + * 121 + * WMR Motion Controller protocol helpers 122 + * 123 + */ 124 + 125 + static inline void 126 + vec3_from_wmr_controller_accel(const int32_t sample[3], struct xrt_vec3 *out_vec) 127 + { 128 + // Reverb G1 observation: 1g is approximately 490,000. 129 + 130 + out_vec->x = (float)sample[0] / (98000 / 2); 131 + out_vec->y = (float)sample[1] / (98000 / 2); 132 + out_vec->z = (float)sample[2] / (98000 / 2); 133 + } 134 + 135 + 136 + static inline void 137 + vec3_from_wmr_controller_gyro(const int32_t sample[3], struct xrt_vec3 *out_vec) 138 + { 139 + out_vec->x = (float)sample[0] * 0.00001f; 140 + out_vec->y = (float)sample[1] * 0.00001f; 141 + out_vec->z = (float)sample[2] * 0.00001f; 142 + } 143 + 144 + static bool 145 + wmr_controller_og_packet_parse(struct wmr_controller_og *ctrl, const unsigned char *buffer, size_t len) 146 + { 147 + struct wmr_controller_og_input *last_input = &ctrl->last_inputs; 148 + struct wmr_controller_base *wcb = (struct wmr_controller_base *)(ctrl); 149 + 150 + if (len != 44) { 151 + U_LOG_IFL_E(wcb->log_level, "WMR Controller: unexpected message length: %zd", len); 152 + return false; 153 + } 154 + 155 + const unsigned char *p = buffer; 156 + 157 + // Read buttons 158 + uint8_t buttons = read8(&p); 159 + last_input->thumbstick.click = buttons & 0x01; 160 + last_input->home = buttons & 0x02; 161 + last_input->menu = buttons & 0x04; 162 + last_input->squeeze = buttons & 0x08; // squeeze-click 163 + last_input->trackpad.click = buttons & 0x10; 164 + last_input->bt_pairing = buttons & 0x20; 165 + last_input->trackpad.touch = buttons & 0x40; 166 + 167 + 168 + // Read thumbstick coordinates (12 bit resolution) 169 + int16_t stick_x = read8(&p); 170 + uint8_t nibbles = read8(&p); 171 + stick_x += ((nibbles & 0x0F) << 8); 172 + int16_t stick_y = (nibbles >> 4); 173 + stick_y += (read8(&p) << 4); 174 + 175 + last_input->thumbstick.values.x = (float)(stick_x - 0x07FF) / 0x07FF; 176 + if (last_input->thumbstick.values.x > 1.0f) { 177 + last_input->thumbstick.values.x = 1.0f; 178 + } 179 + 180 + last_input->thumbstick.values.y = (float)(stick_y - 0x07FF) / 0x07FF; 181 + if (last_input->thumbstick.values.y > 1.0f) { 182 + last_input->thumbstick.values.y = 1.0f; 183 + } 184 + 185 + // Read trigger value (0x00 - 0xFF) 186 + last_input->trigger = (float)read8(&p) / 0xFF; 187 + 188 + // Read trackpad coordinates (0x00 - 0x64. Both are 0xFF when untouched) 189 + uint8_t trackpad_x = read8(&p); 190 + uint8_t trackpad_y = read8(&p); 191 + last_input->trackpad.values.x = (trackpad_x == 0xFF) ? 0.0f : (float)(trackpad_x - 0x32) / 0x32; 192 + last_input->trackpad.values.y = (trackpad_y == 0xFF) ? 0.0f : (float)(trackpad_y - 0x32) / 0x32; 193 + 194 + last_input->battery = read8(&p); 195 + 196 + int32_t acc[3]; 197 + acc[0] = read24(&p); // x 198 + acc[1] = read24(&p); // y 199 + acc[2] = read24(&p); // z 200 + vec3_from_wmr_controller_accel(acc, &last_input->imu.acc); 201 + 202 + U_LOG_IFL_T(wcb->log_level, "Accel [m/s^2] : %f", 203 + sqrtf(last_input->imu.acc.x * last_input->imu.acc.x + 204 + last_input->imu.acc.y * last_input->imu.acc.y + 205 + last_input->imu.acc.z * last_input->imu.acc.z)); 206 + 207 + 208 + last_input->imu.temperature = read16(&p); 209 + 210 + int32_t gyro[3]; 211 + gyro[0] = read24(&p); 212 + gyro[1] = read24(&p); 213 + gyro[2] = read24(&p); 214 + vec3_from_wmr_controller_gyro(gyro, &last_input->imu.gyro); 215 + 216 + 217 + uint32_t prev_ticks = last_input->imu.timestamp_ticks & UINT32_C(0xFFFFFFFF); 218 + 219 + // Write the new ticks value into the lower half of timestamp_ticks 220 + last_input->imu.timestamp_ticks &= (UINT64_C(0xFFFFFFFF) << 32u); 221 + last_input->imu.timestamp_ticks += (uint32_t)read32(&p); 222 + 223 + if ((last_input->imu.timestamp_ticks & UINT64_C(0xFFFFFFFF)) < prev_ticks) { 224 + // Timer overflow, so increment the upper half of timestamp_ticks 225 + last_input->imu.timestamp_ticks += (UINT64_C(0x1) << 32u); 226 + } 227 + 228 + /* Todo: More decoding here 229 + read16(&p); // Unknown. Seems to depend on controller orientation (probably mag) 230 + read32(&p); // Unknown. 231 + read16(&p); // Unknown. Device state, etc. 232 + read16(&p); 233 + read16(&p); 234 + */ 235 + 236 + return true; 237 + } 238 + 239 + static bool 240 + handle_input_packet(struct wmr_controller_base *wcb, uint64_t time_ns, uint8_t *buffer, uint32_t buf_size) 241 + { 242 + struct wmr_controller_og *ctrl = (struct wmr_controller_og *)(wcb); 243 + bool b = wmr_controller_og_packet_parse(ctrl, buffer, buf_size); 244 + if (b) { 245 + m_imu_3dof_update(&wcb->fusion, 246 + ctrl->last_inputs.imu.timestamp_ticks * WMR_MOTION_CONTROLLER_NS_PER_TICK, 247 + &ctrl->last_inputs.imu.acc, &ctrl->last_inputs.imu.gyro); 248 + 249 + wcb->last_imu_timestamp_ns = time_ns; 250 + wcb->last_angular_velocity = ctrl->last_inputs.imu.gyro; 251 + } 252 + 253 + return b; 254 + } 255 + 256 + static void 257 + wmr_controller_og_update_xrt_inputs(struct xrt_device *xdev) 258 + { 259 + DRV_TRACE_MARKER(); 260 + 261 + struct wmr_controller_og *ctrl = (struct wmr_controller_og *)(xdev); 262 + struct wmr_controller_base *wcb = (struct wmr_controller_base *)(xdev); 263 + 264 + os_mutex_lock(&wcb->data_lock); 265 + 266 + struct xrt_input *xrt_inputs = xdev->inputs; 267 + struct wmr_controller_og_input *cur_inputs = &ctrl->last_inputs; 268 + 269 + xrt_inputs[WMR_CONTROLLER_INDEX_MENU_CLICK].value.boolean = cur_inputs->menu; 270 + xrt_inputs[WMR_CONTROLLER_INDEX_SQUEEZE_CLICK].value.boolean = cur_inputs->squeeze; 271 + xrt_inputs[WMR_CONTROLLER_INDEX_TRIGGER_VALUE].value.vec1.x = cur_inputs->trigger; 272 + xrt_inputs[WMR_CONTROLLER_INDEX_THUMBSTICK_CLICK].value.boolean = cur_inputs->thumbstick.click; 273 + xrt_inputs[WMR_CONTROLLER_INDEX_THUMBSTICK].value.vec2 = cur_inputs->thumbstick.values; 274 + xrt_inputs[WMR_CONTROLLER_INDEX_TRACKPAD_CLICK].value.boolean = cur_inputs->trackpad.click; 275 + xrt_inputs[WMR_CONTROLLER_INDEX_TRACKPAD_TOUCH].value.boolean = cur_inputs->trackpad.touch; 276 + xrt_inputs[WMR_CONTROLLER_INDEX_TRACKPAD].value.vec2 = cur_inputs->trackpad.values; 277 + 278 + os_mutex_unlock(&wcb->data_lock); 279 + } 280 + 281 + static void 282 + wmr_controller_og_set_output(struct xrt_device *xdev, enum xrt_output_name name, const union xrt_output_value *value) 283 + { 284 + DRV_TRACE_MARKER(); 285 + 286 + // struct wmr_controller_base *d = wmr_controller_base(xdev); 287 + // Todo: implement 288 + } 289 + 290 + static void 291 + wmr_controller_og_destroy(struct xrt_device *xdev) 292 + { 293 + struct wmr_controller_base *wcb = (struct wmr_controller_base *)(xdev); 294 + 295 + wmr_controller_base_deinit(wcb); 296 + free(wcb); 297 + } 298 + 299 + struct wmr_controller_base * 300 + wmr_controller_og_create(struct wmr_controller_connection *conn, 301 + enum xrt_device_type controller_type, 302 + enum u_logging_level log_level) 303 + { 304 + DRV_TRACE_MARKER(); 305 + 306 + enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE; 307 + struct wmr_controller_og *ctrl = U_DEVICE_ALLOCATE(struct wmr_controller_og, flags, 10, 1); 308 + struct wmr_controller_base *wcb = (struct wmr_controller_base *)(ctrl); 309 + 310 + if (!wmr_controller_base_init(wcb, conn, controller_type, log_level)) { 311 + wmr_controller_og_destroy(&wcb->base); 312 + return NULL; 313 + } 314 + 315 + wcb->handle_input_packet = handle_input_packet; 316 + 317 + wcb->base.destroy = wmr_controller_og_destroy; 318 + wcb->base.update_inputs = wmr_controller_og_update_xrt_inputs; 319 + wcb->base.set_output = wmr_controller_og_set_output; 320 + 321 + SET_INPUT(wcb, MENU_CLICK); 322 + SET_INPUT(wcb, SQUEEZE_CLICK); 323 + SET_INPUT(wcb, TRIGGER_VALUE); 324 + SET_INPUT(wcb, THUMBSTICK_CLICK); 325 + SET_INPUT(wcb, THUMBSTICK); 326 + SET_INPUT(wcb, TRACKPAD_CLICK); 327 + SET_INPUT(wcb, TRACKPAD_TOUCH); 328 + SET_INPUT(wcb, TRACKPAD); 329 + SET_INPUT(wcb, GRIP_POSE); 330 + SET_INPUT(wcb, AIM_POSE); 331 + 332 + for (uint32_t i = 0; i < wcb->base.input_count; i++) { 333 + wcb->base.inputs[0].active = true; 334 + } 335 + 336 + ctrl->last_inputs.imu.timestamp_ticks = 0; 337 + 338 + wcb->base.outputs[0].name = XRT_OUTPUT_NAME_WMR_HAPTIC; 339 + 340 + wcb->base.binding_profiles = binding_profiles; 341 + wcb->base.binding_profile_count = ARRAY_SIZE(binding_profiles); 342 + 343 + u_var_add_bool(wcb, &ctrl->last_inputs.menu, "input.menu"); 344 + u_var_add_bool(wcb, &ctrl->last_inputs.home, "input.home"); 345 + u_var_add_bool(wcb, &ctrl->last_inputs.bt_pairing, "input.bt_pairing"); 346 + u_var_add_bool(wcb, &ctrl->last_inputs.squeeze, "input.squeeze"); 347 + u_var_add_f32(wcb, &ctrl->last_inputs.trigger, "input.trigger"); 348 + u_var_add_u8(wcb, &ctrl->last_inputs.battery, "input.battery"); 349 + u_var_add_bool(wcb, &ctrl->last_inputs.thumbstick.click, "input.thumbstick.click"); 350 + u_var_add_f32(wcb, &ctrl->last_inputs.thumbstick.values.x, "input.thumbstick.values.y"); 351 + u_var_add_f32(wcb, &ctrl->last_inputs.thumbstick.values.y, "input.thumbstick.values.x"); 352 + u_var_add_bool(wcb, &ctrl->last_inputs.trackpad.click, "input.trackpad.click"); 353 + u_var_add_bool(wcb, &ctrl->last_inputs.trackpad.touch, "input.trackpad.touch"); 354 + u_var_add_f32(wcb, &ctrl->last_inputs.trackpad.values.x, "input.trackpad.values.x"); 355 + u_var_add_f32(wcb, &ctrl->last_inputs.trackpad.values.y, "input.trackpad.values.y"); 356 + u_var_add_ro_vec3_f32(wcb, &ctrl->last_inputs.imu.acc, "imu.acc"); 357 + u_var_add_ro_vec3_f32(wcb, &ctrl->last_inputs.imu.gyro, "imu.gyro"); 358 + u_var_add_i32(wcb, &ctrl->last_inputs.imu.temperature, "imu.temperature"); 359 + 360 + return wcb; 361 + }

-118

src/xrt/drivers/wmr/wmr_controller_protocol.c

··· 19 19 * WMR Motion Controller protocol helpers 20 20 * 21 21 */ 22 - 23 - static inline void 24 - vec3_from_wmr_controller_accel(const int32_t sample[3], struct xrt_vec3 *out_vec) 25 - { 26 - // Reverb G1 observation: 1g is approximately 490,000. 27 - 28 - out_vec->x = (float)sample[0] / (98000 / 2); 29 - out_vec->y = (float)sample[1] / (98000 / 2); 30 - out_vec->z = (float)sample[2] / (98000 / 2); 31 - } 32 - 33 - 34 - static inline void 35 - vec3_from_wmr_controller_gyro(const int32_t sample[3], struct xrt_vec3 *out_vec) 36 - { 37 - out_vec->x = (float)sample[0] * 0.00001f; 38 - out_vec->y = (float)sample[1] * 0.00001f; 39 - out_vec->z = (float)sample[2] * 0.00001f; 40 - } 41 - 42 - 43 - bool 44 - wmr_controller_packet_parse(const unsigned char *buffer, 45 - size_t len, 46 - struct wmr_controller_input *decoded_input, 47 - enum u_logging_level log_level) 48 - { 49 - if (len != 44) { 50 - U_LOG_IFL_E(log_level, "WMR Controller: unexpected message length: %zd", len); 51 - return false; 52 - } 53 - 54 - const unsigned char *p = buffer; 55 - 56 - // Read buttons 57 - uint8_t buttons = read8(&p); 58 - decoded_input->thumbstick.click = buttons & 0x01; 59 - decoded_input->home = buttons & 0x02; 60 - decoded_input->menu = buttons & 0x04; 61 - decoded_input->squeeze = buttons & 0x08; // squeeze-click 62 - decoded_input->trackpad.click = buttons & 0x10; 63 - decoded_input->bt_pairing = buttons & 0x20; 64 - decoded_input->trackpad.touch = buttons & 0x40; 65 - 66 - 67 - // Read thumbstick coordinates (12 bit resolution) 68 - int16_t stick_x = read8(&p); 69 - uint8_t nibbles = read8(&p); 70 - stick_x += ((nibbles & 0x0F) << 8); 71 - int16_t stick_y = (nibbles >> 4); 72 - stick_y += (read8(&p) << 4); 73 - 74 - decoded_input->thumbstick.values.x = (float)(stick_x - 0x07FF) / 0x07FF; 75 - if (decoded_input->thumbstick.values.x > 1.0f) { 76 - decoded_input->thumbstick.values.x = 1.0f; 77 - } 78 - 79 - decoded_input->thumbstick.values.y = (float)(stick_y - 0x07FF) / 0x07FF; 80 - if (decoded_input->thumbstick.values.y > 1.0f) { 81 - decoded_input->thumbstick.values.y = 1.0f; 82 - } 83 - 84 - // Read trigger value (0x00 - 0xFF) 85 - decoded_input->trigger = (float)read8(&p) / 0xFF; 86 - 87 - // Read trackpad coordinates (0x00 - 0x64. Both are 0xFF when untouched) 88 - uint8_t trackpad_x = read8(&p); 89 - uint8_t trackpad_y = read8(&p); 90 - decoded_input->trackpad.values.x = (trackpad_x == 0xFF) ? 0.0f : (float)(trackpad_x - 0x32) / 0x32; 91 - decoded_input->trackpad.values.y = (trackpad_y == 0xFF) ? 0.0f : (float)(trackpad_y - 0x32) / 0x32; 92 - 93 - 94 - decoded_input->battery = read8(&p); 95 - 96 - 97 - int32_t acc[3]; 98 - acc[0] = read24(&p); // x 99 - acc[1] = read24(&p); // y 100 - acc[2] = read24(&p); // z 101 - vec3_from_wmr_controller_accel(acc, &decoded_input->imu.acc); 102 - 103 - U_LOG_IFL_T(log_level, "Accel [m/s^2] : %f", 104 - sqrtf(decoded_input->imu.acc.x * decoded_input->imu.acc.x + 105 - decoded_input->imu.acc.y * decoded_input->imu.acc.y + 106 - decoded_input->imu.acc.z * decoded_input->imu.acc.z)); 107 - 108 - 109 - decoded_input->imu.temperature = read16(&p); 110 - 111 - 112 - int32_t gyro[3]; 113 - gyro[0] = read24(&p); 114 - gyro[1] = read24(&p); 115 - gyro[2] = read24(&p); 116 - vec3_from_wmr_controller_gyro(gyro, &decoded_input->imu.gyro); 117 - 118 - 119 - uint32_t prev_ticks = decoded_input->imu.timestamp_ticks & UINT32_C(0xFFFFFFFF); 120 - 121 - // Write the new ticks value into the lower half of timestamp_ticks 122 - decoded_input->imu.timestamp_ticks &= (UINT64_C(0xFFFFFFFF) << 32u); 123 - decoded_input->imu.timestamp_ticks += (uint32_t)read32(&p); 124 - 125 - if ((decoded_input->imu.timestamp_ticks & UINT64_C(0xFFFFFFFF)) < prev_ticks) { 126 - // Timer overflow, so increment the upper half of timestamp_ticks 127 - decoded_input->imu.timestamp_ticks += (UINT64_C(0x1) << 32u); 128 - } 129 - 130 - /* Todo: More decoding here 131 - read16(&p); // Unknown. Seems to depend on controller orientation. 132 - read32(&p); // Unknown. 133 - read16(&p); // Unknown. Device state, etc. 134 - read16(&p); 135 - read16(&p); 136 - */ 137 - 138 - return true; 139 - }

-53

src/xrt/drivers/wmr/wmr_controller_protocol.h

··· 40 40 // Messages types for WMR motion controllers 41 41 #define WMR_MOTION_CONTROLLER_STATUS_MSG 0x01 42 42 43 - 44 - struct wmr_controller_input 45 - { 46 - // buttons clicked 47 - bool menu; 48 - bool home; 49 - bool bt_pairing; 50 - bool squeeze; // Actually a "squeeze" click 51 - 52 - float trigger; 53 - 54 - struct 55 - { 56 - bool click; 57 - struct xrt_vec2 values; 58 - } thumbstick; 59 - struct 60 - { 61 - bool click; 62 - bool touch; 63 - struct xrt_vec2 values; 64 - } trackpad; 65 - 66 - uint8_t battery; 67 - 68 - struct 69 - { 70 - uint64_t timestamp_ticks; 71 - struct xrt_vec3 acc; 72 - struct xrt_vec3 gyro; 73 - int32_t temperature; 74 - } imu; 75 - }; 76 - 77 43 struct wmr_controller_fw_cmd 78 44 { 79 45 union { ··· 118 84 */ 119 85 120 86 #undef WMR_PACKED 121 - 122 - /*! 123 - * WMR Motion Controller protocol helpers 124 - * 125 - * @addtogroup drv_wmr 126 - * @{ 127 - */ 128 - 129 - bool 130 - wmr_controller_packet_parse(const unsigned char *buffer, 131 - size_t len, 132 - struct wmr_controller_input *decoded_input, 133 - enum u_logging_level log_level); 134 - 135 - 136 - /*! 137 - * @} 138 - */ 139 - 140 87 141 88 #ifdef __cplusplus 142 89 }

+2 -1

src/xrt/drivers/wmr/wmr_prober.c

··· 417 417 } 418 418 419 419 // Takes ownership of the hid_controller, even on failure 420 - struct xrt_device *xdev = wmr_bt_controller_create(hid_controller, controller_type, log_level); 420 + struct xrt_device *xdev = 421 + wmr_bt_controller_create(hid_controller, controller_type, xpdev->vendor_id, xpdev->product_id, log_level); 421 422 if (xdev == NULL) { 422 423 U_LOG_IFL_E(log_level, "Failed to create WMR controller (Bluetooth)"); 423 424 return XRT_ERROR_DEVICE_CREATION_FAILED;

Configure Feed

Configure Feed