matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019-2023, Collabora, Ltd.
2// Copyright 2014, Kevin M. Godby
3// Copyright 2014-2018, Sensics, Inc.
4// SPDX-License-Identifier: BSL-1.0
5/*!
6 * @file
7 * @brief Driver for an OSVR Hacker Dev Kit device.
8 *
9 * Based in part on the corresponding VRPN driver,
10 * available under BSL-1.0.
11 *
12 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
13 * @author Kevin M. Godby <kevin@godby.org>
14 * @ingroup drv_hdk
15 */
16
17
18#include "math/m_mathinclude.h"
19
20#include <stdio.h>
21#include <stdlib.h>
22#include <string.h>
23#include <assert.h>
24#include <type_traits>
25
26#include "xrt/xrt_device.h"
27
28#include "os/os_hid.h"
29#include "os/os_time.h"
30
31#include "math/m_api.h"
32
33#include "util/u_debug.h"
34#include "util/u_misc.h"
35#include "util/u_device.h"
36#include "util/u_time.h"
37#include "util/u_distortion_mesh.h"
38
39#include "hdk_device.h"
40
41static constexpr uint8_t BITS_PER_BYTE = 8;
42
43DEBUG_GET_ONCE_LOG_OPTION(hdk_log, "HDK_LOG", U_LOGGING_WARN)
44
45/**
46 * A fixed-point to float conversion function.
47 *
48 * Values are signed, two's-complement, if the supplied integer is.
49 *
50 * The conversion is effectively from the fixed-point arithmetic type known
51 * "unambiguously" as Q INT_BITS.FRAC_BITS - the number of integer
52 * bits is not inferred, though it is checked to ensure it adds up.
53 *
54 * @tparam INT_BITS The number of bits devoted to the integer part.
55 * @tparam FRAC_BITS The number of bits devoted to the fractional
56 * part.
57 * @tparam IntegerType The input integer type, typically deduced (do not need to
58 * specify explicitly)
59 * @param v An input "integer" that is actually a fixed-point value.
60 *
61 * INT_BITS and FRAC_BITS must sum to 8 * sizeof(v), the bit width of
62 * the input integer, for unsigned values, or to one less than that (for the
63 * sign bit) for signed values.
64 *
65 * Based in part on the VRPN header vrpn_FixedPoint.h,
66 * available under BSL-1.0.
67 */
68template <size_t INT_BITS, size_t FRAC_BITS, typename IntegerType>
69static inline float
70fromFixedPoint(IntegerType v)
71{
72 constexpr size_t SIGN_BIT = std::is_signed<IntegerType>::value ? 1 : 0;
73 static_assert(INT_BITS + FRAC_BITS + SIGN_BIT == BITS_PER_BYTE * sizeof(IntegerType),
74 "INT_BITS and FRAC_BITS, plus 1 for a sign bit "
75 "if applicable, must sum to the input "
76 "integer width, but do not.");
77 return static_cast<float>(v) / (1 << FRAC_BITS);
78}
79
80static inline uint16_t
81hdk_get_le_uint16(uint8_t *&bufPtr)
82{
83 assert(bufPtr != nullptr);
84 uint16_t ret = static_cast<uint16_t>(*bufPtr) | (static_cast<uint16_t>(*(bufPtr + 1)) << BITS_PER_BYTE);
85 bufPtr += 2;
86 return ret;
87}
88
89static inline int16_t
90hdk_get_le_int16(uint8_t *&bufPtr)
91{
92 return static_cast<int16_t>(hdk_get_le_uint16(bufPtr));
93}
94
95static void
96hdk_device_destroy(struct xrt_device *xdev)
97{
98 struct hdk_device *hd = hdk_device(xdev);
99
100 os_thread_helper_destroy(&hd->imu_thread);
101
102 // Now that the thread is not running we can destroy the lock.
103 os_mutex_destroy(&hd->lock);
104
105 if (hd->dev != NULL) {
106 os_hid_destroy(hd->dev);
107 hd->dev = NULL;
108 }
109
110 free(hd);
111}
112
113static constexpr uint8_t MSG_LEN_LARGE = 32;
114static constexpr uint8_t MSG_LEN_SMALL = 16;
115
116static int
117hdk_device_update(struct hdk_device *hd)
118{
119 uint8_t buffer[MSG_LEN_LARGE]{};
120
121 auto bytesRead = os_hid_read(hd->dev, buffer, sizeof(buffer), 100);
122 if (bytesRead == -1) {
123 if (!hd->disconnect_notified) {
124 HDK_ERROR(hd,
125 "%s: HDK appeared to disconnect. Please "
126 "quit, reconnect, and try again.",
127 __func__);
128 hd->disconnect_notified = true;
129 }
130 hd->quat_valid = false;
131 return 0;
132 } else if (bytesRead == 0) {
133 HDK_WARN(hd, "Read 0 bytes from device");
134 return 1;
135 }
136 while (bytesRead > 0) {
137 if (bytesRead != MSG_LEN_LARGE && bytesRead != MSG_LEN_SMALL) {
138 HDK_DEBUG(hd, "Only got %d bytes", bytesRead);
139 hd->quat_valid = false;
140 return 1;
141 }
142 bytesRead = os_hid_read(hd->dev, buffer, sizeof(buffer), 0);
143 }
144
145 uint8_t *buf = &(buffer[0]);
146
147#if 0
148 uint8_t version = uint8_t(0x0f) & *buf;
149 uint8_t hdmi_status = (uint8_t(0xf0) & *buf) >> 4;
150#endif
151 buf++;
152
153 // HDMI status only valid in reports version 3.
154 // Expecting either version 1 (100Hz) or 3 (400Hz):
155 // https://github.com/OSVR/OSVR-HDK-MCU-Firmware/blob/main/Source%20code/Embedded/src/DeviceDrivers/BNO070_using_hostif.c#L511
156
157 // Next byte is sequence number, ignore
158 buf++;
159
160 struct xrt_quat quat;
161 static constexpr int INT_BITS = 1;
162 static constexpr int FRAC_BITS = 14;
163 quat.x = fromFixedPoint<INT_BITS, FRAC_BITS>(hdk_get_le_int16(buf));
164 quat.z = fromFixedPoint<INT_BITS, FRAC_BITS>(hdk_get_le_int16(buf)) * -1;
165 quat.y = fromFixedPoint<INT_BITS, FRAC_BITS>(hdk_get_le_int16(buf));
166 quat.w = fromFixedPoint<INT_BITS, FRAC_BITS>(hdk_get_le_int16(buf));
167
168// Used to produce 90 degree rotations
169#define HDK_SIN_PI_OVER_4 0.7071068f
170 struct xrt_quat rot_90_about_x
171 {
172 HDK_SIN_PI_OVER_4, 0, 0, HDK_SIN_PI_OVER_4
173 };
174 struct xrt_quat negative_90_about_y
175 {
176 0, -HDK_SIN_PI_OVER_4, 0, HDK_SIN_PI_OVER_4
177 };
178 // The flipping of components and this get us close, except we are
179 // looking 90 to the right of where we want.
180 math_quat_rotate(&quat, &rot_90_about_x, &quat);
181
182 // Fix that 90
183 math_quat_rotate(&negative_90_about_y, &quat, &quat);
184
185 hd->quat = quat;
186
187 /// @todo might not be accurate on some version 1 reports??
188
189 // This is in the "world" coordinate system.
190
191 // Note that we must "rotate" this velocity by the first transform from earlier
192 // (90 about x), hence putting it in a pure quat.
193 struct xrt_quat ang_vel_quat;
194 ang_vel_quat.x = fromFixedPoint<6, 9>(hdk_get_le_int16(buf));
195 ang_vel_quat.z = fromFixedPoint<6, 9>(hdk_get_le_int16(buf)) * -1;
196 ang_vel_quat.y = fromFixedPoint<6, 9>(hdk_get_le_int16(buf));
197 ang_vel_quat.w = 0;
198
199 // need the inverse rotation here
200 struct xrt_quat negative_90_about_x
201 {
202 - HDK_SIN_PI_OVER_4, 0, 0, HDK_SIN_PI_OVER_4
203 };
204 math_quat_rotate(&ang_vel_quat, &rot_90_about_x, &ang_vel_quat);
205 math_quat_rotate(&negative_90_about_x, &ang_vel_quat, &ang_vel_quat);
206
207 os_mutex_lock(&hd->lock);
208 hd->ang_vel_quat = ang_vel_quat;
209
210 hd->quat_valid = true;
211 os_mutex_unlock(&hd->lock);
212
213 return 1;
214}
215
216static xrt_result_t
217hdk_device_get_tracked_pose(struct xrt_device *xdev,
218 enum xrt_input_name name,
219 int64_t requested_timestamp_ns,
220 struct xrt_space_relation *out_relation)
221{
222 struct hdk_device *hd = hdk_device(xdev);
223
224 if (name != XRT_INPUT_GENERIC_HEAD_POSE) {
225 U_LOG_XDEV_UNSUPPORTED_INPUT(&hd->base, hd->log_level, name);
226 return XRT_ERROR_INPUT_UNSUPPORTED;
227 }
228
229 os_mutex_lock(&hd->lock);
230 if (!hd->quat_valid) {
231 out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE;
232 HDK_TRACE(hd, "GET_TRACKED_POSE: No pose");
233 os_mutex_unlock(&hd->lock);
234 return XRT_SUCCESS;
235 }
236
237 out_relation->pose.orientation = hd->quat;
238
239 out_relation->angular_velocity.x = hd->ang_vel_quat.x;
240 out_relation->angular_velocity.y = hd->ang_vel_quat.y;
241 out_relation->angular_velocity.z = hd->ang_vel_quat.z;
242
243 os_mutex_unlock(&hd->lock);
244
245 out_relation->relation_flags = xrt_space_relation_flags(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
246 XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT |
247 XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
248
249 HDK_TRACE(hd, "GET_TRACKED_POSE (%f, %f, %f, %f) ANG_VEL (%f, %f, %f)", hd->quat.x, hd->quat.y, hd->quat.z,
250 hd->quat.w, hd->ang_vel_quat.x, hd->ang_vel_quat.y, hd->ang_vel_quat.z);
251 return XRT_SUCCESS;
252}
253
254static void *
255hdk_device_run_thread(void *ptr)
256{
257 struct hdk_device *hd = hdk_device((struct xrt_device *)ptr);
258
259 os_thread_helper_lock(&hd->imu_thread);
260 while (os_thread_helper_is_running_locked(&hd->imu_thread)) {
261 os_thread_helper_unlock(&hd->imu_thread);
262
263 if (!hdk_device_update(hd)) {
264 return NULL;
265 }
266
267 // Just keep swimming.
268 os_thread_helper_lock(&hd->imu_thread);
269 }
270 return NULL;
271}
272
273#define HDK_DEBUG_INT(hd, name, val) HDK_DEBUG(hd, "\t%s = %u", name, val)
274
275#define HDK_DEBUG_MM(hd, name, val) \
276 HDK_DEBUG(hd, "\t%s = %i.%02imm", name, (int32_t)(val * 1000.f), abs((int32_t)(val * 100000.f)) % 100)
277
278#define HDK_DEBUG_ANGLE(hd, name, val) HDK_DEBUG(hd, "\t%s = %f (%i)", name, val, (int32_t)(val * (180 / M_PI)))
279
280#define HDK_DEBUG_MAT2X2(hd, name, rot) \
281 HDK_DEBUG(hd, "\t%s = {%f, %f} {%f, %f}", name, rot.v[0], rot.v[1], rot.v[2], rot.v[3])
282
283struct hdk_device *
284hdk_device_create(struct os_hid_device *dev, enum HDK_VARIANT variant)
285{
286 enum u_device_alloc_flags flags =
287 (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
288 struct hdk_device *hd = U_DEVICE_ALLOCATE(struct hdk_device, flags, 1, 0);
289
290 size_t idx = 0;
291 hd->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
292 hd->base.hmd->blend_mode_count = idx;
293
294 hd->base.update_inputs = u_device_noop_update_inputs;
295 hd->base.get_tracked_pose = hdk_device_get_tracked_pose;
296 hd->base.get_view_poses = u_device_get_view_poses;
297 hd->base.destroy = hdk_device_destroy;
298 hd->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
299 hd->base.name = XRT_DEVICE_GENERIC_HMD;
300 hd->dev = dev;
301 hd->log_level = debug_get_log_option_hdk_log();
302
303 snprintf(hd->base.str, XRT_DEVICE_NAME_LEN, "OSVR HDK-family Device");
304 snprintf(hd->base.serial, XRT_DEVICE_NAME_LEN, "OSVR HDK-family Device");
305
306 if (variant == HDK_UNKNOWN) {
307 HDK_ERROR(hd, "Don't know which HDK variant this is.");
308 hdk_device_destroy(&hd->base);
309 return NULL;
310 }
311
312 double hFOV;
313 double vFOV;
314 double hCOP = 0.5;
315 double vCOP = 0.5;
316
317 switch (variant) {
318 default:
319 case HDK_UNKNOWN:
320 HDK_ERROR(hd, "Don't know which HDK variant this is.");
321 hdk_device_destroy(&hd->base);
322 return NULL;
323
324 case HDK_VARIANT_1_2:
325 // Distortion optional - this is for no distortion.
326 hFOV = 90;
327 vFOV = 96.73;
328 break;
329
330 case HDK_VARIANT_1_3_1_4:
331 // Non-mesh distortion.
332 hFOV = 90;
333 vFOV = 96.73;
334 hCOP = 0.529;
335 break;
336
337 case HDK_VARIANT_2:
338 // Mesh distortion (ideally)
339 hFOV = vFOV = 92.0;
340 break;
341 }
342
343 constexpr double DEGREES_TO_RADIANS = M_PI / 180.0;
344 {
345 /* right eye */
346 math_compute_fovs(1.0, hCOP, hFOV * DEGREES_TO_RADIANS, 1, vCOP, vFOV * DEGREES_TO_RADIANS,
347 &hd->base.hmd->distortion.fov[1]);
348 }
349 {
350 /* left eye - just mirroring right eye now */
351 hd->base.hmd->distortion.fov[0].angle_up = hd->base.hmd->distortion.fov[1].angle_up;
352 hd->base.hmd->distortion.fov[0].angle_down = hd->base.hmd->distortion.fov[1].angle_down;
353
354 hd->base.hmd->distortion.fov[0].angle_left = -hd->base.hmd->distortion.fov[1].angle_right;
355 hd->base.hmd->distortion.fov[0].angle_right = -hd->base.hmd->distortion.fov[1].angle_left;
356 }
357
358 hd->base.hmd->screens[0].scanout_direction = XRT_SCANOUT_DIRECTION_NONE;
359 hd->base.hmd->screens[0].scanout_time_ns = 0;
360
361 switch (variant) {
362 case HDK_UNKNOWN: assert(!"unknown device"); break;
363
364 case HDK_VARIANT_2: {
365 hd->base.hmd->screens[0].nominal_frame_interval_ns = time_s_to_ns(1.0f / 90.0f);
366 constexpr int panel_w = 1080;
367 constexpr int panel_h = 1200;
368 // Padding needed horizontally per side.
369 constexpr int vert_padding = (panel_h - panel_w) / 2;
370 // HDK2 is upside down :facepalm:
371
372 // clang-format off
373 // Main display.
374 hd->base.hmd->screens[0].w_pixels = panel_w * 2;
375 hd->base.hmd->screens[0].h_pixels = panel_h;
376#ifndef HDK_DO_NOT_FLIP_HDK2_SCREEN
377 // Left
378 hd->base.hmd->views[0].display.w_pixels = panel_w;
379 hd->base.hmd->views[0].display.h_pixels = panel_h;
380 hd->base.hmd->views[0].viewport.x_pixels = panel_w; // right half of display
381 hd->base.hmd->views[0].viewport.y_pixels = vert_padding;
382 hd->base.hmd->views[0].viewport.w_pixels = panel_w;
383 hd->base.hmd->views[0].viewport.h_pixels = panel_w;
384 hd->base.hmd->views[0].rot = u_device_rotation_180;
385
386 // Right
387 hd->base.hmd->views[1].display.w_pixels = panel_w;
388 hd->base.hmd->views[1].display.h_pixels = panel_h;
389 hd->base.hmd->views[1].viewport.x_pixels = 0;
390 hd->base.hmd->views[1].viewport.y_pixels = vert_padding;
391 hd->base.hmd->views[1].viewport.w_pixels = panel_w;
392 hd->base.hmd->views[1].viewport.h_pixels = panel_w;
393 hd->base.hmd->views[1].rot = u_device_rotation_180;
394#else
395 // Left
396 hd->base.hmd->views[0].display.w_pixels = panel_w;
397 hd->base.hmd->views[0].display.h_pixels = panel_h;
398 hd->base.hmd->views[0].viewport.x_pixels = 0;
399 hd->base.hmd->views[0].viewport.y_pixels = vert_padding;
400 hd->base.hmd->views[0].viewport.w_pixels = panel_w;
401 hd->base.hmd->views[0].viewport.h_pixels = panel_w;
402 hd->base.hmd->views[0].rot = u_device_rotation_ident;
403
404 // Right
405 hd->base.hmd->views[1].display.w_pixels = panel_w;
406 hd->base.hmd->views[1].display.h_pixels = panel_h;
407 hd->base.hmd->views[1].viewport.x_pixels = panel_w;
408 hd->base.hmd->views[1].viewport.y_pixels = vert_padding;
409 hd->base.hmd->views[1].viewport.w_pixels = panel_w;
410 hd->base.hmd->views[1].viewport.h_pixels = panel_w;
411 hd->base.hmd->views[1].rot = u_device_rotation_ident;
412#endif
413
414 // clang-format on
415 break;
416 }
417 case HDK_VARIANT_1_3_1_4:
418 // fallthrough intentional
419 case HDK_VARIANT_1_2: {
420 // 1080x1920 screen, with the top at the left.
421 hd->base.hmd->screens[0].nominal_frame_interval_ns = time_s_to_ns(1.0f / 60.0f);
422
423 constexpr int panel_w = 1080;
424 constexpr int panel_h = 1920;
425 constexpr int panel_half_h = panel_h / 2;
426 // clang-format off
427 // Main display.
428 hd->base.hmd->screens[0].w_pixels = panel_w;
429 hd->base.hmd->screens[0].h_pixels = panel_h;
430
431 // Left
432 hd->base.hmd->views[0].display.w_pixels = panel_half_h;
433 hd->base.hmd->views[0].display.h_pixels = panel_w;
434 hd->base.hmd->views[0].viewport.x_pixels = 0;
435 hd->base.hmd->views[0].viewport.y_pixels = 0;// top half of display
436 hd->base.hmd->views[0].viewport.w_pixels = panel_w;
437 hd->base.hmd->views[0].viewport.h_pixels = panel_half_h;
438 hd->base.hmd->views[0].rot = u_device_rotation_left;
439
440 // Right
441 hd->base.hmd->views[1].display.w_pixels = panel_half_h;
442 hd->base.hmd->views[1].display.h_pixels = panel_w;
443 hd->base.hmd->views[1].viewport.x_pixels = 0;
444 hd->base.hmd->views[1].viewport.y_pixels = panel_half_h; // bottom half of display
445 hd->base.hmd->views[1].viewport.w_pixels = panel_w;
446 hd->base.hmd->views[1].viewport.h_pixels = panel_half_h;
447 hd->base.hmd->views[1].rot = u_device_rotation_left;
448 // clang-format on
449 break;
450 }
451 }
452
453 // Distortion
454 // "None" is correct or at least acceptable for 1.2.
455 // We have coefficients for 1.3/1.4, though the mesh is better.
456 // We only have a mesh for 2, so use "none" there until it's supported.
457 // Distortion information, fills in xdev->compute_distortion().
458 u_distortion_mesh_set_none(&hd->base);
459 // if (variant == HDK_VARIANT_1_3_1_4) {
460 // hd->base.hmd->distortion.models =
461 // xrt_distortion_model(hd->base.hmd->distortion.models |
462 // XRT_DISTORTION_MODEL_PANOTOOLS);
463 // hd->base.hmd->distortion.preferred =
464 // XRT_DISTORTION_MODEL_PANOTOOLS;
465 // }
466
467 int ret = os_thread_helper_init(&hd->imu_thread);
468 if (ret != 0) {
469 HDK_ERROR(hd, "Failed to start imu thread!");
470 hdk_device_destroy((struct xrt_device *)hd);
471 return 0;
472 }
473
474 if (hd->dev) {
475 // Mutex before thread.
476 ret = os_mutex_init(&hd->lock);
477 if (ret != 0) {
478 HDK_ERROR(hd, "Failed to init mutex!");
479 hdk_device_destroy(&hd->base);
480 return NULL;
481 }
482
483 ret = os_thread_helper_start(&hd->imu_thread, hdk_device_run_thread, hd);
484 if (ret != 0) {
485 HDK_ERROR(hd, "Failed to start mainboard thread!");
486 hdk_device_destroy((struct xrt_device *)hd);
487 return 0;
488 }
489 }
490
491 if (hd->log_level <= U_LOGGING_DEBUG) {
492 u_device_dump_config(&hd->base, __func__, hd->base.str);
493 }
494
495 hd->base.supported.orientation_tracking = true;
496 hd->base.supported.position_tracking = false;
497 hd->base.device_type = XRT_DEVICE_TYPE_HMD;
498
499 return hd;
500}