// Copyright 2019, Collabora, Ltd. // Copyright 2014, Kevin M. Godby // Copyright 2014-2018, Sensics, Inc. // SPDX-License-Identifier: BSL-1.0 /*! * @file * @brief Driver for an OSVR Hacker Dev Kit device. * * Based in part on the corresponding VRPN driver, * available under BSL-1.0. * * @author Ryan Pavlik * @author Kevin M. Godby */ #include #include #include #include #include #include #include #include "math/m_api.h" #include "xrt/xrt_device.h" #include "util/u_debug.h" #include "util/u_misc.h" #include "util/u_device.h" #include "hdk_device.h" /** * A fixed-point to float conversion function. * * Values are signed, two's-complement, if the supplied integer is. * * The conversion is effectively from the fixed-point arithmetic type known * "unambiguously" as Q INTEGER_BITS.FRACTIONAL_BITS - the number of integer * bits is not inferred, though it is checked to ensure it adds up. * * @tparam INTEGER_BITS The number of bits devoted to the integer part. * @tparam FRACTIONAL_BITS The number of bits devoted to the fractional * part. * @tparam IntegerType The input integer type, typically deduced (do not need to * specify explicitly) * @param v An input "integer" that is actually a fixed-point value. * * INTEGER_BITS and FRACTIONAL_BITS must sum to 8 * sizeof(v), the bit width of * the input integer, for unsigned values, or to one less than that (for the * sign bit) for signed values. * * Based in part on the VRPN header vrpn_FixedPoint.h, * available under BSL-1.0. */ template static inline float fromFixedPoint(IntegerType v) { constexpr size_t SIGN_BIT = std::is_signed::value ? 1 : 0; static_assert(INTEGER_BITS + FRACTIONAL_BITS + SIGN_BIT == 8 * sizeof(IntegerType), "INTEGER_BITS and FRACTIONAL_BITS, plus 1 for a sign bit " "if applicable, must sum to the input " "integer width, but do not."); return static_cast(v) / (1 << FRACTIONAL_BITS); } static inline uint16_t hdk_get_le_uint16(uint8_t *&bufPtr) { assert(bufPtr != nullptr); uint16_t ret = static_cast(*bufPtr) | (static_cast(*(bufPtr + 1)) << 8); bufPtr += 2; return ret; } static inline int16_t hdk_get_le_int16(uint8_t *&bufPtr) { return static_cast(hdk_get_le_uint16(bufPtr)); } static void hdk_device_destroy(struct xrt_device *xdev) { struct hdk_device *hd = hdk_device(xdev); if (hd->dev != NULL) { hid_close(hd->dev); hd->dev = NULL; } free(hd); } static void hdk_device_get_tracked_pose(struct xrt_device *xdev, struct xrt_space_relation *out_relation) { struct hdk_device *hd = hdk_device(xdev); uint8_t buffer[32]; auto bytesRead = hid_read(hd->dev, &(buffer[0]), sizeof(buffer)); if (bytesRead == -1) { if (!hd->disconnect_notified) { fprintf(stderr, "%s: HDK appeared to disconnect. Please quit, " "reconnect, and try again.\n", __func__); hd->disconnect_notified = true; } out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; return; } if (bytesRead != 32 && bytesRead != 16) { HDK_DEBUG(hd, "Only got %d bytes", bytesRead); out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; return; } uint8_t *buf = &(buffer[0]); #if 0 uint8_t version = uint8_t(0x0f) & *buf; uint8_t hdmi_status = (uint8_t(0xf0) & *buf) >> 4; #endif buf++; // HDMI status only valid in reports version 3. // Expecting either version 1 (100Hz) or 3 (400Hz): // https://github.com/OSVR/OSVR-HDK-MCU-Firmware/blob/master/Source%20code/Embedded/src/DeviceDrivers/BNO070_using_hostif.c#L511 // Next byte is sequence number, ignore buf++; struct xrt_quat quat; quat.x = fromFixedPoint<1, 14>(hdk_get_le_int16(buf)) * -1; quat.y = fromFixedPoint<1, 14>(hdk_get_le_int16(buf)) * -1; quat.z = fromFixedPoint<1, 14>(hdk_get_le_int16(buf)); quat.w = fromFixedPoint<1, 14>(hdk_get_le_int16(buf)); out_relation->pose.orientation = quat; /// @todo might not be accurate on some version 1 reports?? // This is in the "world" coordinate system. struct xrt_vec3 ang_vel; ang_vel.x = fromFixedPoint<6, 9>(hdk_get_le_int16(buf)); ang_vel.y = fromFixedPoint<6, 9>(hdk_get_le_int16(buf)); ang_vel.z = fromFixedPoint<6, 9>(hdk_get_le_int16(buf)); out_relation->angular_velocity = ang_vel; out_relation->relation_flags = xrt_space_relation_flags( XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); HDK_SPEW(hd, "GET_TRACKED_POSE (%f, %f, %f, %f) ANG_VEL (%f, %f, %f)", quat.x, quat.y, quat.z, quat.w, ang_vel.x, ang_vel.y, ang_vel.z); } static void hdk_device_get_view_pose(struct xrt_device *xdev, struct xrt_vec3 *eye_relation, uint32_t view_index, struct xrt_pose *out_pose) { struct xrt_pose pose = {{0.0f, 0.0f, 0.0f, 1.0f}, {0.0f, 0.0f, 0.0f}}; bool adjust = view_index == 0; pose.position.x = eye_relation->x / 2.0f; pose.position.y = eye_relation->y / 2.0f; pose.position.z = eye_relation->z / 2.0f; // Adjust for left/right while also making sure there aren't any -0.f. if (pose.position.x > 0.0f && adjust) { pose.position.x = -pose.position.x; } if (pose.position.y > 0.0f && adjust) { pose.position.y = -pose.position.y; } if (pose.position.z > 0.0f && adjust) { pose.position.z = -pose.position.z; } *out_pose = pose; } #define HDK_DEBUG_INT(hd, name, val) HDK_DEBUG(hd, "\t%s = %u", name, val) #define HDK_DEBUG_MM(hd, name, val) \ HDK_DEBUG(hd, "\t%s = %i.%02imm", name, (int32_t)(val * 1000.f), \ abs((int32_t)(val * 100000.f)) % 100) #define HDK_DEBUG_ANGLE(hd, name, val) \ HDK_DEBUG(hd, "\t%s = %f (%i)", name, val, \ (int32_t)(val * (180 / M_PI))) #define HDK_DEBUG_MAT2X2(hd, name, rot) \ HDK_DEBUG(hd, "\t%s = {%f, %f} {%f, %f}", name, rot.v[0], rot.v[1], \ rot.v[2], rot.v[3]) struct hdk_device * hdk_device_create(hid_device *dev, enum HDK_VARIANT variant, bool print_spew, bool print_debug) { struct hdk_device *hd = (struct hdk_device *)calloc(1, sizeof(struct hdk_device)); hd->base.blend_mode = XRT_BLEND_MODE_OPAQUE; hd->base.destroy = hdk_device_destroy; hd->base.get_tracked_pose = hdk_device_get_tracked_pose; hd->base.get_view_pose = hdk_device_get_view_pose; hd->dev = dev; hd->print_spew = print_spew; hd->print_debug = print_debug; if (variant == HDK_UNKNOWN) { HDK_ERROR(hd, "Don't know which HDK variant this is."); hdk_device_destroy(&hd->base); return NULL; } double hFOV; double vFOV; double hCOP = 0.5; double vCOP = 0.5; switch (variant) { case HDK_VARIANT_1_2: // Distortion optional - this is for no distortion. hFOV = 90; vFOV = 96.73; break; case HDK_VARIANT_1_3_1_4: // Non-mesh distortion. hFOV = 90; vFOV = 96.73; hCOP = 0.529; break; case HDK_VARIANT_2: // Mesh distortion (ideally) hFOV = vFOV = 92.0; break; } constexpr double DEGREES_TO_RADIANS = M_PI / 180.0; { /* right eye */ math_compute_fovs(1.0, hCOP, hFOV * DEGREES_TO_RADIANS, 1, vCOP, vFOV * DEGREES_TO_RADIANS, &hd->base.views[1].fov); } { /* left eye - just mirroring right eye now */ hd->base.views[0].fov.angle_up = hd->base.views[1].fov.angle_up; hd->base.views[0].fov.angle_down = hd->base.views[1].fov.angle_down; hd->base.views[0].fov.angle_left = -hd->base.views[1].fov.angle_right; hd->base.views[0].fov.angle_right = -hd->base.views[1].fov.angle_left; } switch (variant) { case HDK_VARIANT_2: { constexpr int panel_w = 1080; constexpr int panel_h = 1200; // Padding needed horizontally per side. constexpr int horiz_padding = (panel_h - panel_w) / 2; // HDK2 is upside down :facepalm: // clang-format off // Main display. hd->base.screens[0].w_pixels = panel_w * 2; hd->base.screens[0].h_pixels = panel_h; // Left hd->base.views[0].display.w_pixels = panel_w; hd->base.views[0].display.h_pixels = panel_h; hd->base.views[0].viewport.x_pixels = panel_w; // right half of display hd->base.views[0].viewport.y_pixels = horiz_padding; hd->base.views[0].viewport.w_pixels = panel_w; hd->base.views[0].viewport.h_pixels = panel_w; hd->base.views[0].rot = u_device_rotation_180; // Right hd->base.views[1].display.w_pixels = panel_w; hd->base.views[1].display.h_pixels = panel_h; hd->base.views[1].viewport.x_pixels = 0; hd->base.views[1].viewport.y_pixels = horiz_padding; hd->base.views[1].viewport.w_pixels = panel_w; hd->base.views[1].viewport.h_pixels = panel_w; hd->base.views[1].rot = u_device_rotation_180; // clang-format on break; } case HDK_VARIANT_1_3_1_4: // fallthrough intentional case HDK_VARIANT_1_2: { // 1080x1920 screen, with the top at the left. constexpr int panel_w = 1080; constexpr int panel_h = 1920; constexpr int panel_half_h = panel_h / 2; // clang-format off // Main display. hd->base.screens[0].w_pixels = panel_w; hd->base.screens[0].h_pixels = panel_h; // Left hd->base.views[0].display.w_pixels = panel_half_h; hd->base.views[0].display.h_pixels = panel_w; hd->base.views[0].viewport.x_pixels = 0; hd->base.views[0].viewport.y_pixels = 0;// top half of display hd->base.views[0].viewport.w_pixels = panel_w; hd->base.views[0].viewport.h_pixels = panel_half_h; hd->base.views[0].rot = u_device_rotation_left; // Right hd->base.views[1].display.w_pixels = panel_half_h; hd->base.views[1].display.h_pixels = panel_w; hd->base.views[1].viewport.x_pixels = 0; hd->base.views[1].viewport.y_pixels = panel_half_h; // bottom half of display hd->base.views[1].viewport.w_pixels = panel_w; hd->base.views[1].viewport.h_pixels = panel_half_h; hd->base.views[1].rot = u_device_rotation_left; // clang-format on break; } } // Distortion // "None" is correct or at least acceptable for 1.2. // We have coefficients for 1.3/1.4, though the mesh is better. // We only have a mesh for 2, so use "none" there until it's supported. hd->base.distortion.models = XRT_DISTORTION_MODEL_NONE; hd->base.distortion.preferred = XRT_DISTORTION_MODEL_NONE; // if (variant == HDK_VARIANT_1_3_1_4) { // hd->base.distortion.models = // xrt_distortion_model(hd->base.distortion.models | // XRT_DISTORTION_MODEL_PANOTOOLS); // hd->base.distortion.preferred = XRT_DISTORTION_MODEL_PANOTOOLS; // } if (hd->print_debug) { u_device_dump_config(&hd->base, __func__, "OSVR HDK-family Device"); } return hd; }