From f4dc0ec36e1109a997c8b388fb30de3cf1dc01b0 Mon Sep 17 00:00:00 2001 From: Beyley Cardellio Date: Wed, 25 Feb 2026 01:58:16 -0500 Subject: [PATCH] d/rift: Parse out LED model Part-of: --- src/xrt/drivers/rift/rift_driver.c | 100 +++++++++++++++++++++++++++ src/xrt/drivers/rift/rift_internal.h | 69 ++++++++++++++++++ src/xrt/drivers/rift/rift_usb.c | 16 +++++ src/xrt/drivers/rift/rift_usb.h | 3 + 4 files changed, 188 insertions(+) diff --git a/src/xrt/drivers/rift/rift_driver.c b/src/xrt/drivers/rift/rift_driver.c index 5dd8ebb90..d9003d22c 100644 --- a/src/xrt/drivers/rift/rift_driver.c +++ b/src/xrt/drivers/rift/rift_driver.c @@ -23,6 +23,7 @@ #include "math/m_clock_tracking.h" #include "math/m_api.h" #include "math/m_vec2.h" +#include "math/m_vec3.h" #include "math/m_mathinclude.h" // IWYU pragma: keep #include "util/u_debug.h" @@ -324,6 +325,90 @@ rift_radio_thread(void *ptr) return NULL; } +#define PARSE_MICROMETER_TRIPLET(out, values) \ + do { \ + out.x = MICROMETERS_TO_METERS(values[0]); \ + out.y = MICROMETERS_TO_METERS(values[1]); \ + out.z = MICROMETERS_TO_METERS(values[2]); \ + } while (0) + +static int +rift_read_led_model(struct rift_hmd *hmd) +{ + int result; + + struct rift_position_calibration_report position_report; + + result = rift_get_position_calibration_report(hmd, &position_report); + if (result < 0) { + HMD_ERROR(hmd, "Failed to read position calibration report, reason %d", result); + return result; + } + + // minus one to get rid of the IMU, which is also in here + hmd->led_model.leds = U_TYPED_ARRAY_CALLOC(struct t_constellation_tracker_led, position_report.position_count); + + for (uint32_t i = 0; i < position_report.position_count; i++) { + result = rift_get_position_calibration_report(hmd, &position_report); + if (result < 0) { + HMD_ERROR(hmd, "Failed to read position calibration report, reason %d", result); + goto read_led_model_err; + } + + if (position_report.position_type == RIFT_POSITION_CALIBRATION_TYPE_INERTIAL_SENSOR) { + PARSE_MICROMETER_TRIPLET(hmd->T_device_imu.position, position_report.position); + hmd->T_device_imu.position.x = -hmd->T_device_imu.position.x; + hmd->T_device_imu.position.y = hmd->T_device_imu.position.y; + hmd->T_device_imu.position.z = -hmd->T_device_imu.position.z; + + hmd->T_device_imu.orientation = (struct xrt_quat)XRT_QUAT_IDENTITY; + + // NOTE: we ignore the IMU orientation since it's always zeroed out.. + + // Invert the pose for device relative to IMU + math_pose_invert(&hmd->T_device_imu, &hmd->T_imu_device); + } else { + struct xrt_vec3 pos; + PARSE_MICROMETER_TRIPLET(pos, position_report.position); + + struct xrt_vec3 normal; + PARSE_MICROMETER_TRIPLET(normal, position_report.normal); + math_vec3_normalize(&normal); // normalize the direction + + pos.x = -pos.x; + pos.y = pos.y; + pos.z = -pos.z; + normal.x = -normal.x; + normal.y = normal.y; + normal.z = -normal.z; + + hmd->led_model.leds[hmd->led_model.led_count++] = (struct t_constellation_tracker_led){ + .position = pos, + .normal = normal, + .radius_m = 0.0035f, // 3.5mm + .visibility_angle = DEG_TO_RAD(90.0f), // TODO: tune this value properly + .id = hmd->led_model.led_count - 1, + }; + } + } + + // Make the LEDs relative to the IMU + for (size_t i = 0; i < hmd->led_model.led_count; i++) { + struct xrt_vec3 led_pos = hmd->led_model.leds[i].position; + hmd->led_model.leds[i].position = m_vec3_add(hmd->T_imu_device.position, led_pos); + } + + return 0; + +read_led_model_err: + free(hmd->led_model.leds); + hmd->led_model.leds = NULL; + + return result; +} + +#undef PARSE_MICROMETER_TRIPLET + /* * * Driver functions @@ -370,6 +455,13 @@ rift_hmd_destroy(struct xrt_device *xdev) } } + if (hmd->led_model.leds != NULL) { + free(hmd->led_model.leds); + + hmd->led_model.leds = NULL; + hmd->led_model.led_count = 0; + } + u_device_free(&hmd->base); } @@ -635,6 +727,14 @@ rift_devices_create(struct os_hid_device *hmd_dev, } } + result = rift_read_led_model(hmd); + if (result < 0) { + HMD_ERROR(hmd, "Failed to read LED model, reason %d", result); + goto error; + } else { + HMD_DEBUG(hmd, "Read LED model from Rift."); + } + // fill in extra display info about the headset switch (hmd->variant) { diff --git a/src/xrt/drivers/rift/rift_internal.h b/src/xrt/drivers/rift/rift_internal.h index a07009ead..5e22be110 100644 --- a/src/xrt/drivers/rift/rift_internal.h +++ b/src/xrt/drivers/rift/rift_internal.h @@ -20,6 +20,7 @@ #include "math/m_clock_tracking.h" #include "tracking/t_imu.h" +#include "tracking/t_constellation.h" #include "os/os_hid.h" #include "os/os_threading.h" @@ -237,6 +238,70 @@ struct rift_lens_distortion_report SIZE_ASSERT(struct rift_lens_distortion_report, 9 + sizeof(struct rift_catmull_rom_distortion_report_data)); +enum rift_position_calibration_version +{ + // no data stored + RIFT_POSITION_CALIBRATION_VERSION_NONE = 0, + // hard-coded default positions + RIFT_POSITION_CALIBRATION_VERSION_DEFAULT = 1, + // factory calibrated + RIFT_POSITION_CALIBRATION_VERSION_FACTORY = 2, + // user calibrated + RIFT_POSITION_CALIBRATION_VERSION_USER = 3, +}; + +enum rift_position_calibration_type +{ + RIFT_POSITION_CALIBRATION_TYPE_LED = 0, + RIFT_POSITION_CALIBRATION_TYPE_INERTIAL_SENSOR = 1, +}; + +struct rift_position_calibration_report +{ + uint16_t command_id; + // the version/type of calibration, see rift_position_calibration_version + uint8_t version; + // the x/y/z position of the object, this is a signed integer in micrometers, position is relative to the center + // of the emitter plane of the display at nominal focus. + int32_t position[3]; + // the x/y/z axis normal of the object, this is a signed integer in micrometers, normal is relative to the + // position + int16_t normal[3]; + // rotation around the normal, in units of 10^-4 radians + uint16_t rotation; + // the current position in the array of LEDs, increments on reads, gets set to the value on writes + uint16_t position_index; + // read-only value of the number of LEDs + uint16_t position_count; + // the type of the object being described, see rift_position_calibration_type + uint16_t position_type; +}; + +SIZE_ASSERT(struct rift_position_calibration_report, 29); + +enum rift_custom_pattern_state +{ + RIFT_CUSTOM_PATTERN_STAT_OFF = 0, + RIFT_CUSTOM_PATTERN_STAT_LOW = 1, + RIFT_CUSTOM_PATTERN_STAT_HIGH = 3, +}; + +struct rift_custom_pattern_report +{ + uint16_t command_id; + // the length of the sequence that each LED goes through + uint8_t sequence_length; + // the sequence the specific LED goes through, 2 bits per state, 0 (off), 1 (low), and 3 (high), ordered from + // LSB to MSB + uint32_t sequence; + // the current LED being described, increments on reads, gets set to the value on writes + uint16_t led_index; + // the number of tracking LEDs present on the device + uint16_t led_count; +}; + +SIZE_ASSERT(struct rift_custom_pattern_report, 11); + struct rift_dk2_keepalive_mux_report { uint16_t command; @@ -873,6 +938,10 @@ struct rift_hmd int added_devices; struct xrt_device *devices[4]; // left touch, right touch, tracked object, remote + struct t_constellation_tracker_led_model led_model; + struct xrt_pose T_imu_device; + struct xrt_pose T_device_imu; + //! Generic state for the radio state machine struct { diff --git a/src/xrt/drivers/rift/rift_usb.c b/src/xrt/drivers/rift/rift_usb.c index 0faa51213..4fd1d2544 100644 --- a/src/xrt/drivers/rift/rift_usb.c +++ b/src/xrt/drivers/rift/rift_usb.c @@ -128,6 +128,22 @@ rift_get_lens_distortion(struct rift_hmd *hmd, struct rift_lens_distortion_repor return 0; } +int +rift_get_position_calibration_report(struct rift_hmd *hmd, struct rift_position_calibration_report *position_report) +{ + uint8_t buf[REPORT_MAX_SIZE] = {0}; + + int result = rift_get_report(hmd, false, FEATURE_REPORT_POS_CALIBRATION, buf, sizeof(buf)); + if (result < 0) { + return result; + } + + // FIXME: handle endianness + memcpy(position_report, buf + 1, sizeof(*position_report)); + + return 0; +} + int rift_set_config(struct rift_hmd *hmd, struct rift_config_report *config) { diff --git a/src/xrt/drivers/rift/rift_usb.h b/src/xrt/drivers/rift/rift_usb.h index cdf09d91a..af42397fd 100644 --- a/src/xrt/drivers/rift/rift_usb.h +++ b/src/xrt/drivers/rift/rift_usb.h @@ -13,6 +13,9 @@ int rift_get_lens_distortion(struct rift_hmd *hmd, struct rift_lens_distortion_report *lens_distortion); +int +rift_get_position_calibration_report(struct rift_hmd *hmd, struct rift_position_calibration_report *position_report); + void rift_parse_distortion_report(struct rift_lens_distortion_report *report, struct rift_lens_distortion *out); -- 2.51.2