From 6b2a0e76f0fa0f9e3fbf628cd2dff52a5714646e Mon Sep 17 00:00:00 2001 From: Rylie Pavlik Date: Tue, 19 Dec 2023 09:45:18 -0600 Subject: [PATCH] d/sample: Clean up sample HMD, use m_relation_history, populate mesh --- src/xrt/drivers/sample/sample_hmd.c | 155 +++++++++++++--------- src/xrt/drivers/sample/sample_interface.h | 18 ++- src/xrt/drivers/sample/sample_prober.c | 29 ++-- 3 files changed, 116 insertions(+), 86 deletions(-) diff --git a/src/xrt/drivers/sample/sample_hmd.c b/src/xrt/drivers/sample/sample_hmd.c index fa114c481..def7d2775 100644 --- a/src/xrt/drivers/sample/sample_hmd.c +++ b/src/xrt/drivers/sample/sample_hmd.c @@ -1,4 +1,4 @@ -// Copyright 2020-2021, Collabora, Ltd. +// Copyright 2020-2024, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file @@ -12,17 +12,21 @@ * @ingroup drv_sample */ +#include "os/os_time.h" +#include "xrt/xrt_defines.h" #include "xrt/xrt_device.h" +#include "math/m_relation_history.h" #include "math/m_api.h" #include "math/m_mathinclude.h" // IWYU pragma: keep -#include "util/u_var.h" -#include "util/u_time.h" #include "util/u_debug.h" #include "util/u_device.h" -#include "util/u_logging.h" #include "util/u_distortion_mesh.h" +#include "util/u_logging.h" +#include "util/u_misc.h" +#include "util/u_time.h" +#include "util/u_var.h" #include "util/u_visibility_mask.h" #include "xrt/xrt_results.h" @@ -47,6 +51,9 @@ struct sample_hmd struct xrt_pose pose; enum u_logging_level log_level; + + // has built-in mutex so thread safe + struct m_relation_history *relation_hist; }; @@ -59,26 +66,30 @@ sample_hmd(struct xrt_device *xdev) DEBUG_GET_ONCE_LOG_OPTION(sample_log, "SAMPLE_LOG", U_LOGGING_WARN) -#define SH_TRACE(p, ...) U_LOG_XDEV_IFL_T(&sh->base, sh->log_level, __VA_ARGS__) -#define SH_DEBUG(p, ...) U_LOG_XDEV_IFL_D(&sh->base, sh->log_level, __VA_ARGS__) -#define SH_ERROR(p, ...) U_LOG_XDEV_IFL_E(&sh->base, sh->log_level, __VA_ARGS__) +#define HMD_TRACE(hmd, ...) U_LOG_XDEV_IFL_T(&hmd->base, hmd->log_level, __VA_ARGS__) +#define HMD_DEBUG(hmd, ...) U_LOG_XDEV_IFL_D(&hmd->base, hmd->log_level, __VA_ARGS__) +#define HMD_INFO(hmd, ...) U_LOG_XDEV_IFL_I(&hmd->base, hmd->log_level, __VA_ARGS__) +#define HMD_ERROR(hmd, ...) U_LOG_XDEV_IFL_E(&hmd->base, hmd->log_level, __VA_ARGS__) static void sample_hmd_destroy(struct xrt_device *xdev) { - struct sample_hmd *sh = sample_hmd(xdev); + struct sample_hmd *hmd = sample_hmd(xdev); // Remove the variable tracking. - u_var_remove_root(sh); + u_var_remove_root(hmd); - u_device_free(&sh->base); + + m_relation_history_destroy(&hmd->relation_hist); + + u_device_free(&hmd->base); } static void sample_hmd_update_inputs(struct xrt_device *xdev) { /* - * Empty for the sampler driver, if you need to you should + * Empty for the sample driver, if you need to you should * put code to update the attached inputs fields. If not you can use * the u_device_noop_update_inputs helper to make it a no-op. */ @@ -90,19 +101,29 @@ sample_hmd_get_tracked_pose(struct xrt_device *xdev, uint64_t at_timestamp_ns, struct xrt_space_relation *out_relation) { - struct sample_hmd *sh = sample_hmd(xdev); + struct sample_hmd *hmd = sample_hmd(xdev); if (name != XRT_INPUT_GENERIC_HEAD_POSE) { - SH_ERROR(sh, "unknown input name"); + HMD_ERROR(hmd, "unknown input name"); return; } - // Estimate pose at timestamp at_timestamp_ns! - math_quat_normalize(&sh->pose.orientation); - out_relation->pose = sh->pose; - out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | - XRT_SPACE_RELATION_POSITION_VALID_BIT | - XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + struct xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; + + enum m_relation_history_result history_result = + m_relation_history_get(hmd->relation_hist, at_timestamp_ns, &relation); + if (history_result == M_RELATION_HISTORY_RESULT_INVALID) { + // If you get in here, it means you did not push any poses into the relation history. + // You may want to handle this differently. + HMD_ERROR(hmd, "Internal error: no poses pushed?"); + } + + if ((relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0) { + // If we provide an orientation, make sure that it is normalized. + math_quat_normalize(&relation.pose.orientation); + } + + *out_relation = relation; } static void @@ -128,13 +149,6 @@ sample_hmd_get_view_poses(struct xrt_device *xdev, out_poses); // } -bool -sample_hmd_compute_distortion( - struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *out_result) -{ - return u_compute_distortion_none(u, u, out_result); -} - xrt_result_t sample_hmd_get_visibility_mask(struct xrt_device *xdev, enum xrt_visibility_mask_type type, @@ -153,37 +167,44 @@ sample_hmd_create(void) enum u_device_alloc_flags flags = (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE); - struct sample_hmd *sh = U_DEVICE_ALLOCATE(struct sample_hmd, flags, 1, 0); + struct sample_hmd *hmd = U_DEVICE_ALLOCATE(struct sample_hmd, flags, 1, 0); // This list should be ordered, most preferred first. size_t idx = 0; - sh->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE; - sh->base.hmd->blend_mode_count = idx; + hmd->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE; + hmd->base.hmd->blend_mode_count = idx; - sh->base.update_inputs = sample_hmd_update_inputs; - sh->base.get_tracked_pose = sample_hmd_get_tracked_pose; - sh->base.get_view_poses = sample_hmd_get_view_poses; - sh->base.compute_distortion = sample_hmd_compute_distortion; - sh->base.get_visibility_mask = sample_hmd_get_visibility_mask; - sh->base.destroy = sample_hmd_destroy; + hmd->base.update_inputs = sample_hmd_update_inputs; + hmd->base.get_tracked_pose = sample_hmd_get_tracked_pose; + hmd->base.get_view_poses = sample_hmd_get_view_poses; + hmd->base.get_visibility_mask = sample_hmd_get_visibility_mask; + hmd->base.destroy = sample_hmd_destroy; - sh->pose = (struct xrt_pose)XRT_POSE_IDENTITY; - sh->log_level = debug_get_log_option_sample_log(); + // Distortion information, fills in xdev->compute_distortion(). + u_distortion_mesh_set_none(&hmd->base); + + // populate this with something more complex if required + // hmd->base.compute_distortion = sample_hmd_compute_distortion; + + hmd->pose = (struct xrt_pose)XRT_POSE_IDENTITY; + hmd->log_level = debug_get_log_option_sample_log(); // Print name. - snprintf(sh->base.str, XRT_DEVICE_NAME_LEN, "Sample HMD"); - snprintf(sh->base.serial, XRT_DEVICE_NAME_LEN, "Sample HMD S/N"); + snprintf(hmd->base.str, XRT_DEVICE_NAME_LEN, "Sample HMD"); + snprintf(hmd->base.serial, XRT_DEVICE_NAME_LEN, "Sample HMD S/N"); + + m_relation_history_create(&hmd->relation_hist); // Setup input. - sh->base.name = XRT_DEVICE_GENERIC_HMD; - sh->base.device_type = XRT_DEVICE_TYPE_HMD; - sh->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE; - sh->base.orientation_tracking_supported = true; - sh->base.position_tracking_supported = false; + hmd->base.name = XRT_DEVICE_GENERIC_HMD; + hmd->base.device_type = XRT_DEVICE_TYPE_HMD; + hmd->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE; + hmd->base.orientation_tracking_supported = true; + hmd->base.position_tracking_supported = true; // Set up display details // refresh rate - sh->base.hmd->screens[0].nominal_frame_interval_ns = time_s_to_ns(1.0f / 90.0f); + hmd->base.hmd->screens[0].nominal_frame_interval_ns = time_s_to_ns(1.0f / 90.0f); const double hFOV = 90 * (M_PI / 180.0); const double vFOV = 96.73 * (M_PI / 180.0); @@ -192,46 +213,52 @@ sample_hmd_create(void) const double vCOP = 0.5; if ( /* right eye */ - !math_compute_fovs(1, hCOP, hFOV, 1, vCOP, vFOV, &sh->base.hmd->distortion.fov[1]) || + !math_compute_fovs(1, hCOP, hFOV, 1, vCOP, vFOV, &hmd->base.hmd->distortion.fov[1]) || /* * left eye - same as right eye, except the horizontal center of projection is moved in the opposite * direction now */ - !math_compute_fovs(1, 1.0 - hCOP, hFOV, 1, vCOP, vFOV, &sh->base.hmd->distortion.fov[0])) { + !math_compute_fovs(1, 1.0 - hCOP, hFOV, 1, vCOP, vFOV, &hmd->base.hmd->distortion.fov[0])) { // If those failed, it means our math was impossible. - SH_ERROR(sh, "Failed to setup basic device info"); - sample_hmd_destroy(&sh->base); + HMD_ERROR(hmd, "Failed to setup basic device info"); + sample_hmd_destroy(&hmd->base); return NULL; } const int panel_w = 1080; const int panel_h = 1200; // Single "screen" (always the case) - sh->base.hmd->screens[0].w_pixels = panel_w * 2; - sh->base.hmd->screens[0].h_pixels = panel_h; + hmd->base.hmd->screens[0].w_pixels = panel_w * 2; + hmd->base.hmd->screens[0].h_pixels = panel_h; // Left, Right for (uint8_t eye = 0; eye < 2; ++eye) { - sh->base.hmd->views[eye].display.w_pixels = panel_w; - sh->base.hmd->views[eye].display.h_pixels = panel_h; - sh->base.hmd->views[eye].viewport.y_pixels = 0; - sh->base.hmd->views[eye].viewport.w_pixels = panel_w; - sh->base.hmd->views[eye].viewport.h_pixels = panel_h; + hmd->base.hmd->views[eye].display.w_pixels = panel_w; + hmd->base.hmd->views[eye].display.h_pixels = panel_h; + hmd->base.hmd->views[eye].viewport.y_pixels = 0; + hmd->base.hmd->views[eye].viewport.w_pixels = panel_w; + hmd->base.hmd->views[eye].viewport.h_pixels = panel_h; // if rotation is not identity, the dimensions can get more complex. - sh->base.hmd->views[eye].rot = u_device_rotation_ident; + hmd->base.hmd->views[eye].rot = u_device_rotation_ident; } // left eye starts at x=0, right eye starts at x=panel_width - sh->base.hmd->views[0].viewport.x_pixels = 0; - sh->base.hmd->views[1].viewport.x_pixels = panel_w; + hmd->base.hmd->views[0].viewport.x_pixels = 0; + hmd->base.hmd->views[1].viewport.x_pixels = panel_w; // Distortion information, fills in xdev->compute_distortion(). - u_distortion_mesh_set_none(&sh->base); + u_distortion_mesh_set_none(&hmd->base); + + // Just put an initial identity value in the tracker + struct xrt_space_relation identity = XRT_SPACE_RELATION_ZERO; + identity.relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); + uint64_t now = os_monotonic_get_ns(); + m_relation_history_push(hmd->relation_hist, &identity, now); // Setup variable tracker: Optional but useful for debugging - u_var_add_root(sh, "Sample HMD", true); - u_var_add_pose(sh, &sh->pose, "pose"); - u_var_add_log_level(sh, &sh->log_level, "log_level"); + u_var_add_root(hmd, "Sample HMD", true); + u_var_add_log_level(hmd, &hmd->log_level, "log_level"); - return &sh->base; + return &hmd->base; } diff --git a/src/xrt/drivers/sample/sample_interface.h b/src/xrt/drivers/sample/sample_interface.h index 084b185ad..b217f083f 100644 --- a/src/xrt/drivers/sample/sample_interface.h +++ b/src/xrt/drivers/sample/sample_interface.h @@ -1,8 +1,8 @@ -// Copyright 2020-2021, Collabora, Ltd. +// Copyright 2020-2024, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file - * @brief Interface to sample driver. + * @brief Interface to Sample HMD driver. * @author Jakob Bornecrantz * @author Rylie Pavlik * @ingroup drv_sample @@ -15,10 +15,13 @@ extern "C" { #endif /*! - * @defgroup drv_sample Sample driver + * @defgroup drv_sample Sample HMD driver * @ingroup drv * - * @brief Simple do-nothing sample driver, that cannot be detected by USB VID/PID + * @brief Driver for a Sample HMD. + * + * Does no actual work. + * Assumed to not be detectable by USB VID/PID, * and thus exposes an "auto-prober" to explicitly discover the device. * * See @ref writing-driver for additional information. @@ -30,10 +33,11 @@ extern "C" { * you can skip the @ref xrt_auto_prober implementation, and instead implement a * "found" function that matches the signature expected by xrt_prober_entry::found. * See for example @ref hdk_found. + * Alternately, you might create a builder or an instance implementation directly. */ /*! - * Create a auto prober for a sample device. + * Create a auto prober for a Sample HMD. * * @ingroup drv_sample */ @@ -41,10 +45,10 @@ struct xrt_auto_prober * sample_create_auto_prober(void); /*! - * Create a sample hmd. + * Create a Sample HMD. * * This is only exposed so that the prober (in one source file) - * can call the construction function (in another.) + * can call the construction function (in another) * @ingroup drv_sample */ struct xrt_device * diff --git a/src/xrt/drivers/sample/sample_prober.c b/src/xrt/drivers/sample/sample_prober.c index 027030a24..e0aed0036 100644 --- a/src/xrt/drivers/sample/sample_prober.c +++ b/src/xrt/drivers/sample/sample_prober.c @@ -1,8 +1,8 @@ -// Copyright 2020-2021, Collabora, Ltd. +// Copyright 2020-2024, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file - * @brief Sample prober code. + * @brief "auto-prober" for Sample HMD that can be autodetected but not through USB VID/PID. * @author Jakob Bornecrantz * @ingroup drv_sample */ @@ -10,7 +10,6 @@ #include "xrt/xrt_prober.h" #include "util/u_misc.h" -#include "util/u_debug.h" #include "sample_interface.h" @@ -25,18 +24,18 @@ struct sample_auto_prober //! @private @memberof sample_auto_prober static inline struct sample_auto_prober * -sample_auto_prober(struct xrt_auto_prober *p) +sample_auto_prober(struct xrt_auto_prober *xap) { - return (struct sample_auto_prober *)p; + return (struct sample_auto_prober *)xap; } //! @private @memberof sample_auto_prober static void sample_auto_prober_destroy(struct xrt_auto_prober *p) { - struct sample_auto_prober *sap = sample_auto_prober(p); + struct sample_auto_prober *ap = sample_auto_prober(p); - free(sap); + free(ap); } //! @public @memberof sample_auto_prober @@ -47,10 +46,10 @@ sample_auto_prober_autoprobe(struct xrt_auto_prober *xap, struct xrt_prober *xp, struct xrt_device **out_xdevs) { - struct sample_auto_prober *sap = sample_auto_prober(xap); - (void)sap; + struct sample_auto_prober *ap = sample_auto_prober(xap); + (void)ap; - // Do not create a sample HMD if we are not looking for HMDs. + // Do not create an HMD device if we are not looking for HMDs. if (no_hmds) { return 0; } @@ -62,10 +61,10 @@ sample_auto_prober_autoprobe(struct xrt_auto_prober *xap, struct xrt_auto_prober * sample_create_auto_prober(void) { - struct sample_auto_prober *sap = U_TYPED_CALLOC(struct sample_auto_prober); - sap->base.name = "Sample"; - sap->base.destroy = sample_auto_prober_destroy; - sap->base.lelo_dallas_autoprobe = sample_auto_prober_autoprobe; + struct sample_auto_prober *ap = U_TYPED_CALLOC(struct sample_auto_prober); + ap->base.name = "Sample HMD Auto-Prober"; + ap->base.destroy = sample_auto_prober_destroy; + ap->base.lelo_dallas_autoprobe = sample_auto_prober_autoprobe; - return &sap->base; + return &ap->base; }