matrixfurry.com / monado
The open source OpenXR runtime
0
fork

Configure Feed

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

d/rift: Various tracking fixes

Handle remote clock wraparounds, dont drop samples, and tweak IMU fusion
settings.

Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2428>

Beyley Thomas 89e88f9a 6490046d

+59 -30
+50 -24
src/xrt/drivers/rift/rift_hmd.c
··· 348 348 } 349 349 350 350 if (result == 0) { 351 - HMD_WARN(hmd, "Timed out waiting for packet from headset, packet should come in every %d milliseconds", 351 + HMD_WARN(hmd, "Timed out waiting for packet from headset, packets should come in at %dhz", 352 352 IMU_SAMPLE_RATE); 353 353 return 0; 354 354 } ··· 362 362 } 363 363 364 364 struct dk2_in_report report; 365 - assert(result >= (int)sizeof(report)); 365 + 366 + // don't treat invalid IN reports as fatal, just ignore them 367 + if (result < (int)sizeof(report)) { 368 + HMD_WARN(hmd, "Got malformed DK2 IN report with size %d", result); 369 + return 0; 370 + } 366 371 372 + // TODO: handle endianness 367 373 memcpy(&report, buf + 1, sizeof(report)); 368 374 369 375 // if there's no samples, just do nothing. ··· 371 377 return 0; 372 378 } 373 379 374 - int64_t remote_sample_timestamp_ns = (int64_t)report.sample_timestamp * 1000; 375 - 376 - // ignore samples that are behind the latest current sample 377 - if (remote_sample_timestamp_ns < hmd->last_sample_time_ns) { 378 - return 0; 380 + if (!hmd->processed_sample_packet) { 381 + hmd->last_remote_sample_time_us = report.sample_timestamp; 382 + hmd->processed_sample_packet = true; 379 383 } 380 384 381 - hmd->last_sample_time_ns = remote_sample_timestamp_ns; 385 + // wrap-around intentional and A-OK, given these are unsigned 386 + uint32_t remote_sample_delta_us = report.sample_timestamp - hmd->last_remote_sample_time_us; 387 + 388 + hmd->last_remote_sample_time_us = report.sample_timestamp; 389 + 390 + hmd->last_remote_sample_time_ns += (int64_t)remote_sample_delta_us * OS_NS_PER_USEC; 382 391 383 392 m_clock_windowed_skew_tracker_push(hmd->clock_tracker, os_monotonic_get_ns(), 384 - remote_sample_timestamp_ns); 393 + hmd->last_remote_sample_time_ns); 385 394 386 395 int64_t local_timestamp_ns; 387 396 // if we haven't synchronized our clocks, just do nothing 388 - if (!m_clock_windowed_skew_tracker_to_local(hmd->clock_tracker, remote_sample_timestamp_ns, 397 + if (!m_clock_windowed_skew_tracker_to_local(hmd->clock_tracker, hmd->last_remote_sample_time_ns, 389 398 &local_timestamp_ns)) { 390 399 return 0; 391 400 } 392 401 393 - struct dk2_sample_pack sample_pack = report.samples[report.num_samples >= 2 ? 1 : 0]; 402 + if (report.num_samples > 1) 403 + HMD_TRACE(hmd, 404 + "Had more than one sample queued! We aren't receiving IN reports fast enough, HMD " 405 + "had %d samples in the queue! Having to work back that first sample...", 406 + report.num_samples); 407 + 408 + for (int i = 0; i < MIN(DK2_MAX_SAMPLES, report.num_samples); i++) { 409 + struct dk2_sample_pack latest_sample_pack = report.samples[i]; 394 410 395 - int32_t accel_raw[3], gyro_raw[3]; 396 - rift_decode_sample(sample_pack.accel.data, accel_raw); 397 - rift_decode_sample(sample_pack.gyro.data, gyro_raw); 411 + int32_t accel_raw[3], gyro_raw[3]; 412 + rift_decode_sample(latest_sample_pack.accel.data, accel_raw); 413 + rift_decode_sample(latest_sample_pack.gyro.data, gyro_raw); 398 414 399 - struct xrt_vec3 accel, gyro; 400 - rift_sample_to_imu_space(accel_raw, &accel); 401 - rift_sample_to_imu_space(gyro_raw, &gyro); 415 + struct xrt_vec3 accel, gyro; 416 + rift_sample_to_imu_space(accel_raw, &accel); 417 + rift_sample_to_imu_space(gyro_raw, &gyro); 418 + 419 + // work back the likely timestamp of the current sample 420 + // if there's only one sample, then this will always be zero, if there's two or more samples, 421 + // the previous samples will be offset by the sample rate of the IMU 422 + int64_t sample_local_timestamp_ns = 423 + local_timestamp_ns - ((MIN(report.num_samples, DK2_MAX_SAMPLES) - 1) * NS_PER_SAMPLE); 402 424 403 - m_imu_3dof_update(&hmd->fusion, local_timestamp_ns, &accel, &gyro); 425 + // update the IMU for that sample 426 + m_imu_3dof_update(&hmd->fusion, sample_local_timestamp_ns, &accel, &gyro); 404 427 405 - struct xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; 406 - relation.relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | 407 - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); 408 - relation.pose.orientation = hmd->fusion.rot; 409 - m_relation_history_push(hmd->relation_hist, &relation, local_timestamp_ns); 428 + // push the pose of the IMU for that sample, doing so per sample 429 + struct xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; 430 + relation.relation_flags = (enum xrt_space_relation_flags)( 431 + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); 432 + relation.pose.orientation = hmd->fusion.rot; 433 + m_relation_history_push(hmd->relation_hist, &relation, sample_local_timestamp_ns); 434 + } 410 435 411 436 break; 412 437 } ··· 662 687 goto error; 663 688 } 664 689 665 - m_imu_3dof_init(&hmd->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_300MS); 690 + m_imu_3dof_init(&hmd->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); 666 691 hmd->clock_tracker = m_clock_windowed_skew_tracker_alloc(64); 667 692 668 693 result = os_thread_helper_start(&hmd->sensor_thread, sensor_thread, hmd); ··· 675 700 // Setup variable tracker: Optional but useful for debugging 676 701 u_var_add_root(hmd, "Rift HMD", true); 677 702 u_var_add_log_level(hmd, &hmd->log_level, "log_level"); 703 + u_var_add_f32(hmd, &hmd->extra_display_info.icd, "ICD"); 678 704 m_imu_3dof_add_vars(&hmd->fusion, hmd, "3dof_"); 679 705 680 706 return hmd;
+9 -6
src/xrt/drivers/rift/rift_interface.h
··· 32 32 33 33 #define REPORT_MAX_SIZE 69 // max size of a feature report (FEATURE_REPORT_CALIBRATE) 34 34 #define KEEPALIVE_INTERVAL_NS 10000000000 // 10 seconds 35 - #define KEEPALIVE_SEND_RATE_NS \ 36 - (KEEPALIVE_INTERVAL_NS * 19) / \ 37 - 20 // give a 5% breathing room (at 10 seconds, this is 500 milliseconds of breathing room) 38 - #define IMU_SAMPLE_RATE 1000 35 + // give a 5% breathing room (at 10 seconds, this is 500 milliseconds of breathing room) 36 + #define KEEPALIVE_SEND_RATE_NS ((KEEPALIVE_INTERVAL_NS * 19) / 20) 37 + #define IMU_SAMPLE_RATE (1000) // 1000hz 38 + #define NS_PER_SAMPLE (1000 * 1000) // 1ms (1,000,000 ns) per sample 39 39 40 40 #define DEG_TO_RAD(DEG) (DEG * M_PI / 180.0) 41 41 #define MICROMETERS_TO_METERS(microns) (float)microns / 1000000.0f ··· 267 267 struct dk2_sensor_sample gyro; 268 268 } RIFT_PACKED; 269 269 270 + #define DK2_MAX_SAMPLES 2 270 271 struct dk2_in_report 271 272 { 272 273 uint16_t command_id; ··· 274 275 uint16_t sample_count; 275 276 uint16_t temperature; 276 277 uint32_t sample_timestamp; 277 - struct dk2_sample_pack samples[2]; 278 + struct dk2_sample_pack samples[DK2_MAX_SAMPLES]; 278 279 int16_t mag_x; 279 280 int16_t mag_y; 280 281 int16_t mag_z; ··· 383 384 384 385 struct os_hid_device *hid_dev; 385 386 struct os_thread_helper sensor_thread; 386 - int64_t last_sample_time_ns; 387 + bool processed_sample_packet; 388 + uint32_t last_remote_sample_time_us; 389 + int64_t last_remote_sample_time_ns; 387 390 388 391 struct m_imu_3dof fusion; 389 392 struct m_clock_windowed_skew_tracker *clock_tracker;