diff --git a/src/xrt/drivers/rift_s/rift_s_controller.c b/src/xrt/drivers/rift_s/rift_s_controller.c index a15301192..1141e2319 100644 --- a/src/xrt/drivers/rift_s/rift_s_controller.c +++ b/src/xrt/drivers/rift_s/rift_s_controller.c @@ -269,6 +269,7 @@ attitude_sample(struct rift_s_controller *ctrl, unsigned int index) static void suspend_position_tracking(struct rift_s_controller *ctrl) { + ctrl->motion_generation++; rift_s_position_filter_suspend(&ctrl->position_filter); if (ctrl->sys != NULL) { rift_s_motion_reset_hand(ctrl->sys->motion, @@ -921,6 +922,7 @@ imu_descriptor_give_up(struct rift_s_controller *ctrl, const char *reason) static void reset_position_filter(struct rift_s_controller *ctrl) { + ctrl->motion_generation++; rift_s_position_filter_reset(&ctrl->position_filter); float accel_noise = debug_get_float_option_rift_s_position_accel_noise(); float bias_walk = debug_get_float_option_rift_s_position_bias_walk(); @@ -970,40 +972,69 @@ position_tracking_at(const struct rift_s_controller *ctrl, timepoint_ns t, bool return age > RIFT_S_COAST_AFTER_NS ? POSITION_COASTING : POSITION_TRACKED; } -// Adapt the Monado filter's current uncertainty to the model reliability gate. Normal -// vision delivery latency must not activate a model merely because capture time is old. -void -rift_s_controller_update_motion(struct rift_s_controller *ctrl, timepoint_ns capture_ns) +// Capture controller state before the potentially slow HMD query. The radio dispatch +// keeps the devices alive and serializes IMU reports with configuration changes. +struct motion_update +{ + struct rift_s_motion_sample sample; + uint64_t generation; +}; + +static bool +prepare_motion_update(struct rift_s_controller *ctrl, timepoint_ns capture_ns, struct motion_update *update) { if (ctrl->sys == NULL || ctrl->sys->motion == NULL || !ctrl->position_filter.initialized) { - return; + return false; } + bool velocity_tracked; + enum position_tracking state = position_tracking_at(ctrl, capture_ns, &velocity_tracked); + *update = (struct motion_update){0}; + update->generation = ctrl->motion_generation; + struct rift_s_motion_sample *sample = &update->sample; + sample->timestamp_ns = capture_ns; + sample->pose.orientation = ctrl->pose.orientation; + rift_s_position_filter_predict(&ctrl->position_filter, capture_ns, 0, &sample->pose.position, + &sample->linear_velocity); + sample->linear_acceleration = ctrl->world_accel; + sample->angular_velocity = ctrl->fusion.last.gyro; + for (int axis = 0; axis < 3; ++axis) { + sample->position_variance[axis] = + state != POSITION_LOST && !ctrl->position_filter.suspended && ctrl->track_vision_updates >= 3 + ? ctrl->position_filter.state.P[axis][0][0] + : -1; + } + return true; +} + +static void +submit_motion_update(struct rift_s_controller *ctrl, const struct motion_update *update) +{ struct xrt_device *head = rift_s_system_get_hmd(ctrl->sys); struct xrt_space_relation head_relation = XRT_SPACE_RELATION_ZERO; if (head == NULL || - head->get_tracked_pose(head, XRT_INPUT_GENERIC_HEAD_POSE, capture_ns, &head_relation) != XRT_SUCCESS || + head->get_tracked_pose(head, XRT_INPUT_GENERIC_HEAD_POSE, update->sample.timestamp_ns, &head_relation) != XRT_SUCCESS || (head_relation.relation_flags & (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) != (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) { return; } - bool velocity_tracked; - enum position_tracking state = position_tracking_at(ctrl, capture_ns, &velocity_tracked); - struct rift_s_motion_sample sample = {0}; - sample.timestamp_ns = capture_ns; - sample.pose.orientation = ctrl->pose.orientation; - rift_s_position_filter_predict(&ctrl->position_filter, capture_ns, 0, &sample.pose.position, - &sample.linear_velocity); - sample.linear_acceleration = ctrl->world_accel; - sample.angular_velocity = ctrl->fusion.last.gyro; - for (int axis = 0; axis < 3; ++axis) { - sample.position_variance[axis] = - state != POSITION_LOST && !ctrl->position_filter.suspended && ctrl->track_vision_updates >= 3 - ? ctrl->position_filter.state.P[axis][0][0] - : -1; + os_mutex_lock(&ctrl->mutex); + // A clock reset, loss or replacement during the unlocked query must not revive an old model. + if (update->generation == ctrl->motion_generation) { + unsigned int hand = ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0; + rift_s_motion_update(ctrl->sys->motion, hand, &update->sample, &head_relation.pose); } - unsigned int hand = ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0; - rift_s_motion_update(ctrl->sys->motion, hand, &sample, &head_relation.pose); + os_mutex_unlock(&ctrl->mutex); +} + +void +rift_s_controller_update_motion(struct rift_s_controller *ctrl, timepoint_ns capture_ns) +{ + struct motion_update update; + os_mutex_lock(&ctrl->mutex); + bool prepared = prepare_motion_update(ctrl, capture_ns, &update); + os_mutex_unlock(&ctrl->mutex); + if (prepared) submit_motion_update(ctrl, &update); } bool @@ -1137,7 +1168,12 @@ handle_imu_update(struct rift_s_controller *ctrl, ctrl->last_imu_capture_ns = capture_ns; if (ctrl->world_accel_valid) { rift_s_position_filter_imu(&ctrl->position_filter, capture_ns, &ctrl->world_accel); - rift_s_controller_update_motion(ctrl, capture_ns); + struct motion_update update; + if (prepare_motion_update(ctrl, capture_ns, &update)) { + os_mutex_unlock(&ctrl->mutex); + submit_motion_update(ctrl, &update); + os_mutex_lock(&ctrl->mutex); + } } } if (log_imu) { diff --git a/src/xrt/drivers/rift_s/rift_s_controller.h b/src/xrt/drivers/rift_s/rift_s_controller.h index 14c13df7f..d4c74fdd7 100644 --- a/src/xrt/drivers/rift_s/rift_s_controller.h +++ b/src/xrt/drivers/rift_s/rift_s_controller.h @@ -203,6 +203,8 @@ struct rift_s_controller timepoint_ns last_fix_ns; //! Host capture time and world-frame acceleration (gravity removed) of the newest IMU sample. timepoint_ns last_imu_capture_ns; + //! Invalidates motion snapshots across clock resets, device loss and replacement. + uint64_t motion_generation; struct xrt_vec3 world_accel; bool world_accel_valid; //! Last reported aim/grip position source, for logging transitions. @@ -407,8 +409,8 @@ rift_s_controller_get_led_model(struct xrt_device *xdev, struct t_constellation_ bool rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs); -// Feed motion models from the current capture-time filter state. Caller holds the controller mutex -// and keeps the system devices alive, as the radio dispatch does. +// Feed motion models from a capture-time snapshot. Locks the controller only for snapshot/commit; +// the caller keeps the system devices alive and serializes IMU updates, as radio dispatch does. void rift_s_controller_update_motion(struct rift_s_controller *ctrl, timepoint_ns capture_ns); diff --git a/tests/tests_rift_s_tracking.cpp b/tests/tests_rift_s_tracking.cpp index c431efc9a..0fc6d44d8 100644 --- a/tests/tests_rift_s_tracking.cpp +++ b/tests/tests_rift_s_tracking.cpp @@ -2417,3 +2417,97 @@ TEST_CASE("Rift S durable accelerometer bias is device bound and verified at res unlink(path.c_str()); rmdir(directory); } + +TEST_CASE("Rift S motion queries preserve capture samples without holding the controller lock") +{ + struct Head { + rift_s_hmd hmd{}; + rift_s_controller *ctrl{}; + timepoint_ns requested{}; + bool unlocked{}, reset{}, correct{}; + }; + for (bool right : {false, true}) { + Head head{}; + rift_s_system sys{}; + xrt_atomic_s32_store(&sys.transport_online, 1); + sys.hmd = &head.hmd; + sys.motion = rift_s_motion_create(); + REQUIRE(sys.motion != nullptr); + rift_s_controller ctrl{}; + ctrl.sys = &sys; + ctrl.base.device_type = right ? XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER : XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; + ctrl.gravity_initialized = true; + ctrl.pose.orientation = XRT_QUAT_IDENTITY; + ctrl.P_imu_device = ctrl.P_device_aim = ctrl.P_device_grip = XRT_POSE_IDENTITY; + os_mutex_init(&ctrl.mutex); + head.ctrl = &ctrl; + head.hmd.base.get_tracked_pose = [](xrt_device *device, xrt_input_name name, int64_t timestamp, + xrt_space_relation *out) { + auto &head = *reinterpret_cast(device); + head.requested = timestamp; + head.unlocked = os_mutex_trylock(&head.ctrl->mutex) == 0; + if (head.unlocked) { + // An optical correction after snapshot must affect the next sample only. + if (head.correct) head.ctrl->pose.orientation = {0, .1f, 0, std::sqrt(.99f)}; + os_mutex_unlock(&head.ctrl->mutex); + } + if (head.reset && head.unlocked) { + rift_s_controller_device_lost(head.ctrl); + // The transport supervisor clears the shared model immediately after device_lost. + rift_s_motion_reset_hand(head.ctrl->sys->motion, + head.ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0); + } + return get_test_head_pose(device, name, timestamp, out); + }; + const xrt_vec3 position = {right ? 1.3f : .7f, 1.1f, 2.5f}, variance = {1e-5f, 1e-5f, 1e-5f}; + const timepoint_ns start = 1000000000; + rift_s_position_filter_reset(&ctrl.position_filter); + rift_s_position_filter_fix(&ctrl.position_filter, start, &position, &variance, .15f); + ctrl.last_fix_ns = start; + ctrl.track_vision_updates = 3; + auto *reference = rift_s_motion_create(); + REQUIRE(reference != nullptr); + unsigned hand = right ? 1 : 0; + timepoint_ns timestamp = start; + for (int i = 0; i < 1200; ++i) { + timestamp += (i % 17 == 0 ? 7 : i % 3 + 1) * U_TIME_1MS_IN_NS; + ctrl.last_fix_ns = timestamp; + ctrl.last_imu_capture_ns = timestamp; + const xrt_vec3 acceleration = {.01f * std::sin(i * .01f), 0, .02f * std::cos(i * .02f)}; + rift_s_position_filter_imu(&ctrl.position_filter, timestamp, &acceleration); + ctrl.world_accel = acceleration; + ctrl.fusion.last.gyro = {0, .01f, 0}; + rift_s_motion_sample sample{}; + sample.timestamp_ns = timestamp; + sample.pose.orientation = ctrl.pose.orientation; + rift_s_position_filter_predict(&ctrl.position_filter, timestamp, 0, &sample.pose.position, + &sample.linear_velocity); + sample.linear_acceleration = ctrl.world_accel; + sample.angular_velocity = ctrl.fusion.last.gyro; + for (int axis = 0; axis < 3; ++axis) sample.position_variance[axis] = ctrl.position_filter.state.P[axis][0][0]; + xrt_space_relation head_relation{}; + get_test_head_pose(&head.hmd.base, XRT_INPUT_GENERIC_HEAD_POSE, timestamp, &head_relation); + rift_s_motion_update(reference, hand, &sample, &head_relation.pose); + head.correct = i == 500; + rift_s_controller_update_motion(&ctrl, timestamp); + REQUIRE(head.unlocked); + REQUIRE(head.requested == timestamp); + for (time_duration_ns ahead : {time_duration_ns{0}, time_duration_ns{25000000}}) { + xrt_space_relation actual{}, expected{}; + CHECK(rift_s_motion_get(sys.motion, hand, timestamp + ahead, &actual) == + rift_s_motion_get(reference, hand, timestamp + ahead, &expected)); + CHECK(std::memcmp(&actual, &expected, sizeof(actual)) == 0); + } + } + // Device loss during the unlocked query clears the cache. The staged sample must not revive it. + head.reset = true; + rift_s_controller_update_motion(&ctrl, timestamp + U_TIME_1MS_IN_NS); + REQUIRE(head.unlocked); + xrt_space_relation relation{}; + CHECK(rift_s_motion_get(sys.motion, hand, timestamp + U_TIME_1MS_IN_NS, &relation) == RIFT_S_MOTION_NONE); + CHECK_FALSE(ctrl.position_filter.initialized); + rift_s_motion_destroy(&reference); + rift_s_motion_destroy(&sys.motion); + os_mutex_destroy(&ctrl.mutex); + } +}