d/wmr: Split the connection from controller · matrixfurry.com/monado@d326ff9

+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

Configure Feed

Configure Feed