From 9d6ca80af365f3bdf642e199426c52e469decec6 Mon Sep 17 00:00:00 2001 From: Moses Turner <moses@collabora.com> Date: Sun, 11 Sep 2022 23:26:39 -0500 Subject: [PATCH] xrt: Add SimulaVR driver --- CMakeLists.txt | 3 + src/xrt/drivers/CMakeLists.txt | 11 + src/xrt/drivers/simula/svr_hmd.c | 297 ++++++++++++++++++ src/xrt/drivers/simula/svr_interface.h | 47 +++ src/xrt/targets/common/CMakeLists.txt | 8 + .../targets/common/target_builder_interface.h | 17 +- .../targets/common/target_builder_simulavr.c | 264 ++++++++++++++++ src/xrt/targets/common/target_lists.c | 4 + 8 files changed, 649 insertions(+), 2 deletions(-) create mode 100644 src/xrt/drivers/simula/svr_hmd.c create mode 100644 src/xrt/drivers/simula/svr_interface.h create mode 100644 src/xrt/targets/common/target_builder_simulavr.c diff --git a/CMakeLists.txt b/CMakeLists.txt index e286d4852..861ac52ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -299,6 +299,7 @@ option_with_deps(XRT_BUILD_DRIVER_ULV2 "Enable Ultraleap v2 driver" DEPENDS Leap option_with_deps(XRT_BUILD_DRIVER_VF "Build video frame driver (for video file support, uses gstreamer)" DEPENDS XRT_HAVE_GST) option_with_deps(XRT_BUILD_DRIVER_VIVE "Enable driver for HTC Vive, Vive Pro, Valve Index, and their controllers" DEPENDS ZLIB_FOUND XRT_HAVE_LINUX) option_with_deps(XRT_BUILD_DRIVER_WMR "Enable Windows Mixed Reality driver" DEPENDS "NOT WIN32") +option_with_deps(XRT_BUILD_DRIVER_SIMULAVR "Enable simula driver" DEPENDS XRT_HAVE_REALSENSE) option(XRT_BUILD_DRIVER_SIMULATED "Enable simulated driver" ON) option(XRT_BUILD_SAMPLES "Enable compiling sample code implementations that will not be linked into any final targets" ON) @@ -354,6 +355,7 @@ list( "QWERTY" "WMR" "EUROC" + "SIMULAVR" ) # Package name needs to be known by the native code itself. @@ -520,6 +522,7 @@ message(STATUS "# DRIVER_ULV2: ${XRT_BUILD_DRIVER_ULV2}") message(STATUS "# DRIVER_VF: ${XRT_BUILD_DRIVER_VF}") message(STATUS "# DRIVER_VIVE: ${XRT_BUILD_DRIVER_VIVE}") message(STATUS "# DRIVER_WMR: ${XRT_BUILD_DRIVER_WMR}") +message(STATUS "# DRIVER_SIMULAVR: ${XRT_BUILD_DRIVER_SIMULAVR}") message(STATUS "#####----- Config -----#####") if(XRT_FEATURE_SERVICE AND NOT XRT_FEATURE_OPENXR) diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 2115463a3..5e664ebdd 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -373,6 +373,17 @@ if(XRT_BUILD_DRIVER_EUROC) list(APPEND ENABLED_DRIVERS euroc) endif() +if(XRT_BUILD_DRIVER_SIMULAVR) + add_library( + drv_svr STATIC + simula/svr_hmd.c + simula/svr_interface.h + ) + target_link_libraries(drv_svr PRIVATE xrt-interfaces aux_math xrt-external-cjson) + list(APPEND ENABLED_HEADSET_DRIVERS svr) +endif() + + if(XRT_BUILD_SAMPLES) # We build the sample driver to make sure it stays valid, # but it never gets linked into a final target. diff --git a/src/xrt/drivers/simula/svr_hmd.c b/src/xrt/drivers/simula/svr_hmd.c new file mode 100644 index 000000000..38dfdbb66 --- /dev/null +++ b/src/xrt/drivers/simula/svr_hmd.c @@ -0,0 +1,297 @@ +// Copyright 2020, Collabora, Ltd. +// Copyright 2020, Moses Turner. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief SimulaVR driver code. + * @author Moses Turner <moses@collabora.com> + * @ingroup drv_svr + */ + +#include "math/m_mathinclude.h" + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <assert.h> + +#include "svr_interface.h" + +#include "xrt/xrt_defines.h" +#include "xrt/xrt_device.h" + +#include "math/m_api.h" +#include "math/m_space.h" +#include "math/m_vec2.h" + +#include "os/os_time.h" +#include "os/os_threading.h" + + +#include "util/u_var.h" +#include "util/u_debug.h" +#include "util/u_device.h" +#include "util/u_time.h" +#include "util/u_json.h" +#include "util/u_misc.h" +#include "util/u_logging.h" +#include "util/u_distortion_mesh.h" + + +DEBUG_GET_ONCE_LOG_OPTION(svr_log, "SIMULA_LOG", U_LOGGING_INFO) + +#define SVR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) +#define SVR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) +#define SVR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) +#define SVR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) +#define SVR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) + +struct svr_hmd +{ + struct xrt_device base; + + struct svr_two_displays_distortion distortion; + + enum u_logging_level log_level; +}; + +static inline struct svr_hmd * +svr_hmd(struct xrt_device *xdev) +{ + return (struct svr_hmd *)xdev; +} + +static void +svr_hmd_destroy(struct xrt_device *xdev) +{ + struct svr_hmd *ns = svr_hmd(xdev); + + // Remove the variable tracking. + u_var_remove_root(ns); + + u_device_free(&ns->base); +} +// +static void +svr_hmd_update_inputs(struct xrt_device *xdev) +{} + +static void +svr_hmd_get_tracked_pose(struct xrt_device *xdev, + enum xrt_input_name name, + uint64_t at_timestamp_ns, + struct xrt_space_relation *out_relation) +{ + struct svr_hmd *ns = svr_hmd(xdev); + + if (name != XRT_INPUT_GENERIC_HEAD_POSE) { + SVR_ERROR(ns, "unknown input name"); + return; + } + + + out_relation->angular_velocity = (struct xrt_vec3)XRT_VEC3_ZERO; + out_relation->linear_velocity = (struct xrt_vec3)XRT_VEC3_ZERO; + out_relation->pose = + (struct xrt_pose)XRT_POSE_IDENTITY; // This is so that tracking overrides/multi driver just transforms us by + // the tracker + offset from the tracker. + out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; +} + +#define DEG_TO_RAD(DEG) (DEG * M_PI / 180.) + +static void +svr_hmd_get_view_poses(struct xrt_device *xdev, + const struct xrt_vec3 *default_eye_relation, + uint64_t at_timestamp_ns, + uint32_t view_count, + struct xrt_space_relation *out_head_relation, + struct xrt_fov *out_fovs, + struct xrt_pose *out_poses) +{ + //!@todo: default_eye_relation inherits from the env var OXR_DEBUG_IPD_MM / oxr_session.c + // probably needs a lot more attention + + u_device_get_view_poses(xdev, default_eye_relation, at_timestamp_ns, view_count, out_head_relation, out_fovs, + out_poses); + + + + //!@todo you may need to invert this - I can't test locally + float turn_vals[2] = {5.0, -5.0}; + for (uint32_t i = 0; i < view_count && i < 2; i++) { + struct xrt_vec3 y_up = (struct xrt_vec3)XRT_VEC3_UNIT_Y; + math_quat_from_angle_vector(DEG_TO_RAD(turn_vals[i]), &y_up, &out_poses[i].orientation); + } +} + +//!@todo: remove hard-coding and move to u_distortion_mesh +bool +svr_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) +{ + struct svr_hmd *svr = svr_hmd(xdev); + + struct svr_one_display_distortion *dist = &svr->distortion.views[view]; + + struct svr_display_distortion_polynomial_values *distortion_channels[3] = {&dist->red, &dist->green, + &dist->blue}; + + + // Somewhere at the program (constants definition) + /* Display size in mm */ + // note for people expecting everything to be in meters: no, really, this is millimeters and we don't need a + // scaling factor + // float _DispDimsX = 51.7752; + // float _DispDimsY = 51.7752; + /* Half of the horizontal field of view (in radians) fovH/2 */ + float _FoVh_2 = dist->half_fov; + /* Field of view aspect ratio (fovH/fovV), equals to 1 if fovH = fovV */ + float _aspect = 1.0f; + + + // Results r/g/b. + struct xrt_vec2 tc[3] = {{0, 0}, {0, 0}, {0, 0}}; + + // Dear compiler, please vectorize. + for (int i = 0; i < 3; i++) { + + // Just before applying the polynomial (maybe before the loop or at the beginning of it) + // Denormalization: conversion from uv texture coordinates (origin at bottom left corner) to mm display + // coordinates + struct xrt_vec2 XoYo = {0, 0}; // Assuming (0,0) at the center of the display: -DispDimsX/2 <= XoYo.x <= + // DispDimsX/2; -DispDimsY <= XoYo.y <= DispDimsY + XoYo.x = dist->display_size_mm.x * (u - 0.5f); + XoYo.y = dist->display_size_mm.y * (v - 0.5f); + + struct xrt_vec2 tanH_tanV = { + 0, 0}; // Resulting angular coordinates (tan(H), tan(V)) of input image corresponding to the + // coordinates of the input texture whose color will be sampled + + float r2 = m_vec2_dot(XoYo, XoYo); + float r = sqrtf(r2); + + // 9 degree polynomial (only odd coefficients) + struct svr_display_distortion_polynomial_values *vals = distortion_channels[i]; + float k1 = vals->k1; + float k3 = vals->k3; + float k5 = vals->k5; + float k7 = vals->k7; + float k9 = vals->k9; + + float k = r * (k1 + r2 * (k3 + r2 * (k5 + r2 * (k7 + r2 * k9)))); + + // Avoid problems when r = 0 + if (r > 0) { + tanH_tanV.x = (k * XoYo.x) / r; + tanH_tanV.y = (k * XoYo.y) / r; + } else { + tanH_tanV.x = 0; + tanH_tanV.y = 0; + } + + // Normalization: Transformation from angular coordinates (tan(H), tan(V)) of input image to tc + // (normalized coordinates with origin at the bottom left corner) + tc[i].x = (tanH_tanV.x + tanf(_FoVh_2)) / (2 * tanf(_FoVh_2)); + tc[i].y = ((tanH_tanV.y + tanf(_FoVh_2) / _aspect) / (2 * tanf(_FoVh_2))) * _aspect; + + // SVR_TRACE(svr, "Distortion %f %f -> %i %f %f", u, v, i, tc[i].x, tc[i].y); + } + result->r = tc[0]; + result->g = tc[1]; + result->b = tc[2]; + + return true; +} + + +/* + * + * Create function. + * + */ + +struct xrt_device * +svr_hmd_create(struct svr_two_displays_distortion *distortion) +{ + enum u_device_alloc_flags flags = + (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE); + struct svr_hmd *svr = U_DEVICE_ALLOCATE(struct svr_hmd, flags, 1, 0); + + // Slow copy. Could refcount it but who cares, this runs once. + svr->distortion = *distortion; + + svr->log_level = debug_get_log_option_svr_log(); + + + + svr->base.update_inputs = svr_hmd_update_inputs; + svr->base.get_tracked_pose = svr_hmd_get_tracked_pose; + svr->base.get_view_poses = svr_hmd_get_view_poses; + svr->base.destroy = svr_hmd_destroy; + svr->base.name = XRT_DEVICE_GENERIC_HMD; + + // Sorta a lie, we have to do this to make the state tracker happy. (Should multi.c override these?) + svr->base.orientation_tracking_supported = true; + svr->base.position_tracking_supported = true; + + svr->base.device_type = XRT_DEVICE_TYPE_HMD; + + svr->base.hmd->screens[0].nominal_frame_interval_ns = (uint64_t)time_s_to_ns(1.0f / 90.0f); + + + // Print name. + snprintf(svr->base.str, XRT_DEVICE_NAME_LEN, "SimulaVR HMD"); + snprintf(svr->base.serial, XRT_DEVICE_NAME_LEN, "0001"); + // Setup input. + svr->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE; + + struct u_extents_2d exts; + + // one screen is 2448px wide, but there are two of them. + exts.w_pixels = 2448 * 2; + // Both screens are 2448px tall + exts.h_pixels = 2448; + + u_extents_2d_split_side_by_side(&svr->base, &exts); + + for (int view = 0; view < 2; view++) { + svr->base.hmd->distortion.fov[view].angle_left = -svr->distortion.views[view].half_fov; + svr->base.hmd->distortion.fov[view].angle_right = svr->distortion.views[view].half_fov; + svr->base.hmd->distortion.fov[view].angle_up = svr->distortion.views[view].half_fov; + svr->base.hmd->distortion.fov[view].angle_down = -svr->distortion.views[view].half_fov; + } + + u_distortion_mesh_set_none(&svr->base); + svr->base.hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE; + svr->base.hmd->distortion.preferred = XRT_DISTORTION_MODEL_COMPUTE; + svr->base.compute_distortion = svr_mesh_calc; + + // Setup variable tracker. + u_var_add_root(svr, "Simula HMD", true); + svr->base.orientation_tracking_supported = true; + svr->base.device_type = XRT_DEVICE_TYPE_HMD; + + size_t idx = 0; + + //!@todo these should be true for the final product iirc but possibly not for the demo unit + svr->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_ADDITIVE; + svr->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE; + svr->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_ALPHA_BLEND; + + svr->base.hmd->blend_mode_count = idx; + + uint64_t start; + uint64_t end; + + start = os_monotonic_get_ns(); + u_distortion_mesh_fill_in_compute(&svr->base); + end = os_monotonic_get_ns(); + + float diff = (end - start); + diff /= U_TIME_1MS_IN_NS; + + SVR_DEBUG(svr, "Filling mesh took %f ms", diff); + + + return &svr->base; +} diff --git a/src/xrt/drivers/simula/svr_interface.h b/src/xrt/drivers/simula/svr_interface.h new file mode 100644 index 000000000..9a8a784ad --- /dev/null +++ b/src/xrt/drivers/simula/svr_interface.h @@ -0,0 +1,47 @@ +// Copyright 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief SimulaVR driver interface. + * @author Moses Turner <moses@collabora.com> + * @ingroup drv_svr + */ + +#pragma once + +#include "xrt/xrt_defines.h" +#ifdef __cplusplus +extern "C" { +#endif + +struct svr_display_distortion_polynomial_values +{ + float k1; + float k3; + float k5; + float k7; + float k9; +}; + +struct svr_one_display_distortion +{ + float half_fov; + struct xrt_vec2 display_size_mm; + + struct svr_display_distortion_polynomial_values red, green, blue; +}; + +struct svr_two_displays_distortion +{ + struct svr_one_display_distortion views[2]; // left, right +}; + +// Doesn't take possession of *distortion - feel free to free it after. +struct xrt_device * +svr_hmd_create(struct svr_two_displays_distortion *distortion); + + + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/targets/common/CMakeLists.txt b/src/xrt/targets/common/CMakeLists.txt index 6c8bc4b6c..533adb04e 100644 --- a/src/xrt/targets/common/CMakeLists.txt +++ b/src/xrt/targets/common/CMakeLists.txt @@ -33,6 +33,10 @@ if(XRT_BUILD_DRIVER_SURVIVE OR XRT_BUILD_DRIVER_VIVE) target_sources(target_lists PRIVATE target_builder_lighthouse.c) endif() +if(XRT_BUILD_DRIVER_SURVIVE) + target_sources(target_lists PRIVATE target_builder_simulavr.c) +endif() + # Drivers if(XRT_BUILD_DRIVER_ARDUINO) target_link_libraries(target_lists PRIVATE drv_arduino) @@ -132,6 +136,10 @@ if(XRT_BUILD_DRIVER_EUROC) target_link_libraries(target_lists PRIVATE drv_euroc) endif() +if(XRT_BUILD_DRIVER_SIMULAVR) + target_link_libraries(target_lists PRIVATE drv_svr) +endif() + #### # Instance # diff --git a/src/xrt/targets/common/target_builder_interface.h b/src/xrt/targets/common/target_builder_interface.h index 2d27586b8..58273b5a1 100644 --- a/src/xrt/targets/common/target_builder_interface.h +++ b/src/xrt/targets/common/target_builder_interface.h @@ -28,6 +28,10 @@ #define T_BUILDER_LIGHTHOUSE #endif +#if defined(XRT_BUILD_DRIVER_SIMULAVR) || defined(XRT_DOXYGEN) +#define T_BUILDER_SIMULAVR +#endif + // Always enabled. #define T_BUILDER_LEGACY @@ -62,9 +66,18 @@ struct xrt_builder * t_builder_legacy_create(void); #endif - +#ifdef T_BUILDER_LIGHTHOUSE /*! - * Builder used as a fallback for drivers not converted to builders yet. + * Builder for Lighthouse-tracked devices (vive, index, tundra trackers, etc.) */ struct xrt_builder * t_builder_lighthouse_create(void); +#endif + +#ifdef T_BUILDER_SIMULAVR +/*! + * Builder for SimulaVR headsets + */ +struct xrt_builder * +t_builder_simula_create(void); +#endif diff --git a/src/xrt/targets/common/target_builder_simulavr.c b/src/xrt/targets/common/target_builder_simulavr.c new file mode 100644 index 000000000..eee14d09d --- /dev/null +++ b/src/xrt/targets/common/target_builder_simulavr.c @@ -0,0 +1,264 @@ +// Copyright 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Builder for SimulaVR devices + * @author Moses Turner <moses@collabora.com> + * @ingroup xrt_iface + */ + +#include "multi_wrapper/multi.h" +#include "realsense/rs_interface.h" +#include "tracking/t_hand_tracking.h" +#include "tracking/t_tracking.h" + +#include "xrt/xrt_config_drivers.h" +#include "xrt/xrt_device.h" +#include "xrt/xrt_prober.h" + +#include "util/u_builders.h" +#include "util/u_config_json.h" +#include "util/u_debug.h" +#include "util/u_device.h" +#include "util/u_sink.h" +#include "util/u_system_helpers.h" +#include "util/u_file.h" + +#include "target_builder_interface.h" + +#include "simula/svr_interface.h" +#include "v4l2/v4l2_interface.h" + +#include "xrt/xrt_frameserver.h" +#include "xrt/xrt_results.h" +#include "xrt/xrt_tracking.h" + +#include <assert.h> + +DEBUG_GET_ONCE_OPTION(simula_config_path, "SIMULA_CONFIG_PATH", NULL) +DEBUG_GET_ONCE_LOG_OPTION(svr_log, "SIMULA_LOG", U_LOGGING_WARN) + + +#define SVR_TRACE(...) U_LOG_IFL_T(debug_get_log_option_svr_log(), __VA_ARGS__) +#define SVR_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_svr_log(), __VA_ARGS__) +#define SVR_INFO(...) U_LOG_IFL_I(debug_get_log_option_svr_log(), __VA_ARGS__) +#define SVR_WARN(...) U_LOG_IFL_W(debug_get_log_option_svr_log(), __VA_ARGS__) +#define SVR_ERROR(...) U_LOG_IFL_E(debug_get_log_option_svr_log(), __VA_ARGS__) + +static const char *driver_list[] = { + "simula", +}; + +#define MOVIDIUS_VID 0x03E7 +#define MOVIDIUS_PID 0x2150 + +#define TM2_VID 0x8087 +#define TM2_PID 0x0B37 + +struct simula_builder +{ + struct xrt_builder base; + struct svr_two_displays_distortion display_distortion; +}; + +bool +process_poly_values(const cJSON *values, struct svr_display_distortion_polynomial_values *out_values) +{ + bool good = true; + good = good && u_json_get_float(u_json_get(values, "k1"), &out_values->k1); + good = good && u_json_get_float(u_json_get(values, "k3"), &out_values->k3); + good = good && u_json_get_float(u_json_get(values, "k5"), &out_values->k5); + good = good && u_json_get_float(u_json_get(values, "k7"), &out_values->k7); + good = good && u_json_get_float(u_json_get(values, "k9"), &out_values->k9); + return good; +} + +// struct svr_two_displays_distortion +// process_eye_config(cJSON *config_json) +// { + +// } + +static bool +process_config(const char *config_path, struct svr_two_displays_distortion *out_dist) +{ + FILE *file = fopen(config_path, "r"); + const char *file_content = u_file_read_content(file); + int ret = fclose(file); + if (ret != 0) { + // Apparently I have to handle this. I have no idea how this could happen or if it would be bad, so + // let's print and keep going. + U_LOG_E("Failed to close file?"); + } + + cJSON *config_json = cJSON_Parse(file_content); + + + + if (config_json == NULL) { + const char *error_ptr = cJSON_GetErrorPtr(); + U_LOG_E("The JSON file at path \"%s\" was unable to parse", config_path); + if (error_ptr != NULL) { + U_LOG_E("because of an error before %s", error_ptr); + } + free((void *)file_content); + return false; + } + free((void *)file_content); + + bool good = true; + + + const cJSON *dd = u_json_get(config_json, "display_distortion"); + + if (dd == NULL) { + good = false; + goto end; + } + + // struct svr_two_displays_distortion distortion = {0}; + + const char *eye_names[] = {"left_eye", "right_eye"}; + for (int eye = 0; eye < 2; eye++) { + const cJSON *this_eye = u_json_get(dd, eye_names[eye]); + if (this_eye == NULL) { + good = false; + goto end; + } + // u_json does its own null checking from here on out + + good = good && u_json_get_float(u_json_get(this_eye, "half_fov"), &out_dist->views[eye].half_fov); + good = good && u_json_get_float(u_json_get(this_eye, "display_size_mm_x"), + &out_dist->views[eye].display_size_mm.x); + good = good && u_json_get_float(u_json_get(this_eye, "display_size_mm_y"), + &out_dist->views[eye].display_size_mm.y); + + good = good && process_poly_values(u_json_get(this_eye, "params_red"), &out_dist->views[eye].red); + good = good && process_poly_values(u_json_get(this_eye, "params_green"), &out_dist->views[eye].green); + good = good && process_poly_values(u_json_get(this_eye, "params_blue"), &out_dist->views[eye].blue); + } + +end: + + + + cJSON_free(config_json); + + return good; +} + +static xrt_result_t +svr_estimate_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_builder_estimate *estimate) +{ + struct simula_builder *sb = (struct simula_builder *)xb; + U_ZERO(estimate); + + const char *config_path = debug_get_option_simula_config_path(); + + if (config_path == NULL) { + // No failure occurred - the user just didn't ask for Simula + return XRT_SUCCESS; + } + + bool config_valid = process_config(config_path, &sb->display_distortion); + + if (!config_valid) { + U_LOG_E("Failed to parse SimulaVR config"); + return XRT_SUCCESS; + } + + struct xrt_prober_device **xpdevs = NULL; + size_t xpdev_count = 0; + xrt_result_t xret = XRT_SUCCESS; + + // Lock the device list + xret = xrt_prober_lock_list(xp, &xpdevs, &xpdev_count); + if (xret != XRT_SUCCESS) { + return xret; + } + + bool movidius = u_builder_find_prober_device(xpdevs, xpdev_count, MOVIDIUS_VID, MOVIDIUS_PID, XRT_BUS_TYPE_USB); + bool tm2 = u_builder_find_prober_device(xpdevs, xpdev_count, TM2_VID, TM2_PID, XRT_BUS_TYPE_USB); + + if (!movidius && !tm2) { + U_LOG_E("Simula enabled but couldn't find realsense device!"); + return XRT_SUCCESS; + } + + // I think that ideally we want `movidius` - in that case I think when we grab the device, it reboots to `tm2` + + + estimate->maybe.head = true; + estimate->certain.head = true; + + + return XRT_SUCCESS; +} + +static xrt_result_t +svr_open_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_system_devices **out_xsysd) +{ + struct simula_builder *sb = (struct simula_builder *)xb; + struct u_system_devices *usysd = u_system_devices_allocate(); + xrt_result_t result = XRT_SUCCESS; + + if (out_xsysd == NULL || *out_xsysd != NULL) { + SVR_ERROR("Invalid output system pointer"); + result = XRT_ERROR_DEVICE_CREATION_FAILED; + goto end; + } + + + // The below is a garbage hack - we should remove the autoprober entirely - but I'm tired af and need to ship it + + struct xrt_device *t265_dev = rs_create_tracked_device_internal_slam(xp); + + struct xrt_device *svr_dev = svr_hmd_create(&sb->display_distortion); + + struct xrt_pose ident = XRT_POSE_IDENTITY; + + + struct xrt_device *head_device = multi_create_tracking_override( + XRT_TRACKING_OVERRIDE_ATTACHED, svr_dev, t265_dev, XRT_INPUT_GENERIC_TRACKER_POSE, &ident); + + usysd->base.roles.head = head_device; + usysd->base.xdevs[0] = usysd->base.roles.head; + usysd->base.xdev_count = 1; + + +end: + if (result == XRT_SUCCESS) { + *out_xsysd = &usysd->base; + } else { + u_system_devices_destroy(&usysd); + } + + return result; +} + +static void +svr_destroy(struct xrt_builder *xb) +{ + free(xb); +} + +/* + * + * 'Exported' functions. + * + */ + +struct xrt_builder * +t_builder_simula_create(void) +{ + struct simula_builder *sb = U_TYPED_CALLOC(struct simula_builder); + sb->base.estimate_system = svr_estimate_system; + sb->base.open_system = svr_open_system; + sb->base.destroy = svr_destroy; + sb->base.identifier = "simula"; + sb->base.name = "SimulaVR headset"; + sb->base.driver_identifiers = driver_list; + sb->base.driver_identifier_count = ARRAY_SIZE(driver_list); + + return &sb->base; +} diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c index 1cac6a272..dd834df4c 100644 --- a/src/xrt/targets/common/target_lists.c +++ b/src/xrt/targets/common/target_lists.c @@ -96,6 +96,10 @@ xrt_builder_create_func_t target_builder_list[] = { t_builder_rgb_tracking_create, #endif // T_BUILDER_RGB_TRACKING +#ifdef T_BUILDER_SIMULAVR + t_builder_simula_create, +#endif + #ifdef T_BUILDER_LIGHTHOUSE t_builder_lighthouse_create, #endif // T_BUILDER_LIGHTHOUSE