From 6e16959098b2c9ef6be0c33c937b5d123dfa2686 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Mon, 25 Jul 2022 12:05:53 -0300 Subject: [PATCH] d/vive: Add vive_source to convert v4l2 timestamps into monotonic clock Use a sink in the middle of the stream to correct for v4l2 timestamps with hardware timestamps to monotonic clock. This sink, together with other utilities related to data streaming, lives in a new vive_source entity, with similar functionality to wmr_source or rs_source. The vive_source lifetime is managed by the builder xfctx, which prevents deallocation dependencies between vive_device and the v4l2_fs to cause segfaults. --- src/xrt/drivers/CMakeLists.txt | 2 + src/xrt/drivers/vive/vive.h | 15 +- src/xrt/drivers/vive/vive_controller.c | 2 - src/xrt/drivers/vive/vive_device.c | 28 +- src/xrt/drivers/vive/vive_device.h | 7 +- src/xrt/drivers/vive/vive_prober.c | 19 +- src/xrt/drivers/vive/vive_prober.h | 1 + src/xrt/drivers/vive/vive_source.c | 240 ++++++++++++++++++ src/xrt/drivers/vive/vive_source.h | 45 ++++ .../common/target_builder_lighthouse.c | 31 +-- 10 files changed, 346 insertions(+), 44 deletions(-) create mode 100644 src/xrt/drivers/vive/vive_source.c create mode 100644 src/xrt/drivers/vive/vive_source.h diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 4e13b6e7b..a8dc36727 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -177,6 +177,8 @@ if(XRT_BUILD_DRIVER_VIVE) vive/vive_controller.c vive/vive_lighthouse.h vive/vive_lighthouse.c + vive/vive_source.h + vive/vive_source.c ) target_link_libraries( drv_vive diff --git a/src/xrt/drivers/vive/vive.h b/src/xrt/drivers/vive/vive.h index 51c8436a5..e67932d03 100644 --- a/src/xrt/drivers/vive/vive.h +++ b/src/xrt/drivers/vive/vive.h @@ -13,6 +13,7 @@ #include #include +#include "util/u_debug.h" #include "util/u_time.h" /* @@ -27,7 +28,11 @@ #define VIVE_WARN(d, ...) U_LOG_IFL_W(d->log_level, __VA_ARGS__) #define VIVE_ERROR(d, ...) U_LOG_IFL_E(d->log_level, __VA_ARGS__) +DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN) + #define VIVE_CLOCK_FREQ 48e6 // 48 MHz +#define CAMERA_FREQUENCY 54 +#define IMU_FREQUENCY 1000 /*! * Helper function to convert raw device ticks to nanosecond timestamps. @@ -45,12 +50,10 @@ ticks_to_ns(uint32_t sample_ticks_raw, uint32_t *inout_prev_ticks, timepoint_ns uint32_t sample_ticks = __le32_to_cpu(sample_ticks_raw); uint32_t prev_ticks = *inout_prev_ticks; - uint32_t delta_ticks = 0; - if (prev_ticks < sample_ticks) { - delta_ticks = sample_ticks - prev_ticks; - } else { // Handle overflow - delta_ticks = (UINT32_MAX - prev_ticks) + sample_ticks; - } + // From the C standard https://www.open-std.org/jtc1/sc22/wg14/www/docs/n2912.pdf + // "A computation involving unsigned operands can never produce an overflow, + // because arithmetic for the unsigned type is performed modulo 2^N" + uint32_t delta_ticks = sample_ticks - prev_ticks; const double one_tick_in_s = (1 / VIVE_CLOCK_FREQ); const double one_tick_in_ns = one_tick_in_s * U_TIME_1S_IN_NS; diff --git a/src/xrt/drivers/vive/vive_controller.c b/src/xrt/drivers/vive/vive_controller.c index 07f2a6674..d607ae1b4 100644 --- a/src/xrt/drivers/vive/vive_controller.c +++ b/src/xrt/drivers/vive/vive_controller.c @@ -53,8 +53,6 @@ * */ -DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN) - enum vive_controller_input_index { // common inputs diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index 153509c0f..9b7d1c6ab 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -28,10 +28,10 @@ #include "vive.h" #include "vive_device.h" #include "vive_protocol.h" +#include "vive_source.h" +#include "xrt/xrt_tracking.h" -DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN) - static bool vive_mainboard_power_off(struct vive_device *d); @@ -416,6 +416,8 @@ update_imu(struct vive_device *d, const void *buffer) rel.pose.orientation = d->fusion.i3dof.rot; m_relation_history_push(d->fusion.relation_hist, &rel, now_ns); os_mutex_unlock(&d->fusion.mutex); + + vive_source_push_imu_packet(d->source, d->imu.last_sample_ts_ns, acceleration, angular_velocity); } } @@ -537,13 +539,14 @@ _decode_pulse_report(struct vive_device *d, const void *buffer) continue; } - if (sensor_id == 0xfd) { - /* TODO: handle camera sync timestamp */ + if (sensor_id == 0xfd) { // Camera frame timestamp + vive_source_push_frame_ticks(d->source, pulse->timestamp); continue; } if (sensor_id == 0xfb) { - /* TODO: Only turns on when the camera is running but not every frame. */ + /* TODO: Only turns on when the camera is running but not every frame. It + * seems to come with every 16h frame on an Index (~3.37hz) */ continue; } @@ -868,7 +871,7 @@ vive_setup_trackers(struct vive_device *d, struct vive_tracking_status status) } // SLAM tracker and hand tracker, if enabled, should've been initialized in - // the lighthouse builder. + // the lighthouse builder. The out_sinks fields will be set there as well. return true; } @@ -878,7 +881,8 @@ vive_device_create(struct os_hid_device *mainboard_dev, struct os_hid_device *sensors_dev, struct os_hid_device *watchman_dev, enum VIVE_VARIANT variant, - struct vive_tracking_status tstatus) + struct vive_tracking_status tstatus, + struct vive_source *vs) { XRT_TRACE_MARKER(); @@ -1000,6 +1004,7 @@ vive_device_create(struct os_hid_device *mainboard_dev, os_thread_helper_init(&d->sensors_thread); os_thread_helper_init(&d->watchman_thread); + d->source = vs; d->pose = (struct xrt_pose)XRT_POSE_IDENTITY; d->offset = (struct xrt_pose)XRT_POSE_IDENTITY; @@ -1024,8 +1029,6 @@ vive_device_create(struct os_hid_device *mainboard_dev, } } - vive_setup_trackers(d, tstatus); - switch (d->config.variant) { case VIVE_VARIANT_VIVE: snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "HTC Vive (vive)"); break; case VIVE_VARIANT_PRO: snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "HTC Vive Pro (vive)"); break; @@ -1034,6 +1037,13 @@ vive_device_create(struct os_hid_device *mainboard_dev, } snprintf(d->base.serial, XRT_DEVICE_NAME_LEN, "%s", d->config.firmware.device_serial_number); + bool trackers_set = vive_setup_trackers(d, tstatus); + if (!trackers_set) { + VIVE_ERROR(d, "Failed to setup trackers"); + vive_device_destroy((struct xrt_device *)d); + return NULL; + } + ret = os_thread_helper_start(&d->sensors_thread, vive_sensors_run_thread, d); if (ret != 0) { VIVE_ERROR(d, "Failed to start sensors thread!"); diff --git a/src/xrt/drivers/vive/vive_device.h b/src/xrt/drivers/vive/vive_device.h index a28ee0735..263fd7a23 100644 --- a/src/xrt/drivers/vive/vive_device.h +++ b/src/xrt/drivers/vive/vive_device.h @@ -100,6 +100,9 @@ struct vive_device //! Whether to track the HMD with 6dof SLAM or fallback to the 3dof tracker bool slam_over_3dof; + //! In charge of managing raw samples, redirects them for tracking + struct vive_source *source; + //! Last tracked pose struct xrt_pose pose; @@ -129,8 +132,8 @@ vive_device_create(struct os_hid_device *mainboard_dev, struct os_hid_device *sensors_dev, struct os_hid_device *watchman_dev, enum VIVE_VARIANT variant, - struct vive_tracking_status tstatus); - + struct vive_tracking_status tstatus, + struct vive_source *vs); #ifdef __cplusplus } diff --git a/src/xrt/drivers/vive/vive_prober.c b/src/xrt/drivers/vive/vive_prober.c index 696d6ceaa..f0d5f3d12 100644 --- a/src/xrt/drivers/vive/vive_prober.c +++ b/src/xrt/drivers/vive/vive_prober.c @@ -66,6 +66,7 @@ init_vive1(struct xrt_prober *xp, size_t device_count, enum u_logging_level log_level, struct vive_tracking_status tstatus, + struct vive_source *vs, struct xrt_device **out_xdev) { log_vive_device(log_level, xp, dev); @@ -120,7 +121,7 @@ init_vive1(struct xrt_prober *xp, return 0; } struct vive_device *d = - vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_VIVE, tstatus); + vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_VIVE, tstatus, vs); if (d == NULL) { free(sensors_dev); free(mainboard_dev); @@ -139,6 +140,7 @@ init_vive_pro(struct xrt_prober *xp, size_t device_count, enum u_logging_level log_level, struct vive_tracking_status tstatus, + struct vive_source *vs, struct xrt_device **out_xdev) { XRT_TRACE_MARKER(); @@ -195,7 +197,8 @@ init_vive_pro(struct xrt_prober *xp, free(sensors_dev); return 0; } - struct vive_device *d = vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_PRO, tstatus); + struct vive_device *d = + vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_PRO, tstatus, vs); if (d == NULL) { free(sensors_dev); free(mainboard_dev); @@ -214,6 +217,7 @@ init_valve_index(struct xrt_prober *xp, size_t device_count, enum u_logging_level log_level, struct vive_tracking_status tstatus, + struct vive_source *vs, struct vive_config **out_vive_config, struct xrt_device **out_xdevs) { @@ -252,7 +256,7 @@ init_valve_index(struct xrt_prober *xp, return 0; } - struct vive_device *d = vive_device_create(NULL, sensors_dev, watchman_dev, VIVE_VARIANT_INDEX, tstatus); + struct vive_device *d = vive_device_create(NULL, sensors_dev, watchman_dev, VIVE_VARIANT_INDEX, tstatus, vs); if (d == NULL) { return 0; } @@ -272,6 +276,7 @@ vive_found(struct xrt_prober *xp, size_t index, cJSON *attached_data, struct vive_tracking_status tstatus, + struct vive_source *vs, struct vive_config **out_vive_config, struct xrt_device **out_xdev) { @@ -289,10 +294,12 @@ vive_found(struct xrt_prober *xp, } switch (dev->product_id) { - case VIVE_PID: return init_vive1(xp, dev, devices, device_count, log_level, tstatus, out_xdev); - case VIVE_PRO_MAINBOARD_PID: return init_vive_pro(xp, dev, devices, device_count, log_level, tstatus, out_xdev); + case VIVE_PID: return init_vive1(xp, dev, devices, device_count, log_level, tstatus, vs, out_xdev); + case VIVE_PRO_MAINBOARD_PID: + return init_vive_pro(xp, dev, devices, device_count, log_level, tstatus, vs, out_xdev); case VIVE_PRO_LHR_PID: - return init_valve_index(xp, dev, devices, device_count, log_level, tstatus, out_vive_config, out_xdev); + return init_valve_index(xp, dev, devices, device_count, log_level, tstatus, vs, out_vive_config, + out_xdev); default: U_LOG_E("No product ids matched %.4x", dev->product_id); return 0; } diff --git a/src/xrt/drivers/vive/vive_prober.h b/src/xrt/drivers/vive/vive_prober.h index 05ed236a3..93f07e90e 100644 --- a/src/xrt/drivers/vive/vive_prober.h +++ b/src/xrt/drivers/vive/vive_prober.h @@ -54,6 +54,7 @@ vive_found(struct xrt_prober *xp, size_t index, cJSON *attached_data, struct vive_tracking_status tstatus, + struct vive_source *vs, struct vive_config **out_vive_config, struct xrt_device **out_xdev); diff --git a/src/xrt/drivers/vive/vive_source.c b/src/xrt/drivers/vive/vive_source.c new file mode 100644 index 000000000..d946d3794 --- /dev/null +++ b/src/xrt/drivers/vive/vive_source.c @@ -0,0 +1,240 @@ +// Copyright 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Interface for vive data sources + * @author Mateo de Mayo + * @ingroup drv_vive + */ + +#include "os/os_threading.h" +#include "util/u_deque.h" +#include "util/u_logging.h" +#include "xrt/xrt_frame.h" +#include "xrt/xrt_tracking.h" + +#include "vive.h" + +/*! + * Manages the data streaming state related to a vive headset. + * + * @implements xrt_frame_node + */ +struct vive_source +{ + struct xrt_frame_node node; + enum u_logging_level log_level; + + // Sinks + struct xrt_frame_sink sbs_sink; //!< Intermediate sink for SBS frames + struct xrt_imu_sink imu_sink; //!< Intermediate sink for IMU samples + struct xrt_slam_sinks in_sinks; //!< Pointers to intermediate sinks + struct xrt_slam_sinks out_sinks; //!< Pointers to downstream sinks + + // V4L2 frame streaming state + bool timestamps_have_been_zero_until_now; //!< First v4l2 frames are zeroed + bool waiting_for_first_nonempty_frame; //!< Whether the first good frame has been received + + // Frame timestamps + struct u_deque_timepoint_ns frame_timestamps; //! Queue of yet unused frame hw timestamps + struct os_mutex frame_timestamps_lock; //! Lock for accessing frame_timestamps + uint32_t last_frame_ticks; //! Last frame timestamp in device ticks + timepoint_ns last_frame_ts_ns; //! Last frame timestamp in device nanoseconds + + // Clock offsets + time_duration_ns hw2mono; //!< Estimated offset from IMU to monotonic clock + time_duration_ns hw2v4l2; //!< Estimated offset from IMU to V4L2 clock +}; + +//! Given a sample from two timestamp domains a and b that should have been +//! sampled as close as possible, together with an estimate of the offset +//! between a clock and b clock (or zero), it applies a smoothing average on the +//! estimated offset and returns a in b clock. +//! @todo Copy of clock_hw2mono in wmr_source.c, unify into a utility. +static inline timepoint_ns +clock_offset_a2b(double freq, timepoint_ns a, timepoint_ns b, time_duration_ns *inout_a2b) +{ + // Totally arbitrary way of computing alpha, if you have a better one, replace it + const double alpha = 1.0 - 12.5 / freq; // Weight to put on accumulated a2b + time_duration_ns old_a2b = *inout_a2b; + time_duration_ns got_a2b = b - a; + time_duration_ns new_a2b = old_a2b * alpha + got_a2b * (1.0 - alpha); + if (old_a2b == 0) { // a2b has not been set yet + new_a2b = got_a2b; + } + *inout_a2b = new_a2b; + return a + new_a2b; +} + +/* + * + * Vive source methods + * + */ + +//! Find the best corresponding hw timestamp from this v4l2 frame, return +//! whether it was found. +bool +vive_source_try_convert_v4l2_timestamp(struct vive_source *vs, struct xrt_frame *xf) +{ + assert(xf->timestamp != 0 || vs->timestamps_have_been_zero_until_now); + if (xf->timestamp == 0) { + return false; + } + vs->timestamps_have_been_zero_until_now = false; + + struct u_deque_timepoint_ns vive_timestamps = vs->frame_timestamps; + struct os_mutex *vive_timestamps_lock = &vs->frame_timestamps_lock; + + timepoint_ns v4l2_ts = xf->timestamp; + + size_t vive_ts_count = u_deque_timepoint_ns_size(vive_timestamps); + if (vive_ts_count == 0) { // This seems to happen in some runs + // This code assumes vive_timestamps will always be populated before v4l2 + // receives a frame, thus if we reach this, this assumption has failed. + VIVE_ERROR(vs, "Received a v4l2 frame but thwere are no vive timestamps to use"); + VIVE_ERROR(vs, "Will continue, but frame timestamps could be off by one"); + return false; + } + + os_mutex_lock(vive_timestamps_lock); + + // Find i in vive_timestamps that would be closer to xf->timestamp in v4l2 clock + int closer_i = -1; + timepoint_ns vive_ts = -1; + time_duration_ns min_distance = INT64_MAX; + for (size_t i = 0; i < vive_ts_count; i++) { + vive_ts = u_deque_timepoint_ns_at(vive_timestamps, i); + timepoint_ns v4l2_ts_est = vive_ts + vs->hw2v4l2; + time_duration_ns distance = llabs(v4l2_ts_est - v4l2_ts); + if (distance < min_distance) { + closer_i = i; + min_distance = distance; + } + } + + // Discard missed frames and set vive_timestamp to use in this frame + timepoint_ns vive_timestamp = 0; + for (; closer_i >= 0; closer_i--) { + u_deque_timepoint_ns_pop_front(vive_timestamps, &vive_timestamp); + } + + os_mutex_unlock(vive_timestamps_lock); + + // Our estimate is within a reasonable time distance + assert(min_distance < U_TIME_1S_IN_NS / CAMERA_FREQUENCY || vs->waiting_for_first_nonempty_frame); + vs->waiting_for_first_nonempty_frame = false; + + // Update estimate of hw2v4l2 clock offset, only used for matching timestamps + clock_offset_a2b(CAMERA_FREQUENCY, vive_timestamp, xf->timestamp, &vs->hw2v4l2); + + // Use vive_timestamp and put it in monotonic clock + xf->timestamp = vive_timestamp + vs->hw2mono; // Notice that we don't use hw2v4l2 + + return true; +} + +static void +vive_source_receive_sbs_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf) +{ + struct vive_source *vs = container_of(sink, struct vive_source, sbs_sink); + bool should_push = vive_source_try_convert_v4l2_timestamp(vs, xf); + + if (!should_push) { + VIVE_TRACE(vs, "skipped sbs img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp); + return; + } + + VIVE_TRACE(vs, "sbs img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp); + + if (vs->out_sinks.left) { // The split into left right will happen downstream + xrt_sink_push_frame(vs->out_sinks.left, xf); + } +} + +static void +vive_source_receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) +{ + struct vive_source *vs = container_of(sink, struct vive_source, imu_sink); + s->timestamp_ns = clock_offset_a2b(IMU_FREQUENCY, s->timestamp_ns, os_monotonic_get_ns(), &vs->hw2mono); + timepoint_ns ts = s->timestamp_ns; + struct xrt_vec3_f64 a = s->accel_m_s2; + struct xrt_vec3_f64 w = s->gyro_rad_secs; + VIVE_TRACE(vs, "imu t=%ld a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z); + + if (vs->out_sinks.imu) { + xrt_sink_push_imu(vs->out_sinks.imu, s); + } +} + +static void +vive_source_node_break_apart(struct xrt_frame_node *node) +{} + +static void +vive_source_node_destroy(struct xrt_frame_node *node) +{ + struct vive_source *vs = container_of(node, struct vive_source, node); + os_mutex_destroy(&vs->frame_timestamps_lock); + u_deque_timepoint_ns_destroy(&vs->frame_timestamps); +} + + +/*! + * + * Exported functions + * + */ + +struct vive_source * +vive_source_create(struct xrt_frame_context *xfctx) +{ + struct vive_source *vs = U_TYPED_CALLOC(struct vive_source); + vs->log_level = debug_get_log_option_vive_log(); + + // Setup sinks + vs->sbs_sink.push_frame = vive_source_receive_sbs_frame; + vs->imu_sink.push_imu = vive_source_receive_imu_sample; + vs->in_sinks.left = &vs->sbs_sink; + vs->in_sinks.right = NULL; + vs->in_sinks.imu = &vs->imu_sink; + + vs->timestamps_have_been_zero_until_now = true; + vs->waiting_for_first_nonempty_frame = true; + + vs->frame_timestamps = u_deque_timepoint_ns_create(); + os_mutex_init(&vs->frame_timestamps_lock); + + // Setup node + struct xrt_frame_node *xfn = &vs->node; + xfn->break_apart = vive_source_node_break_apart; + xfn->destroy = vive_source_node_destroy; + xrt_frame_context_add(xfctx, &vs->node); + + VIVE_DEBUG(vs, "Vive source created"); + + return vs; +} + +void +vive_source_push_imu_packet(struct vive_source *vs, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g) +{ + struct xrt_vec3_f64 a64 = {a.x, a.y, a.z}; + struct xrt_vec3_f64 g64 = {g.x, g.y, g.z}; + struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = a64, .gyro_rad_secs = g64}; + xrt_sink_push_imu(&vs->imu_sink, &sample); +} + +void +vive_source_push_frame_ticks(struct vive_source *vs, timepoint_ns ticks) +{ + ticks_to_ns(ticks, &vs->last_frame_ticks, &vs->last_frame_ts_ns); + u_deque_timepoint_ns_push_back(vs->frame_timestamps, vs->last_frame_ts_ns); +} + +void +vive_source_hook_into_sinks(struct vive_source *vs, struct xrt_slam_sinks *sinks) +{ + vs->out_sinks = *sinks; + sinks->left = vs->in_sinks.left; +} diff --git a/src/xrt/drivers/vive/vive_source.h b/src/xrt/drivers/vive/vive_source.h new file mode 100644 index 000000000..1b0158552 --- /dev/null +++ b/src/xrt/drivers/vive/vive_source.h @@ -0,0 +1,45 @@ +// Copyright 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Interface for vive data sources + * @author Mateo de Mayo + * @ingroup drv_vive + */ + +#pragma once + +#include "xrt/xrt_frame.h" +#include "xrt/xrt_tracking.h" + +/*! + * Vive data sources + * + * @addtogroup drv_vive + * @{ + */ + + +#ifdef __cplusplus +extern "C" { +#endif + +struct vive_source * +vive_source_create(struct xrt_frame_context *xfctx); + +void +vive_source_push_imu_packet(struct vive_source *vs, timepoint_ns t, struct xrt_vec3 a, struct xrt_vec3 g); + +void +vive_source_push_frame_ticks(struct vive_source *vs, timepoint_ns ticks); + +void +vive_source_hook_into_sinks(struct vive_source *vs, struct xrt_slam_sinks *sinks); + +/*! + * @} + */ + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/targets/common/target_builder_lighthouse.c b/src/xrt/targets/common/target_builder_lighthouse.c index 41ee7b468..c1b8f7a1a 100644 --- a/src/xrt/targets/common/target_builder_lighthouse.c +++ b/src/xrt/targets/common/target_builder_lighthouse.c @@ -36,6 +36,7 @@ #ifdef XRT_BUILD_DRIVER_VIVE #include "vive/vive_prober.h" #include "vive/vive_device.h" +#include "vive/vive_source.h" #endif #ifdef XRT_BUILD_DRIVER_SURVIVE @@ -94,7 +95,6 @@ struct index_camera_finder { struct xrt_fs *xfs; struct xrt_frame_context *xfctx; - bool found; }; /* @@ -124,19 +124,6 @@ get_selected_mode(struct xrt_fs *xfs) return selected_mode; } -static bool -visual_inertial_stream_start(struct xrt_fs *xfs, struct xrt_slam_sinks *sinks) -{ - // Stream frames - struct xrt_frame_sink *sbs_sink = sinks->left; - bool success = xrt_fs_stream_start(xfs, sbs_sink, XRT_FS_CAPTURE_TYPE_TRACKING, get_selected_mode(xfs)); - if (!success) { - return false; - } - - return true; -} - static void on_video_device(struct xrt_prober *xp, struct xrt_prober_device *pdev, @@ -462,13 +449,18 @@ stream_data_sources(struct u_system_devices *usysd, struct xrt_prober *xp, struc } bool success = false; + uint32_t mode = get_selected_mode(finder.xfs); + + // If SLAM is enabled (only on vive driver) we intercept the data sink if (lhs.slam_enabled) { - LH_ASSERT(false, "Not implemented"); - } else { - uint32_t mode = get_selected_mode(finder.xfs); - success = xrt_fs_stream_start(finder.xfs, sinks.left, XRT_FS_CAPTURE_TYPE_TRACKING, mode); + struct vive_device *d = (struct vive_device *)usysd->base.roles.head; + LH_ASSERT_(d != NULL && d->source != NULL); + struct vive_source *vs = d->source; + vive_source_hook_into_sinks(vs, &sinks); } + success = xrt_fs_stream_start(finder.xfs, sinks.left, XRT_FS_CAPTURE_TYPE_TRACKING, mode); + if (!success) { LH_ERROR("Unable to start data streaming"); xrt_frame_context_destroy_nodes(&usysd->xfctx); @@ -551,7 +543,8 @@ lighthouse_open_system(struct xrt_builder *xb, case VIVE_PID: case VIVE_PRO_MAINBOARD_PID: case VIVE_PRO_LHR_PID: { - int num_devices = vive_found(xp, xpdevs, xpdev_count, i, NULL, tstatus, &hmd_config, + struct vive_source *vs = vive_source_create(&usysd->xfctx); + int num_devices = vive_found(xp, xpdevs, xpdev_count, i, NULL, tstatus, vs, &hmd_config, &usysd->base.xdevs[usysd->base.xdev_count]); usysd->base.xdev_count += num_devices;