From 41f42e0b3a79b9268fa76ab9a0d51083037e13a0 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Mon, 6 Dec 2021 08:54:15 -0300 Subject: [PATCH] d/wmr: Add wmr_source with camera streams support --- src/xrt/drivers/CMakeLists.txt | 4 +- src/xrt/drivers/meson.build | 2 + src/xrt/drivers/wmr/wmr_camera.c | 45 ++++- src/xrt/drivers/wmr/wmr_camera.h | 8 +- src/xrt/drivers/wmr/wmr_hmd.c | 35 ++-- src/xrt/drivers/wmr/wmr_hmd.h | 11 +- src/xrt/drivers/wmr/wmr_prober.c | 4 +- src/xrt/drivers/wmr/wmr_source.c | 306 +++++++++++++++++++++++++++++++ src/xrt/drivers/wmr/wmr_source.h | 38 ++++ 9 files changed, 426 insertions(+), 27 deletions(-) create mode 100644 src/xrt/drivers/wmr/wmr_source.c create mode 100644 src/xrt/drivers/wmr/wmr_source.h diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 202f92a3b..789e7610b 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -328,8 +328,10 @@ if(XRT_BUILD_DRIVER_WMR) wmr/wmr_protocol.h wmr/wmr_controller_protocol.c wmr/wmr_controller_protocol.h + wmr/wmr_source.c + wmr/wmr_source.h ) - target_link_libraries(drv_wmr PRIVATE xrt-interfaces aux_util aux_math xrt-external-cjson) + target_link_libraries(drv_wmr PRIVATE xrt-interfaces aux_util aux_math aux_tracking xrt-external-cjson) list(APPEND ENABLED_HEADSET_DRIVERS wmr) # Can only build camera support with libusb diff --git a/src/xrt/drivers/meson.build b/src/xrt/drivers/meson.build index d3c448905..f38255ad1 100644 --- a/src/xrt/drivers/meson.build +++ b/src/xrt/drivers/meson.build @@ -325,6 +325,8 @@ lib_drv_wmr = static_library( 'wmr/wmr_protocol.h', 'wmr/wmr_controller_protocol.c', 'wmr/wmr_controller_protocol.h', + 'wmr/wmr_source.c', + 'wmr/wmr_source.h', ) + wmr_camera_files, include_directories: [ xrt_include, diff --git a/src/xrt/drivers/wmr/wmr_camera.c b/src/xrt/drivers/wmr/wmr_camera.c index 8d9dbbe52..9c3e7d409 100644 --- a/src/xrt/drivers/wmr/wmr_camera.c +++ b/src/xrt/drivers/wmr/wmr_camera.c @@ -96,6 +96,10 @@ struct wmr_camera struct u_sink_debug debug_sinks[2]; + bool submit_frames; //!< Whether to push frames to left/right sinks + struct xrt_frame_sink *left_sink; //!< Downstream sinks to push left tracking frames to + struct xrt_frame_sink *right_sink; //!< Downstream sinks to push right tracking frames to + enum u_logging_level log_level; }; @@ -244,7 +248,6 @@ set_active(struct wmr_camera *cam, bool active) return send_buffer_to_device(cam, (uint8_t *)&cmd, sizeof(cmd)); } - static void LIBUSB_CALL img_xfer_cb(struct libusb_transfer *xfer) { @@ -311,7 +314,7 @@ img_xfer_cb(struct libusb_transfer *xfer) uint16_t unknown16 = read16(&src); uint16_t unknown16_2 = read16(&src); - WMR_CAM_DEBUG( + WMR_CAM_TRACE( cam, "Frame start TS %" PRIu64 " (%" PRIi64 " since last) end %" PRIu64 " dt %" PRIi64 " unknown %u %u", frame_start_ts, frame_start_ts - cam->last_frame_ts, frame_end_ts, delta, unknown16, unknown16_2); @@ -326,19 +329,40 @@ img_xfer_cb(struct libusb_transfer *xfer) cam->frame_sequence, exposure); xf->source_sequence = cam->frame_sequence; - xf->timestamp = xf->source_timestamp = frame_start_ts; + xf->timestamp = frame_start_ts + delta / 2; + xf->source_timestamp = frame_start_ts; cam->last_frame_ts = frame_start_ts; cam->last_seq = seq; - /* Exposure of 0 is a dark frame for controller tracking */ + /* Exposure of 0 is a dark frame for controller tracking (usually ~60fps) */ int sink_index = (exposure == 0) ? 1 : 0; if (u_sink_debug_is_active(&cam->debug_sinks[sink_index])) { u_sink_debug_push_frame(&cam->debug_sinks[sink_index], xf); } - /* TODO: Push frame for tracking */ + // Push to sinks + bool tracking_frame = sink_index == 0; + if (tracking_frame && cam->submit_frames) { + // Tracking frames usually come at ~30fps + struct xrt_frame *xf_left = NULL; + struct xrt_frame *xf_right = NULL; + u_frame_create_roi(xf, cam->configs[0].roi, &xf_left); + u_frame_create_roi(xf, cam->configs[1].roi, &xf_right); + + if (cam->left_sink != NULL) { + xrt_sink_push_frame(cam->left_sink, xf_left); + } + + if (cam->right_sink != NULL) { + xrt_sink_push_frame(cam->right_sink, xf_right); + } + + xrt_frame_reference(&xf_left, NULL); + xrt_frame_reference(&xf_right, NULL); + } + xrt_frame_reference(&xf, NULL); if (cam->last_exposure != cam->debug_exposure || cam->last_gain != cam->debug_gain) { @@ -366,7 +390,10 @@ out: */ struct wmr_camera * -wmr_camera_open(struct xrt_prober_device *dev_holo, enum u_logging_level log_level) +wmr_camera_open(struct xrt_prober_device *dev_holo, + struct xrt_frame_sink *left_sink, + struct xrt_frame_sink *right_sink, + enum u_logging_level log_level) { struct wmr_camera *cam = calloc(1, sizeof(struct wmr_camera)); int res, i; @@ -416,6 +443,12 @@ wmr_camera_open(struct xrt_prober_device *dev_holo, enum u_logging_level log_lev u_var_add_log_level(cam, &cam->log_level, "Log level"); u_var_add_u8(cam, &cam->debug_gain, "Gain"); u_var_add_u8(cam, &cam->debug_exposure, "Exposure * 200"); + u_var_add_bool(cam, &cam->submit_frames, "Submit frames"); + u_var_add_sink_debug(cam, &cam->debug_sinks[0], "Tracking Streams"); + u_var_add_sink_debug(cam, &cam->debug_sinks[1], "Controller Streams"); + + cam->left_sink = left_sink; + cam->right_sink = right_sink; return cam; diff --git a/src/xrt/drivers/wmr/wmr_camera.h b/src/xrt/drivers/wmr/wmr_camera.h index 5991b8be6..94be00a74 100644 --- a/src/xrt/drivers/wmr/wmr_camera.h +++ b/src/xrt/drivers/wmr/wmr_camera.h @@ -12,6 +12,7 @@ #include "xrt/xrt_config_have.h" #include "util/u_logging.h" #include "xrt/xrt_prober.h" +#include "xrt/xrt_frame.h" #include "wmr_config.h" @@ -23,7 +24,10 @@ struct wmr_camera; #ifdef XRT_HAVE_LIBUSB struct wmr_camera * -wmr_camera_open(struct xrt_prober_device *dev_holo, enum u_logging_level log_level); +wmr_camera_open(struct xrt_prober_device *dev_holo, + struct xrt_frame_sink *left_sink, + struct xrt_frame_sink *right_sink, + enum u_logging_level log_level); void wmr_camera_free(struct wmr_camera *cam); @@ -51,7 +55,7 @@ wmr_camera_set_exposure_gain(struct wmr_camera *cam, uint8_t camera_id, uint16_t #else /* Stubs to disable camera functions without libusb */ -#define wmr_camera_open(dev_holo, ll) NULL +#define wmr_camera_open(dev_holo, left_sink, right_sink, log_level) NULL #define wmr_camera_free(cam) #define wmr_camera_start(cam, cam_configs, n_configs) false #define wmr_camera_stop(cam) false diff --git a/src/xrt/drivers/wmr/wmr_hmd.c b/src/xrt/drivers/wmr/wmr_hmd.c index bf0c12a28..0125fa285 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.c +++ b/src/xrt/drivers/wmr/wmr_hmd.c @@ -34,6 +34,7 @@ #include "wmr_common.h" #include "wmr_config_key.h" #include "wmr_protocol.h" +#include "wmr_source.h" #include #include @@ -843,16 +844,15 @@ wmr_hmd_destroy(struct xrt_device *xdev) wh->hid_control_dev = NULL; } - if (wh->camera != NULL) { - wmr_camera_free(wh->camera); - } + // Destroy SLAM source and tracker + xrt_frame_context_destroy_nodes(&wh->slam.xfctx); // Destroy the fusion. m_imu_3dof_close(&wh->fusion.i3dof); os_mutex_destroy(&wh->fusion.mutex); - free(wh); + u_device_free(&wh->base); } static bool @@ -1012,7 +1012,7 @@ struct xrt_device * wmr_hmd_create(enum wmr_headset_type hmd_type, struct os_hid_device *hid_holo, struct os_hid_device *hid_ctrl, - struct wmr_camera *cam, + struct xrt_prober_device *dev_holo, enum u_logging_level log_level) { enum u_device_alloc_flags flags = @@ -1067,12 +1067,6 @@ wmr_hmd_create(enum wmr_headset_type hmd_type, return NULL; } - wh->slam.enabled = slam_enabled; - wh->use_slam_tracker = slam_enabled; - - wh->pose = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}}; - wh->offset = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}}; - // Setup input. wh->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE; @@ -1084,6 +1078,14 @@ wmr_hmd_create(enum wmr_headset_type hmd_type, return NULL; } + // Setup data source + wh->slam.source = wmr_source_create(&wh->slam.xfctx, dev_holo, wh->config); + wh->slam.enabled = slam_enabled; + wh->use_slam_tracker = slam_enabled; + + wh->pose = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}}; + wh->offset = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}}; + /* Now that we have the config loaded, iterate the map of known headsets and see if we have * an entry for this specific headset (otherwise the generic entry will be used) */ @@ -1223,8 +1225,17 @@ wmr_hmd_create(enum wmr_headset_type hmd_type, hololens_sensors_enable_imu(wh); //! @todo Initialize SLAM tracker + struct xrt_slam_sinks *sinks = NULL; - //! @todo Stream data source into sinks (if populated) + // Stream data source into sinks (if populated) + bool stream_started = xrt_fs_slam_stream_start(wh->slam.source, sinks); + if (!stream_started) { + //! @todo Could reach this due to !XRT_HAVE_LIBUSB but the HMD should keep working + WMR_WARN(wh, "Failed to start WMR source"); + wmr_hmd_destroy(&wh->base); + wh = NULL; + return NULL; + } // Hand over hololens sensor device to reading thread. ret = os_thread_helper_start(&wh->oth, wmr_run_thread, wh); diff --git a/src/xrt/drivers/wmr/wmr_hmd.h b/src/xrt/drivers/wmr/wmr_hmd.h index 9f339b796..8e58b287d 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.h +++ b/src/xrt/drivers/wmr/wmr_hmd.h @@ -14,6 +14,7 @@ #pragma once #include "xrt/xrt_device.h" +#include "xrt/xrt_frame.h" #include "xrt/xrt_prober.h" #include "os/os_threading.h" #include "math/m_imu_3dof.h" @@ -82,8 +83,6 @@ struct wmr_hmd //! Packet reading thread. struct os_thread_helper oth; - struct wmr_camera *camera; - enum u_logging_level log_level; /*! @@ -131,6 +130,12 @@ struct wmr_hmd struct { + //! Source of video/IMU data for SLAM + struct xrt_fs *source; + + //! Context for @ref source + struct xrt_frame_context xfctx; + //! Set at start. Whether the tracker was initialized. bool enabled; } slam; @@ -160,7 +165,7 @@ struct xrt_device * wmr_hmd_create(enum wmr_headset_type hmd_type, struct os_hid_device *hid_holo, struct os_hid_device *hid_ctrl, - struct wmr_camera *cam, + struct xrt_prober_device *dev_holo, enum u_logging_level log_level); #define WMR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) diff --git a/src/xrt/drivers/wmr/wmr_prober.c b/src/xrt/drivers/wmr/wmr_prober.c index 8752f80af..aa4da134d 100644 --- a/src/xrt/drivers/wmr/wmr_prober.c +++ b/src/xrt/drivers/wmr/wmr_prober.c @@ -196,9 +196,7 @@ wmr_found(struct xrt_prober *xp, return -1; } - struct wmr_camera *cam = wmr_camera_open(dev_holo, log_level); - - struct xrt_device *p = wmr_hmd_create(hmd_type, hid_holo, hid_companion, cam, log_level); + struct xrt_device *p = wmr_hmd_create(hmd_type, hid_holo, hid_companion, dev_holo, log_level); if (!p) { U_LOG_IFL_E(log_level, "Failed to create WMR HMD device."); return -1; diff --git a/src/xrt/drivers/wmr/wmr_source.c b/src/xrt/drivers/wmr/wmr_source.c new file mode 100644 index 000000000..154f6f6f8 --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_source.c @@ -0,0 +1,306 @@ +// Copyright 2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief WMR camera and IMU data source. + * @author Mateo de Mayo + * @ingroup drv_wmr + */ +#include "wmr_source.h" +#include "wmr_camera.h" +#include "wmr_config.h" +#include "wmr_protocol.h" + +#include "math/m_api.h" +#include "math/m_filter_fifo.h" +#include "util/u_debug.h" +#include "util/u_sink.h" +#include "util/u_var.h" +#include "xrt/xrt_tracking.h" +#include "xrt/xrt_frameserver.h" + +#include +#include + +#define WMR_SOURCE_STR "WMR Source" + +#define WMR_TRACE(w, ...) U_LOG_IFL_T(w->log_level, __VA_ARGS__) +#define WMR_DEBUG(w, ...) U_LOG_IFL_D(w->log_level, __VA_ARGS__) +#define WMR_INFO(w, ...) U_LOG_IFL_I(w->log_level, __VA_ARGS__) +#define WMR_WARN(w, ...) U_LOG_IFL_W(w->log_level, __VA_ARGS__) +#define WMR_ERROR(w, ...) U_LOG_IFL_E(w->log_level, __VA_ARGS__) +#define WMR_ASSERT(predicate, ...) \ + do { \ + bool p = predicate; \ + if (!p) { \ + U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \ + assert(false && "WMR_ASSERT failed: " #predicate); \ + exit(EXIT_FAILURE); \ + } \ + } while (false); +#define WMR_ASSERT_(predicate) WMR_ASSERT(predicate, "Assertion failed " #predicate) + +DEBUG_GET_ONCE_LOG_OPTION(wmr_log, "WMR_LOG", U_LOGGING_INFO) + +/*! + * Handles all the data sources from the WMR driver + * + * @todo Currently only properly handling tracking cameras, move IMU and other sources here + * @implements xrt_fs + * @implements xrt_frame_node + */ +struct wmr_source +{ + struct xrt_fs xfs; + struct xrt_frame_node node; + enum u_logging_level log_level; //!< Log level + + struct wmr_hmd_config config; + struct wmr_camera *camera; + + // Sinks + struct xrt_frame_sink left_sink; //!< Intermediate sink for left camera frames + struct xrt_frame_sink right_sink; //!< Intermediate sink for right camera 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 + + // UI Sinks + struct u_sink_debug ui_left_sink; //!< Sink to display left frames in UI + struct u_sink_debug ui_right_sink; //!< Sink to display right frames in UI + struct m_ff_vec3_f32 *gyro_ff; //!< Queue of gyroscope data to display in UI + struct m_ff_vec3_f32 *accel_ff; //!< Queue of accelerometer data to display in UI + + bool is_running; //!< Whether the device is streaming + bool average_imus; //!< Average 4 IMU samples before sending them to the sinks +}; + +/* + * + * Sinks functionality + * + */ + +static void +receive_left_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf) +{ + struct wmr_source *ws = container_of(sink, struct wmr_source, left_sink); + WMR_TRACE(ws, "left img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp); + u_sink_debug_push_frame(&ws->ui_left_sink, xf); + if (ws->out_sinks.left) { + xrt_sink_push_frame(ws->out_sinks.left, xf); + } +} + +static void +receive_right_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf) +{ + struct wmr_source *ws = container_of(sink, struct wmr_source, right_sink); + WMR_TRACE(ws, "right img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp); + u_sink_debug_push_frame(&ws->ui_right_sink, xf); + if (ws->out_sinks.right) { + xrt_sink_push_frame(ws->out_sinks.right, xf); + } +} + +static void +receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) +{ + struct wmr_source *ws = container_of(sink, struct wmr_source, imu_sink); + + timepoint_ns ts = s->timestamp_ns; + struct xrt_vec3_f64 a = s->accel_m_s2; + struct xrt_vec3_f64 w = s->gyro_rad_secs; + WMR_TRACE(ws, "imu t=%ld a=(%f %f %f) w=(%f %f %f)", ts, a.x, a.y, a.z, w.x, w.y, w.z); + + + // Push to debug UI + struct xrt_vec3 gyro = {(float)w.x, (float)w.y, (float)w.z}; + struct xrt_vec3 accel = {(float)a.x, (float)a.y, (float)a.z}; + m_ff_vec3_f32_push(ws->gyro_ff, &gyro, ts); + m_ff_vec3_f32_push(ws->accel_ff, &accel, ts); + + if (ws->out_sinks.imu) { + xrt_sink_push_imu(ws->out_sinks.imu, s); + } +} + + +/* + * + * Frameserver functionality + * + */ + +static inline struct wmr_source * +wmr_source_from_xfs(struct xrt_fs *xfs) +{ + struct wmr_source *ws = container_of(xfs, struct wmr_source, xfs); + return ws; +} + +static bool +wmr_source_enumerate_modes(struct xrt_fs *xfs, struct xrt_fs_mode **out_modes, uint32_t *out_count) +{ + WMR_ASSERT(false, "Not implemented"); + return false; +} + +static bool +wmr_source_configure_capture(struct xrt_fs *xfs, struct xrt_fs_capture_parameters *cp) +{ + WMR_ASSERT(false, "Not implemented"); + return false; +} + +static bool +wmr_source_stream_stop(struct xrt_fs *xfs) +{ + struct wmr_source *ws = wmr_source_from_xfs(xfs); + + bool stopped = wmr_camera_stop(ws->camera); + if (!stopped) { + WMR_ERROR(ws, "Unable to stop WMR cameras"); + WMR_ASSERT_(false); + } + + return stopped; +} + +static bool +wmr_source_is_running(struct xrt_fs *xfs) +{ + struct wmr_source *ws = wmr_source_from_xfs(xfs); + return ws->is_running; +} + +static bool +wmr_source_stream_start(struct xrt_fs *xfs, + struct xrt_frame_sink *xs, + enum xrt_fs_capture_type capture_type, + uint32_t descriptor_index) +{ + struct wmr_source *ws = wmr_source_from_xfs(xfs); + + if (xs == NULL && capture_type == XRT_FS_CAPTURE_TYPE_TRACKING) { + WMR_INFO(ws, "Starting WMR stream in tracking mode"); + } else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) { + WMR_INFO(ws, "Starting WMR stream in calibration mode, will stream only left frames"); + ws->out_sinks.left = xs; + } else { + WMR_ASSERT(false, "Unsupported stream configuration xs=%p capture_type=%d", (void *)xs, capture_type); + return false; + } + + bool started = wmr_camera_start(ws->camera, ws->config.cameras, ws->config.n_cameras); + if (!started) { + WMR_ERROR(ws, "Unable to start WMR cameras"); + WMR_ASSERT_(false); + } + + ws->is_running = started; + return ws->is_running; +} + +static bool +wmr_source_slam_stream_start(struct xrt_fs *xfs, struct xrt_slam_sinks *sinks) +{ + struct wmr_source *ws = wmr_source_from_xfs(xfs); + if (sinks != NULL) { + ws->out_sinks = *sinks; + } + return wmr_source_stream_start(xfs, NULL, XRT_FS_CAPTURE_TYPE_TRACKING, 0); +} + + +/* + * + * Frame node functionality + * + */ + +static void +wmr_source_node_break_apart(struct xrt_frame_node *node) +{ + struct wmr_source *ws = container_of(node, struct wmr_source, node); + wmr_source_stream_stop(&ws->xfs); +} + +static void +wmr_source_node_destroy(struct xrt_frame_node *node) +{ + struct wmr_source *ws = container_of(node, struct wmr_source, node); + WMR_DEBUG(ws, "Destroying WMR source"); + u_sink_debug_destroy(&ws->ui_left_sink); + u_sink_debug_destroy(&ws->ui_right_sink); + m_ff_vec3_f32_free(&ws->gyro_ff); + m_ff_vec3_f32_free(&ws->accel_ff); + u_var_remove_root(ws); + if (ws->camera != NULL) { // It could be null if XRT_HAVE_LIBUSB is not defined + wmr_camera_free(ws->camera); + } + free(ws); +} + + +/* + * + * Exported functions + * + */ + +//! Create and open the frame server for IMU/camera streaming. +struct xrt_fs * +wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev_holo, struct wmr_hmd_config cfg) +{ + struct wmr_source *ws = U_TYPED_CALLOC(struct wmr_source); + ws->log_level = debug_get_log_option_wmr_log(); + ws->average_imus = true; + + // Setup xrt_fs + struct xrt_fs *xfs = &ws->xfs; + xfs->enumerate_modes = wmr_source_enumerate_modes; + xfs->configure_capture = wmr_source_configure_capture; + xfs->stream_start = wmr_source_stream_start; + xfs->slam_stream_start = wmr_source_slam_stream_start; + xfs->stream_stop = wmr_source_stream_stop; + xfs->is_running = wmr_source_is_running; + snprintf(xfs->name, sizeof(xfs->name), WMR_SOURCE_STR); + snprintf(xfs->product, sizeof(xfs->product), WMR_SOURCE_STR " Product"); + snprintf(xfs->manufacturer, sizeof(xfs->manufacturer), WMR_SOURCE_STR " Manufacturer"); + snprintf(xfs->serial, sizeof(xfs->serial), WMR_SOURCE_STR " Serial"); + xfs->source_id = 0x574d522d53524300; // WMR_SRC\0 in hex + + // Setup sinks + ws->left_sink.push_frame = receive_left_frame; + ws->right_sink.push_frame = receive_right_frame; + ws->imu_sink.push_imu = receive_imu_sample; + ws->in_sinks.left = &ws->left_sink; + ws->in_sinks.right = &ws->right_sink; + ws->in_sinks.imu = &ws->imu_sink; + ws->camera = wmr_camera_open(dev_holo, ws->in_sinks.left, ws->in_sinks.right, ws->log_level); + ws->config = cfg; + + // Setup UI + u_sink_debug_init(&ws->ui_left_sink); + u_sink_debug_init(&ws->ui_right_sink); + m_ff_vec3_f32_alloc(&ws->gyro_ff, 1000); + m_ff_vec3_f32_alloc(&ws->accel_ff, 1000); + u_var_add_root(ws, WMR_SOURCE_STR, false); + u_var_add_log_level(ws, &ws->log_level, "Log Level"); + u_var_add_bool(ws, &ws->average_imus, "Average IMU samples"); + u_var_add_ro_ff_vec3_f32(ws, ws->gyro_ff, "Gyroscope"); + u_var_add_ro_ff_vec3_f32(ws, ws->accel_ff, "Accelerometer"); + u_var_add_sink_debug(ws, &ws->ui_left_sink, "Left Camera"); + u_var_add_sink_debug(ws, &ws->ui_right_sink, "Right Camera"); + + // Setup node + struct xrt_frame_node *xfn = &ws->node; + xfn->break_apart = wmr_source_node_break_apart; + xfn->destroy = wmr_source_node_destroy; + xrt_frame_context_add(xfctx, &ws->node); + + WMR_DEBUG(ws, "WMR Source created"); + + return xfs; +} diff --git a/src/xrt/drivers/wmr/wmr_source.h b/src/xrt/drivers/wmr/wmr_source.h new file mode 100644 index 000000000..bcc91bca3 --- /dev/null +++ b/src/xrt/drivers/wmr/wmr_source.h @@ -0,0 +1,38 @@ +// Copyright 2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Interface for WMR data sources + * @author Mateo de Mayo + * @ingroup drv_wmr + */ + +#pragma once + +#include "wmr_config.h" +#include "xrt/xrt_frameserver.h" +#include "xrt/xrt_prober.h" + +/*! + * WMR video/IMU data sources + * + * @addtogroup drv_wmr + * @{ + */ + + +#ifdef __cplusplus +extern "C" { +#endif + +//! Create and return the data source as a @ref xrt_fs ready for data streaming. +struct xrt_fs * +wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev_holo, struct wmr_hmd_config cfg); + +/*! + * @} + */ + +#ifdef __cplusplus +} +#endif