d/rift: Basic 3dof · matrixfurry.com/monado@0714e01

+230 -44

2 changed files

expand all

src

xrt

drivers

rift

rift_hmd.c

rift_interface.h

+191 -44

src/xrt/drivers/rift/rift_hmd.c

··· 30 30 #include "util/u_time.h" 31 31 #include "util/u_var.h" 32 32 #include "util/u_visibility_mask.h" 33 + #include "util/u_trace_marker.h" 33 34 #include "xrt/xrt_results.h" 34 35 35 36 #include <stdio.h> ··· 53 54 #define HMD_TRACE(hmd, ...) U_LOG_XDEV_IFL_T(&hmd->base, hmd->log_level, __VA_ARGS__) 54 55 #define HMD_DEBUG(hmd, ...) U_LOG_XDEV_IFL_D(&hmd->base, hmd->log_level, __VA_ARGS__) 55 56 #define HMD_INFO(hmd, ...) U_LOG_XDEV_IFL_I(&hmd->base, hmd->log_level, __VA_ARGS__) 57 + #define HMD_WARN(hmd, ...) U_LOG_XDEV_IFL_W(&hmd->base, hmd->log_level, __VA_ARGS__) 56 58 #define HMD_ERROR(hmd, ...) U_LOG_XDEV_IFL_E(&hmd->base, hmd->log_level, __VA_ARGS__) 57 59 58 60 /* ··· 64 66 static int 65 67 rift_send_report(struct rift_hmd *hmd, uint8_t report_id, void *data, size_t data_length) 66 68 { 67 - #define REPORT_WRITE_DATA(ptr, ptr_len) \ 68 - if (ptr_len > sizeof(buffer) - length) { \ 69 - HMD_ERROR(hmd, "Tried to fit %ld bytes into buffer with only %ld bytes left", ptr_len, \ 70 - sizeof(buffer) - length); \ 71 - return -1; \ 72 - } \ 73 - memcpy(buffer + length, ptr, ptr_len); \ 74 - length += ptr_len; 69 + int result; 75 70 76 - #define REPORT_WRITE_VALUE(value) REPORT_WRITE_DATA(&value, sizeof(value)); 77 - 78 - int result; 71 + if (data_length > REPORT_MAX_SIZE - 1) { 72 + return -1; 73 + } 79 74 80 75 uint8_t buffer[REPORT_MAX_SIZE]; 81 - size_t length = 0; 76 + buffer[0] = report_id; 77 + memcpy(buffer + 1, data, data_length); 82 78 83 - REPORT_WRITE_VALUE(report_id) 84 - REPORT_WRITE_DATA(data, data_length) 85 - 86 - result = os_hid_set_feature(hmd->hid_dev, buffer, length); 79 + result = os_hid_set_feature(hmd->hid_dev, buffer, data_length + 1); 87 80 if (result < 0) { 88 81 return result; 89 82 } ··· 100 93 static int 101 94 rift_send_keepalive(struct rift_hmd *hmd) 102 95 { 103 - struct dk2_report_keepalive_mux report = {0, IN_REPORT_DK2, 10000}; 96 + struct dk2_report_keepalive_mux report = {0, IN_REPORT_DK2, 97 + KEEPALIVE_INTERVAL_NS / 1000000}; // convert ns to ms 104 98 105 99 int result = 106 - rift_send_report(hmd, FEATURE_REPORT_KEEPALIVE_MUX, &report, sizeof(struct dk2_report_keepalive_mux)); 100 + rift_send_report(hmd, FEATURE_REPORT_KEEPALIVE_MUX, &report, sizeof(report)); 107 101 108 102 if (result < 0) { 109 103 return result; 110 104 } 111 105 112 106 hmd->last_keepalive_time = os_monotonic_get_ns(); 107 + HMD_TRACE(hmd, "Sent keepalive at time %ld", hmd->last_keepalive_time); 113 108 114 109 return 0; 115 110 } ··· 128 123 memcpy(config, buf + 1, sizeof(*config)); 129 124 130 125 // this value is hardcoded in the DK1 and DK2 firmware 131 - if ((hmd->variant == RIFT_VARIANT_DK1 || hmd->variant == RIFT_VARIANT_DK2) && config->sample_rate != 1000) { 126 + if ((hmd->variant == RIFT_VARIANT_DK1 || hmd->variant == RIFT_VARIANT_DK2) && 127 + config->sample_rate != IMU_SAMPLE_RATE) { 132 128 HMD_ERROR(hmd, "Got invalid config from headset, got sample rate %d when expected %d", 133 - config->sample_rate, 1000); 129 + config->sample_rate, IMU_SAMPLE_RATE); 134 130 return -1; 135 131 } 136 132 ··· 171 167 static int 172 168 rift_set_config(struct rift_hmd *hmd, struct rift_config_report *config) 173 169 { 174 - return rift_send_report(hmd, FEATURE_REPORT_CONFIG, &config, sizeof(*config)); 170 + return rift_send_report(hmd, FEATURE_REPORT_CONFIG, config, sizeof(*config)); 175 171 } 176 172 177 173 /* ··· 187 183 188 184 // Remove the variable tracking. 189 185 u_var_remove_root(hmd); 186 + 187 + if (hmd->sensor_thread.initialized) 188 + os_thread_helper_stop_and_wait(&hmd->sensor_thread); 190 189 191 190 m_relation_history_destroy(&hmd->relation_hist); 192 191 ··· 422 421 } 423 422 } 424 423 424 + /* 425 + * Decode 3 tightly packed 21 bit values from 4 bytes. 426 + * We unpack them in the higher 21 bit values first and then shift 427 + * them down to the lower in order to get the sign bits correct. 428 + * 429 + * "Inspired" (code copied verbatim) from OpenHMD's rift driver 430 + */ 431 + static void 432 + rift_decode_sample(const uint8_t *in, int32_t *out) 433 + { 434 + int x = (in[0] << 24) | (in[1] << 16) | ((in[2] & 0xF8) << 8); 435 + int y = ((in[2] & 0x07) << 29) | (in[3] << 21) | (in[4] << 13) | ((in[5] & 0xC0) << 5); 436 + int z = ((in[5] & 0x3F) << 26) | (in[6] << 18) | (in[7] << 10); 437 + 438 + out[0] = x >> 11; 439 + out[1] = y >> 11; 440 + out[2] = z >> 11; 441 + } 442 + 443 + static void 444 + rift_sample_to_imu_space(const int32_t *in, struct xrt_vec3 *out) 445 + { 446 + out->x = (float)in[0] * 0.0001f; 447 + out->y = (float)in[1] * 0.0001f; 448 + out->z = (float)in[2] * 0.0001f; 449 + } 450 + 451 + static int 452 + sensor_thread_tick(struct rift_hmd *hmd) 453 + { 454 + uint8_t buf[REPORT_MAX_SIZE]; 455 + int result; 456 + 457 + int64_t now = os_monotonic_get_ns(); 458 + 459 + if (now - hmd->last_keepalive_time > KEEPALIVE_SEND_RATE_NS) { 460 + result = rift_send_keepalive(hmd); 461 + 462 + if (result < 0) { 463 + HMD_ERROR(hmd, "Got error sending keepalive, assuming fatal, reason %d", result); 464 + return result; 465 + } 466 + } 467 + 468 + result = os_hid_read(hmd->hid_dev, buf, sizeof(buf), IMU_SAMPLE_RATE); 469 + 470 + if (result < 0) { 471 + HMD_ERROR(hmd, "Got error reading from device, assuming fatal, reason %d", result); 472 + return result; 473 + } 474 + 475 + if (result == 0) { 476 + HMD_WARN(hmd, "Timed out waiting for packet from headset, packet should come in every %d milliseconds", 477 + IMU_SAMPLE_RATE); 478 + return 0; 479 + } 480 + 481 + switch (hmd->variant) { 482 + case RIFT_VARIANT_DK2: { 483 + // skip unknown commands 484 + if (buf[0] != IN_REPORT_DK2) { 485 + HMD_WARN(hmd, "Skipping unknown IN command %d", buf[0]); 486 + return 0; 487 + } 488 + 489 + struct dk2_in_report report; 490 + assert(result >= (int)sizeof(report)); 491 + 492 + memcpy(&report, buf + 1, sizeof(report)); 493 + 494 + // if there's no samples, just do nothing. 495 + if (report.num_samples == 0) { 496 + return 0; 497 + } 498 + 499 + struct dk2_sample_pack sample_pack = report.samples[report.num_samples >= 2 ? 1 : 0]; 500 + 501 + int32_t accel_raw[3], gyro_raw[3]; 502 + rift_decode_sample(sample_pack.accel.data, accel_raw); 503 + rift_decode_sample(sample_pack.gyro.data, gyro_raw); 504 + 505 + struct xrt_vec3 accel, gyro; 506 + rift_sample_to_imu_space(accel_raw, &accel); 507 + rift_sample_to_imu_space(gyro_raw, &gyro); 508 + 509 + uint64_t sample_timestamp_ns = (uint64_t)report.sample_timestamp * 1000; 510 + 511 + HMD_INFO(hmd, "sample timestamp: %ld", sample_timestamp_ns); 512 + m_imu_3dof_update(&hmd->fusion, sample_timestamp_ns, &accel, &gyro); 513 + 514 + struct xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; 515 + relation.relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | 516 + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); 517 + relation.pose.orientation = hmd->fusion.rot; 518 + m_relation_history_push(hmd->relation_hist, &relation, os_monotonic_get_ns()); 519 + 520 + break; 521 + } 522 + case RIFT_VARIANT_DK1: return 0; 523 + } 524 + 525 + return 0; 526 + } 527 + 528 + static void * 529 + sensor_thread(void *ptr) 530 + { 531 + U_TRACE_SET_THREAD_NAME("Rift sensor thread"); 532 + 533 + struct rift_hmd *hmd = (struct rift_hmd *)ptr; 534 + 535 + os_thread_helper_lock(&hmd->sensor_thread); 536 + 537 + int result = 0; 538 + int ticks = 0; 539 + 540 + while (os_thread_helper_is_running_locked(&hmd->sensor_thread) && result >= 0) { 541 + os_thread_helper_unlock(&hmd->sensor_thread); 542 + 543 + result = sensor_thread_tick(hmd); 544 + 545 + os_thread_helper_lock(&hmd->sensor_thread); 546 + ticks += 1; 547 + } 548 + 549 + os_thread_helper_unlock(&hmd->sensor_thread); 550 + 551 + return NULL; 552 + } 553 + 425 554 struct rift_hmd * 426 555 rift_hmd_create(struct os_hid_device *dev, enum rift_variant variant, char *device_name, char *serial_number) 427 556 { ··· 439 568 result = rift_send_keepalive(hmd); 440 569 if (result < 0) { 441 570 HMD_ERROR(hmd, "Failed to send keepalive to spin up headset, reason %d", result); 442 - u_device_free(&hmd->base); 443 - return NULL; 571 + goto error; 444 572 } 445 573 446 - result = rift_get_config(hmd, &hmd->config); 574 + result = rift_get_display_info(hmd, &hmd->display_info); 447 575 if (result < 0) { 448 576 HMD_ERROR(hmd, "Failed to get device config, reason %d", result); 449 - u_device_free(&hmd->base); 450 - return NULL; 577 + goto error; 451 578 } 452 - HMD_INFO(hmd, "Got config from hmd, config flags: %X", hmd->config.config_flags); 579 + HMD_INFO(hmd, "Got display info from hmd, res: %dx%d", hmd->display_info.resolution_x, 580 + hmd->display_info.resolution_y); 453 581 454 - result = rift_get_display_info(hmd, &hmd->display_info); 582 + result = rift_get_config(hmd, &hmd->config); 455 583 if (result < 0) { 456 584 HMD_ERROR(hmd, "Failed to get device config, reason %d", result); 457 - u_device_free(&hmd->base); 458 - return NULL; 585 + goto error; 459 586 } 460 - HMD_INFO(hmd, "Got display info from hmd, res: %dx%d", hmd->display_info.resolution_x, 461 - hmd->display_info.resolution_y); 587 + HMD_INFO(hmd, "Got config from hmd, config flags: %X", hmd->config.config_flags); 462 588 463 589 if (getenv("RIFT_POWER_OVERRIDE") != NULL) { 464 590 hmd->config.config_flags |= RIFT_CONFIG_REPORT_OVERRIDE_POWER; ··· 470 596 // but OpenHMD forces them on, we should do the same, they probably had a reason 471 597 hmd->config.config_flags |= RIFT_CONFIG_REPORT_USE_CALIBRATION; 472 598 hmd->config.config_flags |= RIFT_CONFIG_REPORT_AUTO_CALIBRATION; 599 + 600 + hmd->config.interval = 0; 473 601 474 602 // update the config 475 603 result = rift_set_config(hmd, &hmd->config); 476 604 if (result < 0) { 477 605 HMD_ERROR(hmd, "Failed to set the device config, reason %d", result); 478 - u_device_free(&hmd->base); 479 - return NULL; 606 + goto error; 480 607 } 481 608 609 + // read it back 610 + result = rift_get_config(hmd, &hmd->config); 611 + if (result < 0) { 612 + HMD_ERROR(hmd, "Failed to set the device config, reason %d", result); 613 + goto error; 614 + } 615 + HMD_INFO(hmd, "After writing, HMD has config flags: %X", hmd->config.config_flags); 616 + 482 617 // get the lens distortions 483 618 struct rift_lens_distortion_report lens_distortion; 484 619 result = rift_get_lens_distortion(hmd, &lens_distortion); 485 620 if (result < 0) { 486 621 HMD_ERROR(hmd, "Failed to get lens distortion, reason %d", result); 487 - u_device_free(&hmd->base); 488 - return NULL; 622 + goto error; 489 623 } 490 624 491 625 hmd->num_lens_distortions = lens_distortion.num_distortions; ··· 498 632 result = rift_get_lens_distortion(hmd, &lens_distortion); 499 633 if (result < 0) { 500 634 HMD_ERROR(hmd, "Failed to get lens distortion idx %d, reason %d", i, result); 501 - u_device_free(&hmd->base); 502 - return NULL; 635 + goto error; 503 636 } 504 637 505 638 rift_parse_distortion_report(&lens_distortion, &hmd->lens_distortions[lens_distortion.distortion_idx]); ··· 516 649 hmd->base.get_visibility_mask = rift_hmd_get_visibility_mask; 517 650 hmd->base.destroy = rift_hmd_destroy; 518 651 519 - // Distortion information, fills in xdev->compute_distortion(). 520 - // u_distortion_mesh_set_none(&hmd->base); 521 - 522 - // populate this with something more complex if required 523 652 hmd->base.compute_distortion = rift_hmd_compute_distortion; 524 653 525 654 hmd->pose = (struct xrt_pose)XRT_POSE_IDENTITY; ··· 560 689 561 690 if (!u_device_setup_split_side_by_side(&hmd->base, &info)) { 562 691 HMD_ERROR(hmd, "Failed to setup basic device info"); 563 - rift_hmd_destroy(&hmd->base); 564 - return NULL; 692 + goto error; 565 693 } 566 694 567 695 // Just put an initial identity value in the tracker ··· 571 699 uint64_t now = os_monotonic_get_ns(); 572 700 m_relation_history_push(hmd->relation_hist, &identity, now); 573 701 702 + result = os_thread_helper_init(&hmd->sensor_thread); 703 + 704 + if (result < 0) { 705 + HMD_ERROR(hmd, "Failed to init os thread helper"); 706 + goto error; 707 + } 708 + 709 + m_imu_3dof_init(&hmd->fusion, 0); 710 + 711 + result = os_thread_helper_start(&hmd->sensor_thread, sensor_thread, hmd); 712 + 713 + if (result < 0) { 714 + HMD_ERROR(hmd, "Failed to start sensor thread"); 715 + goto error; 716 + } 717 + 574 718 // Setup variable tracker: Optional but useful for debugging 575 719 u_var_add_root(hmd, "Rift HMD", true); 576 720 u_var_add_log_level(hmd, &hmd->log_level, "log_level"); 577 721 578 722 return hmd; 723 + error: 724 + rift_hmd_destroy(&hmd->base); 725 + return NULL; 579 726 }

+39

src/xrt/drivers/rift/rift_interface.h

··· 14 14 #include "xrt/xrt_prober.h" 15 15 16 16 #include "os/os_hid.h" 17 + #include "os/os_threading.h" 17 18 18 19 #include "util/u_device.h" 19 20 #include "util/u_logging.h" 21 + 22 + #include "math/m_imu_3dof.h" 20 23 21 24 #include <stdlib.h> 22 25 #include <stdio.h> ··· 26 29 #endif 27 30 28 31 #define REPORT_MAX_SIZE 69 // max size of a feature report (FEATURE_REPORT_CALIBRATE) 32 + #define KEEPALIVE_INTERVAL_NS 10000000000 // 10 seconds 33 + #define KEEPALIVE_SEND_RATE_NS (KEEPALIVE_INTERVAL_NS * 19) / 20 // give a 5% breathing room (at 10 seconds, this is 500 milliseconds of breathing room) 34 + #define IMU_SAMPLE_RATE 1000 29 35 30 36 enum rift_feature_reports 31 37 { ··· 120 126 { 121 127 uint16_t command_id; 122 128 uint8_t config_flags; 129 + // the IN report rate of the headset, rate is calculated as `sample_rate / (1 + interval)` 123 130 uint8_t interval; 124 131 // sample rate of the IMU, always 1000hz on DK1/DK2, read-only 125 132 uint16_t sample_rate; ··· 185 192 uint16_t interval; 186 193 } RIFT_PACKED; 187 194 195 + struct dk2_sensor_sample 196 + { 197 + uint8_t data[8]; 198 + } RIFT_PACKED; 199 + 200 + struct dk2_sample_pack 201 + { 202 + struct dk2_sensor_sample accel; 203 + struct dk2_sensor_sample gyro; 204 + } RIFT_PACKED; 205 + 206 + struct dk2_in_report 207 + { 208 + uint16_t command_id; 209 + uint8_t num_samples; 210 + uint16_t sample_count; 211 + uint16_t temperature; 212 + uint32_t sample_timestamp; 213 + struct dk2_sample_pack samples[2]; 214 + int16_t mag_x; 215 + int16_t mag_y; 216 + int16_t mag_z; 217 + uint16_t frame_count; 218 + uint32_t frame_timestamp; 219 + uint8_t frame_id; 220 + uint8_t tracking_pattern; 221 + uint16_t tracking_count; 222 + uint32_t tracking_timestamp; 223 + } RIFT_PACKED; 224 + 188 225 #if defined(_MSC_VER) 189 226 #pragma pack(pop) 190 227 #endif ··· 249 286 struct m_relation_history *relation_hist; 250 287 251 288 struct os_hid_device *hid_dev; 289 + struct os_thread_helper sensor_thread; 290 + struct m_imu_3dof fusion; 252 291 253 292 int64_t last_keepalive_time; 254 293 enum rift_variant variant;

Configure Feed

Configure Feed