+274 -32

src/xrt/drivers/pssense/pssense_driver.c

··· 1 1 // Copyright 2023, Collabora, Ltd. 2 2 + // Copyright 2023, Jarett Millard 2 3 // SPDX-License-Identifier: BSL-1.0 3 4 /*! 4 5 * @file ··· 24 25 #include "util/u_trace_marker.h" 25 26 26 27 #include "pssense_interface.h" 28 28 + #include "math/m_mathinclude.h" 29 29 + #include "math/m_space.h" 30 30 + #include "math/m_imu_3dof.h" 27 31 28 32 #include <stdio.h> 29 33 ··· 37 41 #define PSSENSE_WARN(p, ...) U_LOG_XDEV_IFL_W(&p->base, p->log_level, __VA_ARGS__) 38 42 #define PSSENSE_ERROR(p, ...) U_LOG_XDEV_IFL_E(&p->base, p->log_level, __VA_ARGS__) 39 43 40 40 - DEBUG_GET_ONCE_LOG_OPTION(pssense_log, "PSSENSE_LOG", U_LOGGING_WARN) 44 44 + DEBUG_GET_ONCE_LOG_OPTION(pssense_log, "PSSENSE_LOG", U_LOGGING_INFO) 45 45 + 46 46 + #define DEG_TO_RAD(DEG) (DEG * M_PI / 180.) 47 47 + 48 48 + static struct xrt_binding_input_pair simple_inputs_pssense[4] = { 49 49 + {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_PSSENSE_TRIGGER_VALUE}, 50 50 + {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_PSSENSE_OPTIONS_CLICK}, 51 51 + {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_PSSENSE_GRIP_POSE}, 52 52 + {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_PSSENSE_AIM_POSE}, 53 53 + }; 54 54 + 55 55 + static struct xrt_binding_profile binding_profiles_pssense[1] = { 56 56 + { 57 57 + .name = XRT_DEVICE_SIMPLE_CONTROLLER, 58 58 + .inputs = simple_inputs_pssense, 59 59 + .input_count = ARRAY_SIZE(simple_inputs_pssense), 60 60 + .outputs = NULL, 61 61 + .output_count = 0, 62 62 + }, 63 63 + }; 41 64 42 65 /*! 43 66 * Indices where each input is in the input list. ··· 64 87 PSSENSE_INDEX_TRIGGER_PROXIMITY, 65 88 PSSENSE_INDEX_THUMBSTICK, 66 89 PSSENSE_INDEX_THUMBSTICK_CLICK, 67 67 - PSSENSE_INDEX_THUMBSTICK_TOUCH 90 90 + PSSENSE_INDEX_THUMBSTICK_TOUCH, 91 91 + PSSENSE_INDEX_GRIP_POSE, 92 92 + PSSENSE_INDEX_AIM_POSE, 93 93 + }; 94 94 + 95 95 + const uint8_t HID_PACKET_REPORT_ID = 0x31; 96 96 + const uint8_t CALIBRATION_DATA_FEATURE_REPORT_ID = 0x05; 97 97 + const uint8_t CALIBRATION_DATA_PART_ID_1 = 0; 98 98 + const uint8_t CALIBRATION_DATA_PART_ID_2 = 0x81; 99 99 + 100 100 + /** 101 101 + * Gyro read value range is +-32768. 102 102 + */ 103 103 + const double PSSENSE_GYRO_SCALE_DEG = 180.0 / 1024; 104 104 + /** 105 105 + * Accelerometer read value range is +-32768 and covers +-8 g. 106 106 + */ 107 107 + const double PSSENSE_ACCEL_SCALE = MATH_GRAVITY_M_S2 / 4096; 108 108 + 109 109 + /** 110 110 + * 16-bit little-endian int 111 111 + */ 112 112 + struct pssense_i16_le 113 113 + { 114 114 + uint8_t low; 115 115 + uint8_t high; 116 116 + }; 117 117 + 118 118 + /** 119 119 + * 32-bit little-endian int 120 120 + */ 121 121 + struct pssense_i32_le 122 122 + { 123 123 + uint8_t lowest; 124 124 + uint8_t lower; 125 125 + uint8_t higher; 126 126 + uint8_t highest; 68 127 }; 69 128 70 129 /*! ··· 72 131 */ 73 132 struct pssense_data_packet 74 133 { 75 75 - uint8_t header; 134 134 + uint8_t report_id; 135 135 + uint8_t bt_header; 76 136 uint8_t thumbstick_x; 77 137 uint8_t thumbstick_y; 78 138 uint8_t trigger_value; 79 139 uint8_t trigger_proximity; 80 140 uint8_t squeeze_proximity; 81 81 - uint8_t reserved; 82 82 - uint8_t seq_no; 141 141 + uint8_t unknown1[2]; // Always 0x0001 83 142 uint8_t buttons[3]; 143 143 + uint8_t unknown2; // Always 0x00 144 144 + struct pssense_i32_le seq_no; 145 145 + struct pssense_i16_le gyro[3]; 146 146 + struct pssense_i16_le accel[3]; 147 147 + uint8_t unknown3[3]; 148 148 + uint8_t unknown4; // Increments occasionally 149 149 + uint8_t battery_level; // Range appears to be 0x00-0x0e 150 150 + uint8_t unknown5[10]; 151 151 + uint8_t charging_state; // 0x00 when unplugged, 0x20 when charging 152 152 + uint8_t unknown6[29]; 153 153 + uint8_t crc[4]; 84 154 }; 85 155 86 156 /*! ··· 88 158 */ 89 159 struct pssense_input_state 90 160 { 91 91 - uint64_t timestamp; 92 92 - uint8_t seq_no; 161 161 + uint64_t timestamp_ns; 162 162 + uint32_t seq_no; 93 163 94 164 bool ps_click; 95 165 bool share_click; ··· 112 182 bool thumbstick_click; 113 183 bool thumbstick_touch; 114 184 struct xrt_vec2 thumbstick; 185 185 + 186 186 + struct xrt_vec3_i32 gyro_raw; 187 187 + struct xrt_vec3_i32 accel_raw; 115 188 }; 116 189 117 190 /*! ··· 138 211 //! Input state parsed from most recent packet 139 212 struct pssense_input_state state; 140 213 214 214 + struct m_imu_3dof fusion; 215 215 + struct xrt_pose pose; 216 216 + 141 217 struct 142 218 { 143 219 bool button_states; 220 220 + bool tracking; 144 221 } gui; 145 222 }; 146 223 224 224 + static uint32_t 225 225 + pssense_i32_le_to_u32(const struct pssense_i32_le *from) 226 226 + { 227 227 + return (uint32_t)(from->lowest | from->lower << 8 | from->higher << 16 | from->highest << 24); 228 228 + } 229 229 + 230 230 + static int16_t 231 231 + pssense_i16_le_to_i16(const struct pssense_i16_le *from) 232 232 + { 233 233 + // The cast is important, sign extend properly. 234 234 + return (int16_t)(from->low | from->high << 8); 235 235 + } 236 236 + 147 237 /*! 148 238 * Reads one packet from the device, handles time out, locking and checking if 149 239 * the thread has been told to shut down. 150 240 */ 151 241 static bool 152 152 - pssense_read_one_packet(struct pssense_device *pssense, uint8_t *buffer, size_t size) 242 242 + pssense_read_one_packet(struct pssense_device *pssense, uint8_t *buffer, size_t size, bool check_size) 153 243 { 154 244 os_thread_helper_lock(&pssense->controller_thread); 155 245 ··· 169 259 PSSENSE_ERROR(pssense, "Failed to read device '%i'!", ret); 170 260 return false; 171 261 } 172 172 - if (ret != (int)size) { 262 262 + // Skip this check if we haven't flushed all the compat mode packets yet, since they're shorter. 263 263 + if (check_size && ret != (int)size) { 173 264 PSSENSE_ERROR(pssense, "Unexpected HID packet size %i (expected %zu)", ret, size); 174 265 return false; 175 266 } ··· 180 271 return false; 181 272 } 182 273 183 183 - static void 274 274 + static bool 184 275 pssense_parse_packet(struct pssense_device *pssense, 185 276 struct pssense_data_packet *data, 186 277 struct pssense_input_state *input) 187 278 { 188 188 - input->timestamp = os_monotonic_get_ns(); 189 189 - input->seq_no = data->seq_no; 279 279 + if (data->report_id != HID_PACKET_REPORT_ID) { 280 280 + PSSENSE_WARN(pssense, "Unrecognized HID report id %u", data->report_id); 281 281 + return false; 282 282 + } 283 283 + 284 284 + input->timestamp_ns = os_monotonic_get_ns(); 285 285 + 286 286 + uint32_t seq_no = pssense_i32_le_to_u32(&data->seq_no); 287 287 + if (input->seq_no != 0 && seq_no != input->seq_no + 1) { 288 288 + PSSENSE_WARN(pssense, "Missed seq no %u. Previous was %u", seq_no, input->seq_no); 289 289 + } 290 290 + input->seq_no = seq_no; 190 291 191 292 input->ps_click = (data->buttons[1] & 16) != 0; 192 293 input->squeeze_touch = (data->buttons[2] & 8) != 0; ··· 217 318 input->trigger_click = (data->buttons[0] & 128) != 0; 218 319 input->thumbstick_click = (data->buttons[1] & 8) != 0; 219 320 } 321 321 + 322 322 + input->gyro_raw.x = pssense_i16_le_to_i16(&data->gyro[0]); 323 323 + input->gyro_raw.y = pssense_i16_le_to_i16(&data->gyro[1]); 324 324 + input->gyro_raw.z = pssense_i16_le_to_i16(&data->gyro[2]); 325 325 + 326 326 + input->accel_raw.x = pssense_i16_le_to_i16(&data->accel[0]); 327 327 + input->accel_raw.y = pssense_i16_le_to_i16(&data->accel[1]); 328 328 + input->accel_raw.z = pssense_i16_le_to_i16(&data->accel[2]); 329 329 + 330 330 + return true; 331 331 + } 332 332 + 333 333 + static void 334 334 + pssense_update_fusion(struct pssense_device *pssense) 335 335 + { 336 336 + struct xrt_vec3 gyro; 337 337 + gyro.x = DEG_TO_RAD(pssense->state.gyro_raw.x * PSSENSE_GYRO_SCALE_DEG); 338 338 + gyro.y = DEG_TO_RAD(pssense->state.gyro_raw.y * PSSENSE_GYRO_SCALE_DEG); 339 339 + gyro.z = DEG_TO_RAD(pssense->state.gyro_raw.z * PSSENSE_GYRO_SCALE_DEG); 340 340 + 341 341 + struct xrt_vec3 accel; 342 342 + accel.x = pssense->state.accel_raw.x * PSSENSE_ACCEL_SCALE; 343 343 + accel.y = pssense->state.accel_raw.y * PSSENSE_ACCEL_SCALE; 344 344 + accel.z = pssense->state.accel_raw.z * PSSENSE_ACCEL_SCALE; 345 345 + 346 346 + // TODO: Apply correction from calibration data 347 347 + 348 348 + m_imu_3dof_update(&pssense->fusion, pssense->state.timestamp_ns, &accel, &gyro); 349 349 + pssense->pose.orientation = pssense->fusion.rot; 220 350 } 221 351 222 352 static void * ··· 228 358 229 359 union { 230 360 uint8_t buffer[sizeof(struct pssense_data_packet)]; 231 231 - struct pssense_data_packet input; 361 361 + struct pssense_data_packet packet; 232 362 } data; 233 233 - struct pssense_input_state input = {0}; 363 363 + struct pssense_input_state input_state = {0}; 234 364 235 235 - while (os_hid_read(pssense->hid, data.buffer, sizeof(data), 0) > 0) { 236 236 - // Empty queue first 365 365 + // The Sense controller starts in compat mode with a different HID report ID and format. 366 366 + // We need to discard packets until we get a correct report. 367 367 + while (pssense_read_one_packet(pssense, data.buffer, sizeof(data), false) && 368 368 + data.packet.report_id != HID_PACKET_REPORT_ID) { 369 369 + PSSENSE_DEBUG(pssense, "Discarding compat mode HID report"); 237 370 } 238 371 239 239 - // Now wait for a package to sync up, it's discarded but that's okay. 240 240 - if (!pssense_read_one_packet(pssense, data.buffer, sizeof(data))) { 241 241 - return NULL; 242 242 - } 243 243 - 244 244 - while (pssense_read_one_packet(pssense, data.buffer, sizeof(data))) { 245 245 - pssense_parse_packet(pssense, (struct pssense_data_packet *)data.buffer, &input); 246 246 - os_mutex_lock(&pssense->lock); 247 247 - pssense->state = input; 248 248 - os_mutex_unlock(&pssense->lock); 372 372 + while (pssense_read_one_packet(pssense, data.buffer, sizeof(data), true)) { 373 373 + if (pssense_parse_packet(pssense, &data.packet, &input_state)) { 374 374 + os_mutex_lock(&pssense->lock); 375 375 + pssense->state = input_state; 376 376 + pssense_update_fusion(pssense); 377 377 + os_mutex_unlock(&pssense->lock); 378 378 + } 249 379 } 250 380 251 381 return NULL; ··· 262 392 // Now that the thread is not running we can destroy the lock. 263 393 os_mutex_destroy(&pssense->lock); 264 394 395 395 + m_imu_3dof_close(&pssense->fusion); 396 396 + 265 397 // Remove the variable tracking. 266 398 u_var_remove_root(pssense); 267 399 ··· 282 414 os_mutex_lock(&pssense->lock); 283 415 284 416 for (uint i = 0; i < (uint)sizeof(enum pssense_input_index); i++) { 285 285 - pssense->base.inputs[i].timestamp = (int64_t)pssense->state.timestamp; 417 417 + pssense->base.inputs[i].timestamp = (int64_t)pssense->state.timestamp_ns; 286 418 } 287 419 pssense->base.inputs[PSSENSE_INDEX_PS_CLICK].value.boolean = pssense->state.ps_click; 288 420 pssense->base.inputs[PSSENSE_INDEX_SHARE_CLICK].value.boolean = pssense->state.share_click; ··· 310 442 os_mutex_unlock(&pssense->lock); 311 443 } 312 444 445 445 + static void 446 446 + pssense_get_fusion_pose(struct pssense_device *pssense, 447 447 + enum xrt_input_name name, 448 448 + uint64_t at_timestamp_ns, 449 449 + struct xrt_space_relation *out_relation) 450 450 + { 451 451 + out_relation->pose = pssense->pose; 452 452 + out_relation->linear_velocity.x = 0.0f; 453 453 + out_relation->linear_velocity.y = 0.0f; 454 454 + out_relation->linear_velocity.z = 0.0f; 455 455 + 456 456 + /*! 457 457 + * @todo This is hack, fusion reports angvel relative to the device but 458 458 + * it needs to be in relation to the base space. Rotating it with the 459 459 + * device orientation is enough to get it into the right space, angular 460 460 + * velocity is a derivative so needs a special rotation. 461 461 + */ 462 462 + math_quat_rotate_derivative(&pssense->pose.orientation, &pssense->fusion.last.gyro, 463 463 + &out_relation->angular_velocity); 464 464 + 465 465 + out_relation->relation_flags = (enum xrt_space_relation_flags)( 466 466 + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | 467 467 + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 468 468 + } 469 469 + 470 470 + static void 471 471 + pssense_get_tracked_pose(struct xrt_device *xdev, 472 472 + enum xrt_input_name name, 473 473 + uint64_t at_timestamp_ns, 474 474 + struct xrt_space_relation *out_relation) 475 475 + { 476 476 + struct pssense_device *pssense = (struct pssense_device *)xdev; 477 477 + 478 478 + if (name != XRT_INPUT_PSSENSE_AIM_POSE && name != XRT_INPUT_PSSENSE_GRIP_POSE) { 479 479 + PSSENSE_ERROR(pssense, "Unknown pose name requested %u", name); 480 480 + return; 481 481 + } 482 482 + 483 483 + struct xrt_relation_chain xrc = {0}; 484 484 + struct xrt_pose pose_correction = {0}; 485 485 + 486 486 + // Rotate the grip/aim pose up by 60 degrees around the X axis 487 487 + struct xrt_vec3 axis = {1.0, 0, 0}; 488 488 + math_quat_from_angle_vector(DEG_TO_RAD(60), &axis, &pose_correction.orientation); 489 489 + m_relation_chain_push_pose(&xrc, &pose_correction); 490 490 + 491 491 + struct xrt_space_relation *rel = m_relation_chain_reserve(&xrc); 492 492 + 493 493 + os_mutex_lock(&pssense->lock); 494 494 + pssense_get_fusion_pose(pssense, name, at_timestamp_ns, rel); 495 495 + os_mutex_unlock(&pssense->lock); 496 496 + 497 497 + m_relation_chain_resolve(&xrc, out_relation); 498 498 + } 499 499 + 500 500 + /** 501 501 + * Retrieving the calibration data report will switch the Sense controller from compat mode into full mode. 502 502 + */ 503 503 + bool 504 504 + pssense_get_calibration_data(struct pssense_device *pssense) 505 505 + { 506 506 + int ret; 507 507 + uint8_t buffer[64]; 508 508 + uint8_t data[(sizeof(buffer) - 2) * 2]; 509 509 + for (int i = 0; i < 2; i++) { 510 510 + ret = os_hid_get_feature(pssense->hid, CALIBRATION_DATA_FEATURE_REPORT_ID, buffer, sizeof(buffer)); 511 511 + if (ret < 0) { 512 512 + PSSENSE_ERROR(pssense, "Failed to retrieve calibration report: %d", ret); 513 513 + return false; 514 514 + } 515 515 + if (ret != sizeof(buffer)) { 516 516 + PSSENSE_ERROR(pssense, "Invalid byte count transferred, expected %zu got %d\n", sizeof(buffer), 517 517 + ret); 518 518 + return false; 519 519 + } 520 520 + if (buffer[1] == CALIBRATION_DATA_PART_ID_1) { 521 521 + memcpy(data, buffer + 2, sizeof(buffer) - 2); 522 522 + } else if (buffer[1] == CALIBRATION_DATA_PART_ID_2) { 523 523 + memcpy(data + sizeof(buffer) - 2, buffer + 2, sizeof(buffer) - 2); 524 524 + } else { 525 525 + PSSENSE_ERROR(pssense, "Unknown calibration data part ID %u", buffer[1]); 526 526 + return false; 527 527 + } 528 528 + } 529 529 + 530 530 + // TODO: Parse calibration data into prefiler 531 531 + 532 532 + return true; 533 533 + } 534 534 + 313 535 #define SET_INPUT(NAME) (pssense->base.inputs[PSSENSE_INDEX_##NAME].name = XRT_INPUT_PSSENSE_##NAME) 314 536 315 537 int ··· 335 557 XRT_PROBER_STRING_PRODUCT, // 336 558 product_name, // 337 559 sizeof(product_name)); // 338 338 - if (ret != 0) { 560 560 + if (ret <= 0) { 339 561 U_LOG_E("Failed to get product name from Bluetooth device!"); 340 562 return -1; 341 563 } 342 564 343 565 enum u_device_alloc_flags flags = U_DEVICE_ALLOC_TRACKING_NONE; 344 344 - struct pssense_device *pssense = U_DEVICE_ALLOCATE(struct pssense_device, flags, 13, 1); 345 345 - 566 566 + struct pssense_device *pssense = U_DEVICE_ALLOCATE(struct pssense_device, flags, 23, 0); 346 567 PSSENSE_DEBUG(pssense, "PlayStation Sense controller found"); 347 347 - pssense->base.destroy = pssense_device_destroy; 348 348 - pssense->base.update_inputs = pssense_device_update_inputs; 568 568 + 349 569 pssense->base.name = XRT_DEVICE_PSSENSE; 350 570 snprintf(pssense->base.str, XRT_DEVICE_NAME_LEN, "%s", product_name); 571 571 + pssense->base.update_inputs = pssense_device_update_inputs; 572 572 + pssense->base.get_tracked_pose = pssense_get_tracked_pose; 573 573 + pssense->base.destroy = pssense_device_destroy; 574 574 + pssense->base.orientation_tracking_supported = true; 575 575 + 576 576 + pssense->base.binding_profiles = binding_profiles_pssense; 577 577 + pssense->base.binding_profile_count = ARRAY_SIZE(binding_profiles_pssense); 578 578 + 579 579 + m_imu_3dof_init(&pssense->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); 351 580 352 581 pssense->log_level = debug_get_log_option_pssense_log(); 353 582 pssense->hid = hid; ··· 385 614 SET_INPUT(THUMBSTICK); 386 615 SET_INPUT(THUMBSTICK_CLICK); 387 616 SET_INPUT(THUMBSTICK_TOUCH); 617 617 + SET_INPUT(GRIP_POSE); 618 618 + SET_INPUT(AIM_POSE); 388 619 389 620 ret = os_mutex_init(&pssense->lock); 390 621 if (ret != 0) { ··· 407 638 return -1; 408 639 } 409 640 641 641 + if (!pssense_get_calibration_data(pssense)) { 642 642 + PSSENSE_ERROR(pssense, "Failed to retrieve calibration data"); 643 643 + pssense_device_destroy(&pssense->base); 644 644 + return -1; 645 645 + } 646 646 + 410 647 u_var_add_root(pssense, pssense->base.str, false); 411 648 u_var_add_log_level(pssense, &pssense->log_level, "Log level"); 412 649 ··· 436 673 u_var_add_ro_f32(pssense, &pssense->state.thumbstick.y, "Thumbstick Y"); 437 674 u_var_add_bool(pssense, &pssense->state.thumbstick_click, "Thumbstick Click"); 438 675 u_var_add_bool(pssense, &pssense->state.thumbstick_touch, "Thumbstick Touch"); 676 676 + 677 677 + u_var_add_gui_header(pssense, &pssense->gui.tracking, "Tracking"); 678 678 + u_var_add_ro_vec3_i32(pssense, &pssense->state.gyro_raw, "Raw Gyro"); 679 679 + u_var_add_ro_vec3_i32(pssense, &pssense->state.accel_raw, "Raw Accel"); 680 680 + u_var_add_pose(pssense, &pssense->pose, "Pose"); 439 681 440 682 out_xdevs[0] = &pssense->base; 441 683 return 1;

+1

src/xrt/drivers/pssense/pssense_interface.h

··· 1 1 // Copyright 2023, Collabora, Ltd. 2 2 + // Copyright 2023, Jarett Millard 2 3 // SPDX-License-Identifier: BSL-1.0 3 4 /*! 4 5 * @file

+2

src/xrt/include/xrt/xrt_defines.h

··· 946 946 XRT_INPUT_PSSENSE_THUMBSTICK = XRT_INPUT_NAME(0x0312, VEC2_MINUS_ONE_TO_ONE), 947 947 XRT_INPUT_PSSENSE_THUMBSTICK_CLICK = XRT_INPUT_NAME(0x0313, BOOLEAN), 948 948 XRT_INPUT_PSSENSE_THUMBSTICK_TOUCH = XRT_INPUT_NAME(0x0314, BOOLEAN), 949 949 + XRT_INPUT_PSSENSE_GRIP_POSE = XRT_INPUT_NAME(0x0315, POSE), 950 950 + XRT_INPUT_PSSENSE_AIM_POSE = XRT_INPUT_NAME(0x0316, POSE), 949 951 // clang-format on 950 952 }; 951 953