The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

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.

authored by

Jan Schmidt and committed by
Jakob Bornecrantz
d326ff98 35f72696

+794 -493
+2
src/xrt/drivers/CMakeLists.txt
··· 364 364 wmr/wmr_common.h 365 365 wmr/wmr_config.c 366 366 wmr/wmr_config.h 367 + wmr/wmr_controller_base.c 368 + wmr/wmr_controller_base.h 367 369 wmr/wmr_bt_controller.c 368 370 wmr/wmr_bt_controller.h 369 371 wmr/wmr_hmd.c
+78 -447
src/xrt/drivers/wmr/wmr_bt_controller.c
··· 1 1 // Copyright 2020-2021, N Madsen. 2 2 // Copyright 2020-2021, Collabora, Ltd. 3 + // Copyright 2020-2023, Jan Schmidt 3 4 // SPDX-License-Identifier: BSL-1.0 4 5 /*! 5 6 * @file ··· 11 12 #include "os/os_time.h" 12 13 #include "os/os_hid.h" 13 14 14 - #include "math/m_mathinclude.h" 15 - #include "math/m_api.h" 16 - #include "math/m_vec2.h" 17 - #include "math/m_predict.h" 18 - 19 - #include "util/u_var.h" 20 - #include "util/u_misc.h" 21 - #include "util/u_time.h" 22 - #include "util/u_debug.h" 23 - #include "util/u_device.h" 24 15 #include "util/u_trace_marker.h" 25 16 26 17 #include "wmr_common.h" ··· 32 23 #include <string.h> 33 24 #include <assert.h> 34 25 #include <errno.h> 35 - #ifndef XRT_OS_WINDOWS 36 - #include <unistd.h> // for sleep() 37 - #endif 38 26 39 - #define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) 40 - #define WMR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) 41 - #define WMR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) 42 - #define WMR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) 43 - #define WMR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) 44 - 45 - #define SET_INPUT(NAME) (d->base.inputs[WMR_INDEX_##NAME].name = XRT_INPUT_WMR_##NAME) 46 - 47 - //! file path to store controller JSON configuration blocks that 48 - //! read from the firmware. 49 - DEBUG_GET_ONCE_OPTION(wmr_ctrl_config_path, "WMR_CONFIG_DUMP", NULL) 27 + #define WMR_TRACE(c, ...) U_LOG_IFL_T(c->log_level, __VA_ARGS__) 28 + #define WMR_DEBUG(c, ...) U_LOG_IFL_D(c->log_level, __VA_ARGS__) 29 + #define WMR_INFO(c, ...) U_LOG_IFL_I(c->log_level, __VA_ARGS__) 30 + #define WMR_WARN(c, ...) U_LOG_IFL_W(c->log_level, __VA_ARGS__) 31 + #define WMR_ERROR(c, ...) U_LOG_IFL_E(c->log_level, __VA_ARGS__) 50 32 51 - static inline struct wmr_bt_controller * 52 - wmr_bt_controller(struct xrt_device *p) 33 + static inline struct wmr_bt_connection * 34 + wmr_bt_connection(struct wmr_controller_connection *p) 53 35 { 54 - return (struct wmr_bt_controller *)p; 36 + return (struct wmr_bt_connection *)p; 55 37 } 56 38 57 39 static bool 58 - read_packets(struct wmr_bt_controller *d) 40 + read_packets(struct wmr_bt_connection *conn) 59 41 { 60 42 DRV_TRACE_MARKER(); 61 43 62 44 unsigned char buffer[WMR_MOTION_CONTROLLER_MSG_BUFFER_SIZE]; 63 45 64 46 // Better cpu efficiency with blocking reads instead of multiple reads. 65 - int size = os_hid_read(d->controller_hid, buffer, sizeof(buffer), 500); 47 + os_mutex_lock(&conn->hid_lock); 48 + int size = os_hid_read(conn->controller_hid, buffer, sizeof(buffer), 500); 66 49 67 50 // Get the timing as close to reading packet as possible. 68 51 uint64_t now_ns = os_monotonic_get_ns(); 52 + os_mutex_unlock(&conn->hid_lock); 69 53 70 54 DRV_TRACE_IDENT(read_packets_got); 71 55 72 56 if (size < 0) { 73 - WMR_ERROR(d, "WMR Controller (Bluetooth): Error reading from device"); 57 + WMR_ERROR(conn, "WMR Controller (Bluetooth): Error reading from device"); 74 58 return false; 75 59 } 76 60 if (size == 0) { 77 - WMR_TRACE(d, "WMR Controller (Bluetooth): No data to read from device"); 61 + WMR_TRACE(conn, "WMR Controller (Bluetooth): No data to read from device"); 78 62 return true; // No more messages, return. 79 63 } 80 64 81 - WMR_TRACE(d, "WMR Controller (Bluetooth): Read %u bytes from device", size); 65 + WMR_TRACE(conn, "WMR Controller (Bluetooth): Read %u bytes from device", size); 82 66 83 - switch (buffer[0]) { 84 - case WMR_BT_MOTION_CONTROLLER_MSG: 85 - os_mutex_lock(&d->lock); 86 - // Note: skipping msg type byte 87 - bool b = wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->input, d->log_level); 88 - if (b) { 89 - m_imu_3dof_update(&d->fusion, d->input.imu.timestamp_ticks * WMR_MOTION_CONTROLLER_NS_PER_TICK, 90 - &d->input.imu.acc, &d->input.imu.gyro); 91 - 92 - d->last_imu_timestamp_ns = now_ns; 93 - d->last_angular_velocity = d->input.imu.gyro; 94 - 95 - } else { 96 - WMR_ERROR(d, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i", 97 - buffer[0], size); 98 - os_mutex_unlock(&d->lock); 99 - return false; 100 - } 101 - os_mutex_unlock(&d->lock); 102 - break; 103 - default: 104 - WMR_DEBUG(d, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], size); 105 - break; 106 - } 67 + struct wmr_controller_connection *wcc = (struct wmr_controller_connection *)conn; 68 + wmr_controller_connection_receive_bytes(wcc, now_ns, buffer, size); 107 69 108 70 return true; 109 71 } 110 72 111 - /* 112 - * 113 - * Config functions. 114 - * 115 - */ 116 - 117 - static int 118 - wmr_controller_send_fw_cmd(struct wmr_bt_controller *d, 119 - const struct wmr_controller_fw_cmd *fw_cmd, 120 - unsigned char response_code, 121 - struct wmr_controller_fw_cmd_response *response) 122 - { 123 - // comms timeout. Replies are usually in 10ms or so but the first can take longer 124 - const int timeout_ms = 250; 125 - const int timeout_ns = timeout_ms * U_TIME_1MS_IN_NS; 126 - uint64_t timeout_start = os_monotonic_get_ns(); 127 - uint64_t timeout_end_ns = timeout_start + timeout_ns; 128 - struct os_hid_device *hid = d->controller_hid; 129 - 130 - os_hid_write(hid, fw_cmd->buf, sizeof(fw_cmd->buf)); 131 - 132 - do { 133 - int size = os_hid_read(hid, response->buf, sizeof(response->buf), timeout_ms); 134 - if (size == -1) { 135 - return -1; 136 - } 137 - 138 - if (size < 1) { 139 - // Ignore 0-byte reads (timeout) and try again 140 - continue; 141 - } 142 - 143 - WMR_TRACE(d, "Controller fw read returned %d bytes", size); 144 - if (response->buf[0] == response_code) { 145 - if (size != sizeof(response->buf) || (response->response.cmd_id_echo != fw_cmd->cmd.cmd_id)) { 146 - WMR_DEBUG( 147 - d, "Unexpected fw response - size %d (expected %zu), cmd_id_echo %u != cmd_id %u", 148 - size, sizeof(response->buf), response->response.cmd_id_echo, fw_cmd->cmd.cmd_id); 149 - return -1; 150 - } 151 - 152 - response->response.blk_remain = __le32_to_cpu(response->response.blk_remain); 153 - return size; 154 - } 155 - } while (os_monotonic_get_ns() < timeout_end_ns); 156 - 157 - WMR_WARN(d, "Controller fw read timed out after %u ms", 158 - (unsigned int)((os_monotonic_get_ns() - timeout_start) / U_TIME_1MS_IN_NS)); 159 - return -ETIMEDOUT; 160 - } 161 - 162 - XRT_MAYBE_UNUSED static int 163 - wmr_read_fw_block(struct wmr_bt_controller *d, uint8_t blk_id, uint8_t **out_data, size_t *out_size) 164 - { 165 - struct wmr_controller_fw_cmd_response fw_cmd_response; 166 - 167 - uint8_t *data; 168 - uint8_t *data_pos; 169 - uint8_t *data_end; 170 - uint32_t data_size; 171 - uint32_t remain; 172 - 173 - struct wmr_controller_fw_cmd fw_cmd; 174 - memset(&fw_cmd, 0, sizeof(fw_cmd)); 175 - 176 - fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, 0xffffffff); 177 - if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) { 178 - WMR_WARN(d, "Failed to read fw - cmd 0x02 failed to read header"); 179 - return -1; 180 - } 181 - 182 - data_size = fw_cmd_response.response.blk_remain + fw_cmd_response.response.len; 183 - WMR_DEBUG(d, "FW header %d bytes, %u bytes in block", fw_cmd_response.response.len, data_size); 184 - 185 - data = calloc(1, data_size + 1); 186 - if (!data) { 187 - return -1; 188 - } 189 - data[data_size] = '\0'; 190 - 191 - remain = data_size; 192 - data_pos = data; 193 - data_end = data + data_size; 194 - 195 - uint8_t to_copy = fw_cmd_response.response.len; 196 - 197 - memcpy(data_pos, fw_cmd_response.response.data, to_copy); 198 - data_pos += to_copy; 199 - remain -= to_copy; 200 - 201 - while (remain > 0) { 202 - fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, remain); 203 - 204 - os_nanosleep(U_TIME_1MS_IN_NS * 10); // Sleep 10ms 205 - if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) { 206 - WMR_WARN(d, "Failed to read fw - cmd 0x02 failed @ offset %zu", data_pos - data); 207 - return -1; 208 - } 209 - 210 - uint8_t to_copy = fw_cmd_response.response.len; 211 - if (data_pos + to_copy > data_end) 212 - to_copy = data_end - data_pos; 213 - 214 - WMR_DEBUG(d, "Read %d bytes @ offset %zu / %d", to_copy, data_pos - data, data_size); 215 - memcpy(data_pos, fw_cmd_response.response.data, to_copy); 216 - data_pos += to_copy; 217 - remain -= to_copy; 218 - } 219 - 220 - WMR_DEBUG(d, "Read %d-byte FW data block %d", data_size, blk_id); 221 - 222 - *out_data = data; 223 - *out_size = data_size; 224 - 225 - return 0; 226 - } 227 - 228 73 static bool 229 - read_controller_config(struct wmr_bt_controller *d) 74 + send_bytes(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size) 230 75 { 231 - unsigned char *data = NULL; 232 - unsigned char *config_json_block; 233 - size_t data_size; 234 - int ret; 76 + struct wmr_bt_connection *conn = (struct wmr_bt_connection *)(wcc); 235 77 236 - #if 0 237 - // There are extra firmware blocks that can be read from 238 - // the controllers, like these. Serial numbers and 239 - // USB PID/VID are visible in them, but it's not clear 240 - // what the layout is and we don't use them currently, 241 - // so this if 0 code is just exemplary. 78 + os_mutex_lock(&conn->hid_lock); 79 + int ret = os_hid_write(conn->controller_hid, buffer, buf_size); 80 + os_mutex_unlock(&conn->hid_lock); 242 81 243 - // Read serials 244 - ret = wmr_read_fw_block(d, 0x03, &data, &data_size); 245 - if (ret < 0 || data == NULL) 246 - return false; 247 - free(data); 248 - data = NULL; 249 - 250 - // Read block 0x14 251 - ret = wmr_read_fw_block(d, 0x14, &data, &data_size); 252 - if (ret < 0 || data == NULL) 253 - return false; 254 - free(data); 255 - data = NULL; 256 - #endif 257 - 258 - // Read config block 259 - ret = wmr_read_fw_block(d, 0x02, &data, &data_size); 260 - if (ret < 0 || data == NULL) 261 - return false; 262 - 263 - /* De-obfuscate the JSON config */ 264 - config_json_block = data + sizeof(uint16_t); 265 - for (unsigned int i = 0; i < data_size - sizeof(uint16_t); i++) { 266 - config_json_block[i] ^= wmr_config_key[i % sizeof(wmr_config_key)]; 267 - } 268 - 269 - #if 1 270 - // Option to dump config block to a path. Later, these will be 271 - // stored in a cache to save time on future startup 272 - const char *dump_dir = debug_get_option_wmr_ctrl_config_path(); 273 - if (dump_dir != NULL) { 274 - char fname[256]; 275 - 276 - int device_id = (d->base.device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) ? 0 : 1; 277 - 278 - sprintf(fname, "%s/controller-%d-fw.txt", dump_dir, device_id); 279 - WMR_INFO(d, "Storing controller config JSON to %s", fname); 280 - 281 - FILE *f = fopen(fname, "w"); 282 - fwrite(config_json_block, data_size - 2, 1, f); 283 - fclose(f); 284 - } 285 - #endif 286 - 287 - if (!wmr_controller_config_parse(&d->config, (char *)config_json_block, d->log_level)) { 288 - free(data); 289 - return false; 290 - } 291 - 292 - WMR_DEBUG(d, "Parsed %d LED entries from controller calibration", d->config.led_count); 293 - 294 - free(data); 295 - return true; 82 + return ret != -1 && (uint32_t)(ret) == buf_size; 296 83 } 297 84 298 - static void 299 - wmr_bt_controller_set_output(struct xrt_device *xdev, enum xrt_output_name name, const union xrt_output_value *value) 300 - { 301 - DRV_TRACE_MARKER(); 302 - 303 - // struct wmr_bt_controller *d = wmr_bt_controller(xdev); 304 - // Todo: implement 305 - } 306 - 307 - static void 308 - wmr_bt_controller_get_tracked_pose(struct xrt_device *xdev, 309 - enum xrt_input_name name, 310 - uint64_t at_timestamp_ns, 311 - struct xrt_space_relation *out_relation) 85 + /* Synchronously read a buffer from the HID connection. 86 + * This is only used for reading firmware during startup, 87 + * before the hid reading loop is running */ 88 + static int 89 + read_sync(struct wmr_controller_connection *wcc, uint8_t *buffer, uint32_t buf_size, int timeout_ms) 312 90 { 313 - DRV_TRACE_MARKER(); 314 - 315 - struct wmr_bt_controller *d = wmr_bt_controller(xdev); 316 - 317 - // Variables needed for prediction. 318 - uint64_t last_imu_timestamp_ns = 0; 319 - struct xrt_space_relation relation = {0}; 320 - relation.relation_flags = (enum xrt_space_relation_flags)( 321 - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | 322 - XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 323 - 324 - 325 - struct xrt_pose pose = {{0, 0, 0, 1}, {0, 1.2, -0.5}}; 326 - if (xdev->device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) { 327 - pose.position.x = -0.2; 328 - } else { 329 - pose.position.x = 0.2; 330 - } 331 - relation.pose = pose; 91 + struct wmr_bt_connection *conn = (struct wmr_bt_connection *)(wcc); 332 92 333 - // Copy data while holding the lock. 334 - os_mutex_lock(&d->lock); 335 - relation.pose.orientation = d->fusion.rot; 336 - relation.angular_velocity = d->last_angular_velocity; 337 - last_imu_timestamp_ns = d->last_imu_timestamp_ns; 338 - os_mutex_unlock(&d->lock); 93 + os_mutex_lock(&conn->hid_lock); 94 + int res = os_hid_read(conn->controller_hid, buffer, buf_size, timeout_ms); 95 + os_mutex_unlock(&conn->hid_lock); 339 96 340 - // No prediction needed. 341 - if (at_timestamp_ns < last_imu_timestamp_ns) { 342 - *out_relation = relation; 343 - return; 344 - } 345 - 346 - uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns; 347 - double prediction_s = time_ns_to_s(prediction_ns); 348 - 349 - m_predict_relation(&relation, prediction_s, out_relation); 350 - } 351 - 352 - 353 - 354 - static void 355 - wmr_bt_controller_update_inputs(struct xrt_device *xdev) 356 - { 357 - DRV_TRACE_MARKER(); 358 - 359 - struct wmr_bt_controller *d = wmr_bt_controller(xdev); 360 - 361 - struct xrt_input *inputs = d->base.inputs; 362 - 363 - os_mutex_lock(&d->lock); 364 - 365 - inputs[WMR_INDEX_MENU_CLICK].value.boolean = d->input.menu; 366 - inputs[WMR_INDEX_SQUEEZE_CLICK].value.boolean = d->input.squeeze; 367 - inputs[WMR_INDEX_TRIGGER_VALUE].value.vec1.x = d->input.trigger; 368 - inputs[WMR_INDEX_THUMBSTICK_CLICK].value.boolean = d->input.thumbstick.click; 369 - inputs[WMR_INDEX_THUMBSTICK].value.vec2 = d->input.thumbstick.values; 370 - inputs[WMR_INDEX_TRACKPAD_CLICK].value.boolean = d->input.trackpad.click; 371 - inputs[WMR_INDEX_TRACKPAD_TOUCH].value.boolean = d->input.trackpad.touch; 372 - inputs[WMR_INDEX_TRACKPAD].value.vec2 = d->input.trackpad.values; 373 - 374 - os_mutex_unlock(&d->lock); 97 + return res; 375 98 } 376 99 377 100 static void * 378 - wmr_bt_controller_run_thread(void *ptr) 101 + wmr_bt_connection_run_thread(void *ptr) 379 102 { 380 103 U_TRACE_SET_THREAD_NAME("WMR: BT-Controller"); 381 104 382 - struct wmr_bt_controller *d = wmr_bt_controller(ptr); 105 + struct wmr_bt_connection *conn = wmr_bt_connection(ptr); 383 106 384 - os_thread_helper_lock(&d->controller_thread); 385 - while (os_thread_helper_is_running_locked(&d->controller_thread)) { 386 - os_thread_helper_unlock(&d->controller_thread); 107 + os_thread_helper_lock(&conn->controller_thread); 108 + while (os_thread_helper_is_running_locked(&conn->controller_thread)) { 109 + os_thread_helper_unlock(&conn->controller_thread); 387 110 388 111 // Does not block. 389 - if (!read_packets(d)) { 112 + if (!read_packets(conn)) { 390 113 break; 391 114 } 392 115 } 393 116 394 - WMR_DEBUG(d, "WMR Controller (Bluetooth): Exiting reading thread."); 117 + WMR_DEBUG(conn, "WMR Controller (Bluetooth): Exiting reading thread."); 395 118 396 119 return NULL; 397 120 } 398 121 399 - 400 122 static void 401 - wmr_bt_controller_destroy(struct xrt_device *xdev) 123 + wmr_bt_connection_destroy(struct wmr_controller_connection *base) 402 124 { 403 - DRV_TRACE_MARKER(); 404 - 405 - struct wmr_bt_controller *d = wmr_bt_controller(xdev); 125 + struct wmr_bt_connection *conn = (struct wmr_bt_connection *)base; 406 126 407 - // Remove the variable tracking. 408 - u_var_remove_root(d); 127 + DRV_TRACE_MARKER(); 409 128 410 129 // Destroy the thread object. 411 - os_thread_helper_destroy(&d->controller_thread); 130 + os_thread_helper_destroy(&conn->controller_thread); 412 131 413 - if (d->controller_hid != NULL) { 414 - os_hid_destroy(d->controller_hid); 415 - d->controller_hid = NULL; 132 + if (conn->controller_hid != NULL) { 133 + os_hid_destroy(conn->controller_hid); 134 + conn->controller_hid = NULL; 416 135 } 417 136 418 - os_mutex_destroy(&d->lock); 419 - 420 - // Destroy the fusion. 421 - m_imu_3dof_close(&d->fusion); 137 + os_mutex_destroy(&conn->hid_lock); 422 138 423 - free(d); 139 + free(conn); 424 140 } 425 141 426 - 427 - /* 428 - * 429 - * Bindings 430 - * 431 - */ 432 - 433 - static struct xrt_binding_input_pair simple_inputs[4] = { 434 - {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_WMR_TRIGGER_VALUE}, 435 - {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_WMR_MENU_CLICK}, 436 - {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_WMR_GRIP_POSE}, 437 - {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_WMR_AIM_POSE}, 438 - }; 439 - 440 - static struct xrt_binding_output_pair simple_outputs[1] = { 441 - {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_WMR_HAPTIC}, 442 - }; 443 - 444 - static struct xrt_binding_profile binding_profiles[1] = { 445 - { 446 - .name = XRT_DEVICE_SIMPLE_CONTROLLER, 447 - .inputs = simple_inputs, 448 - .input_count = ARRAY_SIZE(simple_inputs), 449 - .outputs = simple_outputs, 450 - .output_count = ARRAY_SIZE(simple_outputs), 451 - }, 452 - }; 453 - 454 - 455 142 /* 456 143 * 457 144 * 'Exported' functions. 458 145 * 459 146 */ 460 - 461 147 462 148 struct xrt_device * 463 149 wmr_bt_controller_create(struct os_hid_device *controller_hid, ··· 466 152 { 467 153 DRV_TRACE_MARKER(); 468 154 469 - enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE; 470 - struct wmr_bt_controller *d = U_DEVICE_ALLOCATE(struct wmr_bt_controller, flags, 10, 1); 471 - 472 - d->log_level = log_level; 473 - d->controller_hid = controller_hid; 474 - 475 - if (controller_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) { 476 - snprintf(d->base.str, ARRAY_SIZE(d->base.str), "WMR Left Controller"); 477 - } else { 478 - snprintf(d->base.str, ARRAY_SIZE(d->base.str), "WMR Right Controller"); 479 - } 480 - 481 - d->base.destroy = wmr_bt_controller_destroy; 482 - d->base.get_tracked_pose = wmr_bt_controller_get_tracked_pose; 483 - d->base.set_output = wmr_bt_controller_set_output; 484 - d->base.update_inputs = wmr_bt_controller_update_inputs; 155 + struct wmr_bt_connection *conn = calloc(1, sizeof(struct wmr_bt_connection)); 485 156 486 - SET_INPUT(MENU_CLICK); 487 - SET_INPUT(SQUEEZE_CLICK); 488 - SET_INPUT(TRIGGER_VALUE); 489 - SET_INPUT(THUMBSTICK_CLICK); 490 - SET_INPUT(THUMBSTICK); 491 - SET_INPUT(TRACKPAD_CLICK); 492 - SET_INPUT(TRACKPAD_TOUCH); 493 - SET_INPUT(TRACKPAD); 494 - SET_INPUT(GRIP_POSE); 495 - SET_INPUT(AIM_POSE); 157 + conn->log_level = log_level; 158 + conn->controller_hid = controller_hid; 496 159 497 - for (uint32_t i = 0; i < d->base.input_count; i++) { 498 - d->base.inputs[0].active = true; 499 - } 500 - 501 - d->base.outputs[0].name = XRT_OUTPUT_NAME_WMR_HAPTIC; 502 - 503 - d->base.binding_profiles = binding_profiles; 504 - d->base.binding_profile_count = ARRAY_SIZE(binding_profiles); 505 - 506 - d->base.name = XRT_DEVICE_WMR_CONTROLLER; 507 - d->base.device_type = controller_type; 508 - d->base.orientation_tracking_supported = true; 509 - d->base.position_tracking_supported = false; 510 - d->base.hand_tracking_supported = true; 511 - 512 - 513 - d->input.imu.timestamp_ticks = 0; 514 - m_imu_3dof_init(&d->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); 515 - 516 - 160 + conn->base.send_bytes = send_bytes; 161 + conn->base.read_sync = read_sync; 162 + conn->base.disconnect = wmr_bt_connection_destroy; 517 163 518 164 int ret = 0; 519 165 520 - ret = os_mutex_init(&d->lock); 166 + ret = os_mutex_init(&conn->hid_lock); 521 167 if (ret != 0) { 522 - WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to init mutex!"); 523 - wmr_bt_controller_destroy(&d->base); 168 + WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to init mutex!"); 169 + wmr_bt_connection_destroy(&conn->base); 524 170 return NULL; 525 171 } 526 172 527 - // Read config file from controller 528 - if (!read_controller_config(d)) { 529 - wmr_bt_controller_destroy(&d->base); 173 + // Thread and other state. 174 + ret = os_thread_helper_init(&conn->controller_thread); 175 + if (ret != 0) { 176 + WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to init controller threading!"); 177 + wmr_bt_connection_destroy(&conn->base); 530 178 return NULL; 531 179 } 532 180 533 - // Thread and other state. 534 - ret = os_thread_helper_init(&d->controller_thread); 535 - if (ret != 0) { 536 - WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to init controller threading!"); 537 - wmr_bt_controller_destroy(&d->base); 538 - d = NULL; 181 + // Takes ownership of the connection 182 + struct wmr_controller_base *wcb = wmr_controller_base_create(&conn->base, controller_type, log_level); 183 + if (wcb == NULL) { 184 + WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to create controller"); 539 185 return NULL; 540 186 } 541 187 188 + // If the controller device was created, the connection belongs to 189 + // it now and will be cleaned up when it calls disconnect(). 190 + conn->base.wcb = wcb; 191 + 192 + struct xrt_device *xdev = &wcb->base; 193 + 542 194 // Hand over controller device to reading thread. 543 - ret = os_thread_helper_start(&d->controller_thread, wmr_bt_controller_run_thread, d); 195 + ret = os_thread_helper_start(&conn->controller_thread, wmr_bt_connection_run_thread, conn); 544 196 if (ret != 0) { 545 - WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to start controller thread!"); 546 - wmr_bt_controller_destroy(&d->base); 547 - d = NULL; 197 + WMR_ERROR(conn, "WMR Controller (Bluetooth): Failed to start controller thread!"); 198 + xrt_device_destroy(&xdev); 548 199 return NULL; 549 200 } 550 201 551 - 552 - u_var_add_root(d, d->base.str, true); 553 - u_var_add_bool(d, &d->input.menu, "input.menu"); 554 - u_var_add_bool(d, &d->input.home, "input.home"); 555 - u_var_add_bool(d, &d->input.bt_pairing, "input.bt_pairing"); 556 - u_var_add_bool(d, &d->input.squeeze, "input.squeeze"); 557 - u_var_add_f32(d, &d->input.trigger, "input.trigger"); 558 - u_var_add_u8(d, &d->input.battery, "input.battery"); 559 - u_var_add_bool(d, &d->input.thumbstick.click, "input.thumbstick.click"); 560 - u_var_add_f32(d, &d->input.thumbstick.values.x, "input.thumbstick.values.y"); 561 - u_var_add_f32(d, &d->input.thumbstick.values.y, "input.thumbstick.values.x"); 562 - u_var_add_bool(d, &d->input.trackpad.click, "input.trackpad.click"); 563 - u_var_add_bool(d, &d->input.trackpad.touch, "input.trackpad.touch"); 564 - u_var_add_f32(d, &d->input.trackpad.values.x, "input.trackpad.values.x"); 565 - u_var_add_f32(d, &d->input.trackpad.values.y, "input.trackpad.values.y"); 566 - u_var_add_ro_vec3_f32(d, &d->input.imu.acc, "imu.acc"); 567 - u_var_add_ro_vec3_f32(d, &d->input.imu.gyro, "imu.gyro"); 568 - u_var_add_i32(d, &d->input.imu.temperature, "imu.temperature"); 569 - 570 - 571 - return &d->base; 202 + return xdev; 572 203 }
+10 -43
src/xrt/drivers/wmr/wmr_bt_controller.h
··· 1 1 // Copyright 2020-2021, N Madsen. 2 2 // Copyright 2020-2021, Collabora, Ltd. 3 + // Copyright 2020-2023, Jan Schmidt 3 4 // SPDX-License-Identifier: BSL-1.0 4 5 /*! 5 6 * @file 6 7 * @brief Driver interface for Bluetooth based WMR motion controllers. 7 - * Note: Only tested with HP Reverb (G1) controllers that are manually 8 - * paired to a non hmd-integrated, generic BT usb adapter. 9 8 * @author Nis Madsen <nima_zero_one@protonmail.com> 10 9 * @ingroup drv_wmr 11 10 */ 12 11 #pragma once 13 12 14 13 #include "os/os_threading.h" 15 - #include "math/m_imu_3dof.h" 16 14 #include "util/u_logging.h" 17 - #include "xrt/xrt_device.h" 18 15 19 - #include "wmr_controller_protocol.h" 20 - #include "wmr_config.h" 16 + #include "wmr_controller_base.h" 21 17 22 18 #ifdef __cplusplus 23 19 extern "C" { ··· 25 21 26 22 27 23 /*! 28 - * Indices in input list of each input. 29 - */ 30 - enum wmr_bt_input_index 31 - { 32 - WMR_INDEX_MENU_CLICK, 33 - WMR_INDEX_SQUEEZE_CLICK, 34 - WMR_INDEX_TRIGGER_VALUE, 35 - WMR_INDEX_THUMBSTICK_CLICK, 36 - WMR_INDEX_THUMBSTICK, 37 - WMR_INDEX_TRACKPAD_CLICK, 38 - WMR_INDEX_TRACKPAD_TOUCH, 39 - WMR_INDEX_TRACKPAD, 40 - WMR_INDEX_GRIP_POSE, 41 - WMR_INDEX_AIM_POSE, 42 - }; 43 - 44 - /*! 45 - * A Bluetooth connected WMR Controller device, representing just a single controller. 24 + * A connection to a Bluetooth connected WMR Controller device 46 25 * 47 26 * @ingroup drv_wmr 48 - * @implements xrt_device 27 + * @implements wmr_controller_connection 49 28 */ 50 - struct wmr_bt_controller 29 + struct wmr_bt_connection 51 30 { 52 - struct xrt_device base; 31 + struct wmr_controller_connection base; 32 + 33 + enum u_logging_level log_level; 53 34 54 35 struct os_hid_device *controller_hid; 55 36 struct os_thread_helper controller_thread; 56 37 57 - /* firmware configuration block */ 58 - struct wmr_controller_config config; 59 - 60 - struct os_mutex lock; 61 - 62 - //! The last decoded package of IMU and button data 63 - struct wmr_controller_input input; 64 - //! Time of last IMU sample, in CPU time. 65 - uint64_t last_imu_timestamp_ns; 66 - //! Main fusion calculator. 67 - struct m_imu_3dof fusion; 68 - //! The last angular velocity from the IMU, for prediction. 69 - struct xrt_vec3 last_angular_velocity; 38 + struct os_mutex hid_lock; 70 39 71 - enum u_logging_level log_level; 40 + struct wmr_controller_base *wcb; 72 41 }; 73 - 74 42 75 43 struct xrt_device * 76 44 wmr_bt_controller_create(struct os_hid_device *controller_hid, 77 45 enum xrt_device_type controller_type, 78 46 enum u_logging_level log_level); 79 - 80 47 81 48 #ifdef __cplusplus 82 49 }
+565
src/xrt/drivers/wmr/wmr_controller_base.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 Controller. 8 + * @author Jan Schmidt <jan@centricular.com> 9 + * @ingroup drv_wmr 10 + */ 11 + 12 + #include "os/os_time.h" 13 + #include "os/os_hid.h" 14 + 15 + #include "math/m_mathinclude.h" 16 + #include "math/m_api.h" 17 + #include "math/m_vec2.h" 18 + #include "math/m_predict.h" 19 + 20 + #include "util/u_var.h" 21 + #include "util/u_misc.h" 22 + #include "util/u_time.h" 23 + #include "util/u_debug.h" 24 + #include "util/u_device.h" 25 + #include "util/u_trace_marker.h" 26 + 27 + #include "wmr_common.h" 28 + #include "wmr_controller_base.h" 29 + #include "wmr_config_key.h" 30 + 31 + #include <stdio.h> 32 + #include <stdlib.h> 33 + #include <string.h> 34 + #include <assert.h> 35 + #include <errno.h> 36 + 37 + #define WMR_TRACE(wcb, ...) U_LOG_XDEV_IFL_T(&wcb->base, wcb->log_level, __VA_ARGS__) 38 + #define WMR_DEBUG(wcb, ...) U_LOG_XDEV_IFL_D(&wcb->base, wcb->log_level, __VA_ARGS__) 39 + #define WMR_INFO(wcb, ...) U_LOG_XDEV_IFL_I(&wcb->base, wcb->log_level, __VA_ARGS__) 40 + #define WMR_WARN(wcb, ...) U_LOG_XDEV_IFL_W(&wcb->base, wcb->log_level, __VA_ARGS__) 41 + #define WMR_ERROR(wcb, ...) U_LOG_XDEV_IFL_E(&wcb->base, wcb->log_level, __VA_ARGS__) 42 + 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 + }; 59 + 60 + #define SET_INPUT(NAME) (wcb->base.inputs[WMR_INDEX_##NAME].name = XRT_INPUT_WMR_##NAME) 61 + 62 + //! file path to store controller JSON configuration blocks that 63 + //! read from the firmware. 64 + DEBUG_GET_ONCE_OPTION(wmr_ctrl_config_path, "WMR_CONFIG_DUMP", NULL) 65 + 66 + static inline struct wmr_controller_base * 67 + wmr_controller_base(struct xrt_device *p) 68 + { 69 + return (struct wmr_controller_base *)p; 70 + } 71 + 72 + static void 73 + receive_bytes(struct wmr_controller_base *wcb, uint64_t time_ns, uint8_t *buffer, uint32_t buf_size) 74 + { 75 + if (buf_size < 1) { 76 + WMR_ERROR(wcb, "WMR Controller: Error receiving short packet"); 77 + return; 78 + } 79 + 80 + switch (buffer[0]) { 81 + case WMR_MOTION_CONTROLLER_STATUS_MSG: 82 + os_mutex_lock(&wcb->data_lock); 83 + // 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); 89 + 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); 97 + return; 98 + } 99 + os_mutex_unlock(&wcb->data_lock); 100 + break; 101 + default: 102 + WMR_DEBUG(wcb, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], buf_size); 103 + break; 104 + } 105 + 106 + return; 107 + } 108 + 109 + static bool 110 + wmr_controller_send_bytes(struct wmr_controller_base *wcb, const uint8_t *buffer, uint32_t buf_size) 111 + { 112 + bool res = false; 113 + 114 + os_mutex_lock(&wcb->conn_lock); 115 + struct wmr_controller_connection *conn = wcb->wcc; 116 + if (conn != NULL) { 117 + res = wmr_controller_connection_send_bytes(conn, buffer, buf_size); 118 + } 119 + os_mutex_unlock(&wcb->conn_lock); 120 + 121 + return res; 122 + } 123 + 124 + static int 125 + wmr_controller_read_sync(struct wmr_controller_base *wcb, uint8_t *buffer, uint32_t buf_size, int timeout_ms) 126 + { 127 + int res = -1; 128 + os_mutex_lock(&wcb->conn_lock); 129 + struct wmr_controller_connection *conn = wcb->wcc; 130 + if (conn != NULL) { 131 + res = wmr_controller_connection_read_sync(conn, buffer, buf_size, timeout_ms); 132 + } 133 + os_mutex_unlock(&wcb->conn_lock); 134 + 135 + return res; 136 + } 137 + 138 + static int 139 + wmr_controller_send_fw_cmd(struct wmr_controller_base *wcb, 140 + const struct wmr_controller_fw_cmd *fw_cmd, 141 + unsigned char response_code, 142 + struct wmr_controller_fw_cmd_response *response) 143 + { 144 + // comms timeout. Replies are usually in 10ms or so but the first can take longer 145 + const int timeout_ms = 250; 146 + const int timeout_ns = timeout_ms * U_TIME_1MS_IN_NS; 147 + uint64_t timeout_start = os_monotonic_get_ns(); 148 + uint64_t timeout_end_ns = timeout_start + timeout_ns; 149 + 150 + if (!wmr_controller_send_bytes(wcb, fw_cmd->buf, sizeof(fw_cmd->buf))) { 151 + return -1; 152 + } 153 + 154 + do { 155 + int size = wmr_controller_read_sync(wcb, response->buf, sizeof(response->buf), timeout_ms); 156 + if (size == -1) { 157 + return -1; 158 + } 159 + 160 + if (size < 1) { 161 + // Ignore 0-byte reads (timeout) and try again 162 + continue; 163 + } 164 + 165 + WMR_TRACE(wcb, "Controller fw read returned %d bytes", size); 166 + if (response->buf[0] == response_code) { 167 + if (size != sizeof(response->buf) || (response->response.cmd_id_echo != fw_cmd->cmd.cmd_id)) { 168 + WMR_DEBUG( 169 + wcb, "Unexpected fw response - size %d (expected %zu), cmd_id_echo %u != cmd_id %u", 170 + size, sizeof(response->buf), response->response.cmd_id_echo, fw_cmd->cmd.cmd_id); 171 + return -1; 172 + } 173 + 174 + response->response.blk_remain = __le32_to_cpu(response->response.blk_remain); 175 + return size; 176 + } 177 + } while (os_monotonic_get_ns() < timeout_end_ns); 178 + 179 + WMR_WARN(wcb, "Controller fw read timed out after %u ms", 180 + (unsigned int)((os_monotonic_get_ns() - timeout_start) / U_TIME_1MS_IN_NS)); 181 + return -ETIMEDOUT; 182 + } 183 + 184 + XRT_MAYBE_UNUSED static int 185 + wmr_read_fw_block(struct wmr_controller_base *d, uint8_t blk_id, uint8_t **out_data, size_t *out_size) 186 + { 187 + struct wmr_controller_fw_cmd_response fw_cmd_response; 188 + 189 + uint8_t *data; 190 + uint8_t *data_pos; 191 + uint8_t *data_end; 192 + uint32_t data_size; 193 + uint32_t remain; 194 + 195 + struct wmr_controller_fw_cmd fw_cmd; 196 + memset(&fw_cmd, 0, sizeof(fw_cmd)); 197 + 198 + fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, 0xffffffff); 199 + if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) { 200 + WMR_WARN(d, "Failed to read fw - cmd 0x02 failed to read header for block %d", blk_id); 201 + return -1; 202 + } 203 + 204 + data_size = fw_cmd_response.response.blk_remain + fw_cmd_response.response.len; 205 + WMR_DEBUG(d, "FW header %d bytes, %u bytes in block", fw_cmd_response.response.len, data_size); 206 + if (data_size == 0) 207 + return -1; 208 + 209 + data = calloc(1, data_size + 1); 210 + if (!data) { 211 + return -1; 212 + } 213 + data[data_size] = '\0'; 214 + 215 + remain = data_size; 216 + data_pos = data; 217 + data_end = data + data_size; 218 + 219 + uint8_t to_copy = fw_cmd_response.response.len; 220 + 221 + memcpy(data_pos, fw_cmd_response.response.data, to_copy); 222 + data_pos += to_copy; 223 + remain -= to_copy; 224 + 225 + while (remain > 0) { 226 + fw_cmd = WMR_CONTROLLER_FW_CMD_INIT(0x06, 0x02, blk_id, remain); 227 + 228 + os_nanosleep(U_TIME_1MS_IN_NS * 10); // Sleep 10ms 229 + if (wmr_controller_send_fw_cmd(d, &fw_cmd, 0x02, &fw_cmd_response) < 0) { 230 + WMR_WARN(d, "Failed to read fw - cmd 0x02 failed @ offset %zu", data_pos - data); 231 + return -1; 232 + } 233 + 234 + uint8_t to_copy = fw_cmd_response.response.len; 235 + if (data_pos + to_copy > data_end) 236 + to_copy = data_end - data_pos; 237 + 238 + WMR_DEBUG(d, "Read %d bytes @ offset %zu / %d", to_copy, data_pos - data, data_size); 239 + memcpy(data_pos, fw_cmd_response.response.data, to_copy); 240 + data_pos += to_copy; 241 + remain -= to_copy; 242 + } 243 + 244 + WMR_DEBUG(d, "Read %d-byte FW data block %d", data_size, blk_id); 245 + 246 + *out_data = data; 247 + *out_size = data_size; 248 + 249 + return 0; 250 + } 251 + 252 + /* 253 + * 254 + * Config functions. 255 + * 256 + */ 257 + 258 + static bool 259 + read_controller_config(struct wmr_controller_base *wcb) 260 + { 261 + unsigned char *data = NULL; 262 + unsigned char *config_json_block; 263 + size_t data_size; 264 + int ret; 265 + 266 + #if 1 267 + // There are extra firmware blocks that can be read from 268 + // the controllers, like these. Serial numbers and 269 + // USB PID/VID are visible in them, but it's not clear 270 + // what the layout is and we don't use them currently, 271 + // so this if 0 code is just exemplary. 272 + 273 + // Read 0x00 block 274 + ret = wmr_read_fw_block(wcb, 0x0, &data, &data_size); 275 + if (ret < 0 || data == NULL) 276 + return false; 277 + free(data); 278 + data = NULL; 279 + 280 + // Read serials 281 + ret = wmr_read_fw_block(wcb, 0x03, &data, &data_size); 282 + if (ret < 0 || data == NULL) 283 + return false; 284 + free(data); 285 + data = NULL; 286 + 287 + // Read block 0x14 288 + ret = wmr_read_fw_block(wcb, 0x14, &data, &data_size); 289 + if (ret < 0 || data == NULL) 290 + return false; 291 + free(data); 292 + data = NULL; 293 + #endif 294 + 295 + // Read config block 296 + ret = wmr_read_fw_block(wcb, 0x02, &data, &data_size); 297 + if (ret < 0 || data == NULL) 298 + return false; 299 + 300 + /* De-obfuscate the JSON config */ 301 + config_json_block = data + sizeof(uint16_t); 302 + for (unsigned int i = 0; i < data_size - sizeof(uint16_t); i++) { 303 + config_json_block[i] ^= wmr_config_key[i % sizeof(wmr_config_key)]; 304 + } 305 + 306 + #if 1 307 + // Option to dump config block to a path. Later, these will be 308 + // stored in a cache to save time on future startup 309 + const char *dump_dir = debug_get_option_wmr_ctrl_config_path(); 310 + if (dump_dir != NULL) { 311 + char fname[256]; 312 + 313 + int device_id = (wcb->base.device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) ? 0 : 1; 314 + 315 + sprintf(fname, "%s/controller-%d-fw.txt", dump_dir, device_id); 316 + WMR_INFO(wcb, "Storing controller config JSON to %s", fname); 317 + 318 + FILE *f = fopen(fname, "w"); 319 + fwrite(config_json_block, data_size - 2, 1, f); 320 + fclose(f); 321 + } 322 + #endif 323 + 324 + if (!wmr_controller_config_parse(&wcb->config, (char *)config_json_block, wcb->log_level)) { 325 + free(data); 326 + return false; 327 + } 328 + 329 + WMR_DEBUG(wcb, "Parsed %d LED entries from controller calibration", wcb->config.led_count); 330 + 331 + free(data); 332 + return true; 333 + } 334 + 335 + 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 + wmr_controller_base_get_tracked_pose(struct xrt_device *xdev, 346 + enum xrt_input_name name, 347 + uint64_t at_timestamp_ns, 348 + struct xrt_space_relation *out_relation) 349 + { 350 + DRV_TRACE_MARKER(); 351 + 352 + struct wmr_controller_base *wcb = wmr_controller_base(xdev); 353 + 354 + // Variables needed for prediction. 355 + uint64_t last_imu_timestamp_ns = 0; 356 + struct xrt_space_relation relation = {0}; 357 + relation.relation_flags = (enum xrt_space_relation_flags)( 358 + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | 359 + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 360 + 361 + 362 + struct xrt_pose pose = {{0, 0, 0, 1}, {0, 1.2, -0.5}}; 363 + if (xdev->device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) { 364 + pose.position.x = -0.2; 365 + } else { 366 + pose.position.x = 0.2; 367 + } 368 + relation.pose = pose; 369 + 370 + // Copy data while holding the lock. 371 + os_mutex_lock(&wcb->data_lock); 372 + relation.pose.orientation = wcb->fusion.rot; 373 + relation.angular_velocity = wcb->last_angular_velocity; 374 + last_imu_timestamp_ns = wcb->last_imu_timestamp_ns; 375 + os_mutex_unlock(&wcb->data_lock); 376 + 377 + // No prediction needed. 378 + if (at_timestamp_ns < last_imu_timestamp_ns) { 379 + *out_relation = relation; 380 + return; 381 + } 382 + 383 + uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns; 384 + double prediction_s = time_ns_to_s(prediction_ns); 385 + 386 + m_predict_relation(&relation, prediction_s, out_relation); 387 + } 388 + 389 + 390 + 391 + static void 392 + wmr_controller_base_update_inputs(struct xrt_device *xdev) 393 + { 394 + DRV_TRACE_MARKER(); 395 + 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 + // Remove the variable tracking. 422 + u_var_remove_root(wcb); 423 + 424 + // Disconnect from the connection so we don't 425 + // receive any more callbacks 426 + os_mutex_lock(&wcb->conn_lock); 427 + struct wmr_controller_connection *conn = wcb->wcc; 428 + wcb->wcc = NULL; 429 + os_mutex_unlock(&wcb->conn_lock); 430 + 431 + if (conn != NULL) { 432 + wmr_controller_connection_disconnect(conn); 433 + } 434 + 435 + os_mutex_destroy(&wcb->conn_lock); 436 + os_mutex_destroy(&wcb->data_lock); 437 + 438 + // Destroy the fusion. 439 + m_imu_3dof_close(&wcb->fusion); 440 + 441 + free(wcb); 442 + } 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 + 473 + /* 474 + * 475 + * 'Exported' functions. 476 + * 477 + */ 478 + 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) 483 + { 484 + 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 + 489 + wcb->log_level = log_level; 490 + wcb->wcc = conn; 491 + wcb->receive_bytes = receive_bytes; 492 + 493 + if (controller_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) { 494 + snprintf(wcb->base.str, ARRAY_SIZE(wcb->base.str), "WMR Left Controller"); 495 + } else { 496 + snprintf(wcb->base.str, ARRAY_SIZE(wcb->base.str), "WMR Right Controller"); 497 + } 498 + 499 + wcb->base.destroy = wmr_controller_base_destroy; 500 + 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 + 524 + wcb->base.name = XRT_DEVICE_WMR_CONTROLLER; 525 + wcb->base.device_type = controller_type; 526 + wcb->base.orientation_tracking_supported = true; 527 + wcb->base.position_tracking_supported = false; 528 + wcb->base.hand_tracking_supported = true; 529 + 530 + 531 + wcb->input.imu.timestamp_ticks = 0; 532 + m_imu_3dof_init(&wcb->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); 533 + 534 + if (os_mutex_init(&wcb->conn_lock) != 0 || os_mutex_init(&wcb->data_lock) != 0) { 535 + WMR_ERROR(wcb, "WMR Controller: Failed to init mutex!"); 536 + wmr_controller_base_destroy(&wcb->base); 537 + return NULL; 538 + } 539 + 540 + // Read config file from controller 541 + if (!read_controller_config(wcb)) { 542 + wmr_controller_base_destroy(&wcb->base); 543 + return NULL; 544 + } 545 + 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"); 563 + 564 + return wcb; 565 + }
+136
src/xrt/drivers/wmr/wmr_controller_base.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 Common implementation for WMR controllers, handling 9 + * shared behaviour such as communication, configuration reading, 10 + * IMU integration. 11 + * @author Jan Schmidt <jan@centricular.com> 12 + * @author Nis Madsen <nima_zero_one@protonmail.com> 13 + * @ingroup drv_wmr 14 + */ 15 + #pragma once 16 + 17 + #include "os/os_threading.h" 18 + #include "math/m_imu_3dof.h" 19 + #include "util/u_logging.h" 20 + #include "xrt/xrt_device.h" 21 + 22 + #include "wmr_controller_protocol.h" 23 + #include "wmr_config.h" 24 + 25 + #ifdef __cplusplus 26 + extern "C" { 27 + #endif 28 + 29 + struct wmr_controller_base; 30 + 31 + /*! 32 + * A connection for communicating with the controller. 33 + * The mechanism is implementation specific, so there are 34 + * two variants for either communicating directly with a 35 + * controller via bluetooth, and another for talking 36 + * to a controller through a headset tunnelled mapping. 37 + * 38 + * The controller implementation doesn't need to care how 39 + * the communication is implemented. 40 + * 41 + * The connection is reference counted and mutex protected, 42 + * as both the controller and the connection implementation 43 + * may need to hold a reference to it, and to detach 44 + * safely when shutting down. 45 + */ 46 + struct wmr_controller_connection 47 + { 48 + //! The controller this connection is talking to. 49 + struct wmr_controller_base *wcb; 50 + 51 + bool (*send_bytes)(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size); 52 + int (*read_sync)(struct wmr_controller_connection *wcc, uint8_t *buffer, uint32_t buf_size, int timeout_ms); 53 + 54 + void (*disconnect)(struct wmr_controller_connection *wcc); 55 + }; 56 + 57 + static inline bool 58 + wmr_controller_connection_send_bytes(struct wmr_controller_connection *wcc, const uint8_t *buffer, uint32_t buf_size) 59 + { 60 + assert(wcc->send_bytes != NULL); 61 + return wcc->send_bytes(wcc, buffer, buf_size); 62 + } 63 + 64 + static inline int 65 + wmr_controller_connection_read_sync(struct wmr_controller_connection *wcc, 66 + uint8_t *buffer, 67 + uint32_t buf_size, 68 + int timeout_ms) 69 + { 70 + return wcc->read_sync(wcc, buffer, buf_size, timeout_ms); 71 + } 72 + 73 + static inline void 74 + wmr_controller_connection_disconnect(struct wmr_controller_connection *wcc) 75 + { 76 + wcc->disconnect(wcc); 77 + } 78 + 79 + /*! 80 + * Common base for all WMR controllers. 81 + * 82 + * @ingroup drv_wmr 83 + * @implements xrt_device 84 + */ 85 + struct wmr_controller_base 86 + { 87 + //! Base struct. 88 + struct xrt_device base; 89 + 90 + //! Mutex protects the controller connection 91 + struct os_mutex conn_lock; 92 + 93 + //! The connection for this controller. 94 + struct wmr_controller_connection *wcc; 95 + 96 + //! Callback from the connection when a packet has been received. 97 + void (*receive_bytes)(struct wmr_controller_base *wcb, uint64_t time_ns, uint8_t *buffer, uint32_t buf_size); 98 + 99 + enum u_logging_level log_level; 100 + 101 + //! Mutex protects shared data used from OpenXR callbacks 102 + struct os_mutex data_lock; 103 + 104 + /* firmware configuration block */ 105 + struct wmr_controller_config config; 106 + 107 + //! The last decoded package of IMU and button data 108 + struct wmr_controller_input input; 109 + //! Time of last IMU sample, in CPU time. 110 + uint64_t last_imu_timestamp_ns; 111 + //! Main fusion calculator. 112 + struct m_imu_3dof fusion; 113 + //! The last angular velocity from the IMU, for prediction. 114 + struct xrt_vec3 last_angular_velocity; 115 + }; 116 + 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); 121 + 122 + 123 + static inline void 124 + wmr_controller_connection_receive_bytes(struct wmr_controller_connection *wcc, 125 + uint64_t time_ns, 126 + uint8_t *buffer, 127 + uint32_t buf_size) 128 + { 129 + struct wmr_controller_base *wcb = wcc->wcb; 130 + 131 + wcb->receive_bytes(wcb, time_ns, buffer, buf_size); 132 + } 133 + 134 + #ifdef __cplusplus 135 + } 136 + #endif
+2 -2
src/xrt/drivers/wmr/wmr_controller_protocol.h
··· 37 37 #define WMR_MOTION_CONTROLLER_NS_PER_TICK 100 38 38 39 39 40 - // Messages types specific to Bluetooth connected WMR motion controllers 41 - #define WMR_BT_MOTION_CONTROLLER_MSG 0x01 40 + // Messages types for WMR motion controllers 41 + #define WMR_MOTION_CONTROLLER_STATUS_MSG 0x01 42 42 43 43 44 44 struct wmr_controller_input
+1 -1
src/xrt/drivers/wmr/wmr_prober.c
··· 416 416 return XRT_ERROR_DEVICE_CREATION_FAILED; 417 417 } 418 418 419 + // Takes ownership of the hid_controller, even on failure 419 420 struct xrt_device *xdev = wmr_bt_controller_create(hid_controller, controller_type, log_level); 420 421 if (xdev == NULL) { 421 422 U_LOG_IFL_E(log_level, "Failed to create WMR controller (Bluetooth)"); 422 - os_hid_destroy(hid_controller); 423 423 return XRT_ERROR_DEVICE_CREATION_FAILED; 424 424 } 425 425