+288 -11

src/xrt/drivers/blubur_s1/blubur_s1_hmd.c

··· 9 9 10 10 #include "math/m_api.h" 11 11 #include "math/m_mathinclude.h" 12 12 + #include "math/m_clock_tracking.h" 12 13 13 14 #include "util/u_device.h" 14 15 #include "util/u_distortion_mesh.h" 15 16 #include "util/u_misc.h" 16 17 #include "util/u_time.h" 17 18 #include "util/u_debug.h" 19 19 + #include "util/u_trace_marker.h" 20 20 + #include "util/u_linux.h" 18 21 19 22 #include "blubur_s1_interface.h" 20 23 #include "blubur_s1_internal.h" 24 24 + #include "blubur_s1_protocol.h" 21 25 22 26 23 27 DEBUG_GET_ONCE_BOOL_OPTION(blubur_s1_test_distortion, "BLUBUR_S1_TEST_DISTORTION", false) 28 28 + DEBUG_GET_ONCE_LOG_OPTION(blubur_s1_log, "BLUBUR_S1_LOG", U_LOGGING_INFO) 29 29 + 30 30 + #define S1_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) 31 31 + #define S1_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) 32 32 + #define S1_DEBUG_HEX(d, data, data_size) U_LOG_XDEV_IFL_D_HEX(&d->base, d->log_level, data, data_size) 33 33 + #define S1_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) 34 34 + #define S1_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) 35 35 + #define S1_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) 24 36 25 37 #define VIEW_SIZE (1440) 26 38 #define PANEL_WIDTH (VIEW_SIZE * 2) ··· 35 47 blubur_s1_hmd_destroy(struct xrt_device *xdev) 36 48 { 37 49 struct blubur_s1_hmd *hmd = blubur_s1_hmd(xdev); 50 50 + 51 51 + if (hmd->thread.initialized) { 52 52 + os_thread_helper_destroy(&hmd->thread); 53 53 + } 54 54 + 55 55 + if (hmd->dev != NULL) { 56 56 + os_hid_destroy(hmd->dev); 57 57 + hmd->dev = NULL; 58 58 + } 59 59 + 60 60 + m_imu_3dof_close(&hmd->fusion_3dof); 61 61 + 62 62 + if (hmd->relation_history != NULL) { 63 63 + m_relation_history_destroy(&hmd->relation_history); 64 64 + } 65 65 + 38 66 free(hmd); 39 67 } 40 68 ··· 88 116 int64_t at_timestamp_ns, 89 117 struct xrt_space_relation *out_relation) 90 118 { 119 119 + struct blubur_s1_hmd *hmd = blubur_s1_hmd(xdev); 120 120 + 91 121 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 92 122 return XRT_ERROR_INPUT_UNSUPPORTED; 93 123 } 94 124 95 95 - // TODO: track pose 96 96 - *out_relation = (struct xrt_space_relation){ 97 97 - .relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT, 98 98 - .pose = 99 99 - { 100 100 - .orientation = XRT_QUAT_IDENTITY, 101 101 - }, 102 102 - }; 125 125 + m_relation_history_get(hmd->relation_history, at_timestamp_ns, out_relation); 103 126 104 127 return XRT_SUCCESS; 105 128 } ··· 227 250 for (int i = 0; i < 2; i++) { 228 251 math_matrix_3x3_inverse(&affine_xform[i], &hmd->poly_3k_values[i].inv_affine_xform); 229 252 253 253 + struct xrt_fov *fov = &hmd->base.hmd->distortion.fov[i]; 254 254 + 230 255 u_compute_distortion_bounds_poly_3k( 231 231 - &hmd->poly_3k_values[i].inv_affine_xform, hmd->poly_3k_values[i].channels, i, 232 232 - &hmd->base.hmd->distortion.fov[i], &hmd->poly_3k_values[i].tex_x_range, 233 233 - &hmd->poly_3k_values[i].tex_y_range); 256 256 + &hmd->poly_3k_values[i].inv_affine_xform, hmd->poly_3k_values[i].channels, i, fov, 257 257 + &hmd->poly_3k_values[i].tex_x_range, &hmd->poly_3k_values[i].tex_y_range); 258 258 + 259 259 + S1_INFO(hmd, "FoV eye %d angles left %f right %f down %f up %f", i, fov->angle_left, fov->angle_right, 260 260 + fov->angle_down, fov->angle_up); 261 261 + } 262 262 + } 263 263 + 264 264 + struct read_context 265 265 + { 266 266 + uint8_t *data; 267 267 + size_t size; 268 268 + int read; 269 269 + }; 270 270 + 271 271 + static void 272 272 + read_into(struct read_context *ctx, void *dest, size_t size) 273 273 + { 274 274 + if (ctx->read + size > ctx->size) { 275 275 + size = ctx->size - ctx->read; 276 276 + } 277 277 + 278 278 + memcpy(dest, ctx->data + ctx->read, size); 279 279 + ctx->read += size; 280 280 + } 281 281 + 282 282 + #define READ_STRUCTURE(ctx, dst) read_into(ctx, dst, sizeof(*dst)) 283 283 + 284 284 + //! Sign-extend a 21-bit signed integer stored in a 32-bit unsigned integer 285 285 + static int32_t 286 286 + sign21(uint32_t v) 287 287 + { 288 288 + v &= 0x1FFFFF; // keep 21 bits 289 289 + if (v & 0x100000) // sign bit 290 290 + v |= 0xFFE00000; // extend sign to 32 bits 291 291 + return (int32_t)v; 292 292 + } 293 293 + 294 294 + //! Decode 3×21-bit ints from 8 bytes (little-endian packing) 295 295 + static void 296 296 + blubur_s1_decode_triad(const uint8_t b[8], int32_t out_xyz[3]) 297 297 + { 298 298 + uint32_t x = ((uint32_t)(b[2] >> 3)) | (((uint32_t)b[1] | ((uint32_t)b[0] << 8)) << 5); 299 299 + 300 300 + uint32_t y = ((uint32_t)(b[5] >> 6)) | 301 301 + (((uint32_t)b[4] | (((uint32_t)b[3] | (((uint32_t)b[2] & 0x07) << 8)) << 8)) << 2); 302 302 + 303 303 + uint32_t z = ((uint32_t)(b[7] >> 1)) | (((uint32_t)b[6] | (((uint32_t)b[5] & 0x3F) << 8)) << 7); 304 304 + 305 305 + out_xyz[0] = sign21(x); 306 306 + out_xyz[1] = sign21(y); 307 307 + out_xyz[2] = sign21(z); 308 308 + } 309 309 + 310 310 + struct blubur_s1_decoded_sample 311 311 + { 312 312 + int32_t accel[3]; // X,Y,Z 313 313 + int32_t gyro[3]; // X,Y,Z 314 314 + }; 315 315 + 316 316 + static void 317 317 + blubur_s1_decode_sample_pack(const uint8_t sample_pack[32], struct blubur_s1_decoded_sample out_samples[2]) 318 318 + { 319 319 + int32_t triad[3]; 320 320 + 321 321 + // Triad 0: Accel (sample 0) 322 322 + blubur_s1_decode_triad(&sample_pack[0], triad); 323 323 + out_samples[0].accel[0] = triad[0]; 324 324 + out_samples[0].accel[1] = triad[1]; 325 325 + out_samples[0].accel[2] = triad[2]; 326 326 + 327 327 + // Triad 1: Gyro (sample 0) 328 328 + blubur_s1_decode_triad(&sample_pack[8], triad); 329 329 + out_samples[0].gyro[0] = triad[0]; 330 330 + out_samples[0].gyro[1] = triad[1]; 331 331 + out_samples[0].gyro[2] = triad[2]; 332 332 + 333 333 + // Triad 2: Accel (sample 1) 334 334 + blubur_s1_decode_triad(&sample_pack[16], triad); 335 335 + out_samples[1].accel[0] = triad[0]; 336 336 + out_samples[1].accel[1] = triad[1]; 337 337 + out_samples[1].accel[2] = triad[2]; 338 338 + 339 339 + // Triad 3: Gyro (sample 1) 340 340 + blubur_s1_decode_triad(&sample_pack[24], triad); 341 341 + out_samples[1].gyro[0] = triad[0]; 342 342 + out_samples[1].gyro[1] = triad[1]; 343 343 + out_samples[1].gyro[2] = triad[2]; 344 344 + } 345 345 + 346 346 + static void 347 347 + blubur_s1_convert_accel_sample(const int32_t in[3], struct xrt_vec3 *out) 348 348 + { 349 349 + const float scale = 9.80665f / 8192.0f; 350 350 + 351 351 + out->x = in[0] * scale; 352 352 + out->y = in[1] * scale; 353 353 + out->z = in[2] * scale; 354 354 + } 355 355 + 356 356 + static void 357 357 + blubur_s1_convert_gyro_sample(const int32_t in[3], struct xrt_vec3 *out) 358 358 + { 359 359 + // NOTE: this was done by hand, so may not be perfect 360 360 + const float scale = (float)M_PI / (180.0f * 90.0); 361 361 + 362 362 + out->x = in[0] * scale; 363 363 + out->y = in[1] * scale; 364 364 + out->z = in[2] * scale; 365 365 + } 366 366 + 367 367 + int 368 368 + blubur_s1_hmd_tick(struct blubur_s1_hmd *hmd) 369 369 + { 370 370 + int result; 371 371 + 372 372 + uint8_t data[64]; 373 373 + result = os_hid_read(hmd->dev, data, sizeof data, 1000); 374 374 + if (result == 0) { 375 375 + // timeout 376 376 + S1_TRACE(hmd, "Read timeout"); 377 377 + return 0; 378 378 + } 379 379 + 380 380 + if (result != sizeof(data)) { 381 381 + S1_ERROR(hmd, "Got unexpected read size %d!", result); 382 382 + return result; 383 383 + } 384 384 + 385 385 + struct read_context ctx = { 386 386 + .data = data, 387 387 + .size = sizeof(data), 388 388 + .read = 0, 389 389 + }; 390 390 + 391 391 + struct blubur_s1_report_header header; 392 392 + READ_STRUCTURE(&ctx, &header); 393 393 + 394 394 + if (header.id != 0x83) { 395 395 + S1_WARN(hmd, "Got unexpected report id 0x%02x!", header.id); 396 396 + return 0; 397 397 + } 398 398 + 399 399 + struct blubur_s1_report_0x83 body; 400 400 + READ_STRUCTURE(&ctx, &body); 401 401 + 402 402 + // enum blubur_s1_status_bits status = body.status; 403 403 + 404 404 + // this is unsigned, so wraparound is fine 405 405 + uint16_t timestamp_delta_ms = body.timestamp - hmd->last_remote_timestamp_ms; 406 406 + 407 407 + if (timestamp_delta_ms == 0) { 408 408 + // duplicate packet? 409 409 + S1_TRACE(hmd, "Got duplicate timestamp packet (0 delta)!"); 410 410 + return 0; 411 411 + } 412 412 + 413 413 + hmd->last_remote_timestamp_ns += (uint64_t)timestamp_delta_ms * U_TIME_1MS_IN_NS; 414 414 + hmd->last_remote_timestamp_ms = body.timestamp; 415 415 + 416 416 + timepoint_ns now = os_monotonic_get_ns(); 417 417 + 418 418 + // if we don't have enough samples, just wait 419 419 + if (hmd->hw2mono_samples < 250) { 420 420 + m_clock_offset_a2b(1000, hmd->last_remote_timestamp_ns, now, &hmd->hw2mono); 421 421 + 422 422 + hmd->hw2mono_samples++; 423 423 + 424 424 + return 0; 425 425 + } 426 426 + 427 427 + timepoint_ns local_sample_time_ns = hmd->last_remote_timestamp_ns + hmd->hw2mono; 428 428 + 429 429 + struct blubur_s1_decoded_sample samples[2]; 430 430 + blubur_s1_decode_sample_pack(body.sensor.data, samples); 431 431 + 432 432 + struct xrt_vec3 accel_m_s2[2]; 433 433 + blubur_s1_convert_accel_sample(samples[0].accel, &accel_m_s2[0]); 434 434 + blubur_s1_convert_accel_sample(samples[1].accel, &accel_m_s2[1]); 435 435 + 436 436 + struct xrt_vec3 gyro_rad_s[2]; 437 437 + blubur_s1_convert_gyro_sample(samples[0].gyro, &gyro_rad_s[0]); 438 438 + blubur_s1_convert_gyro_sample(samples[1].gyro, &gyro_rad_s[1]); 439 439 + 440 440 + m_imu_3dof_update(&hmd->fusion_3dof, local_sample_time_ns, &accel_m_s2[0], &gyro_rad_s[0]); 441 441 + m_imu_3dof_update(&hmd->fusion_3dof, local_sample_time_ns + (U_TIME_1MS_IN_NS / 2LLU), &accel_m_s2[1], 442 442 + &gyro_rad_s[1]); 443 443 + 444 444 + struct xrt_space_relation latest_3dof_relation = { 445 445 + .angular_velocity = hmd->fusion_3dof.last.gyro, 446 446 + .pose.orientation = hmd->fusion_3dof.rot, 447 447 + .relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | 448 448 + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT, 449 449 + }; 450 450 + m_relation_history_push(hmd->relation_history, &latest_3dof_relation, hmd->fusion_3dof.last.timestamp_ns); 451 451 + 452 452 + // S1_DEBUG_HEX(hmd, data, result); 453 453 + 454 454 + return 0; 455 455 + } 456 456 + 457 457 + static void * 458 458 + blubur_s1_hmd_thread(void *ptr) 459 459 + { 460 460 + U_TRACE_SET_THREAD_NAME("Blubur S1 HMD"); 461 461 + 462 462 + struct blubur_s1_hmd *hmd = (struct blubur_s1_hmd *)ptr; 463 463 + 464 464 + #ifdef XRT_OS_LINUX 465 465 + u_linux_try_to_set_realtime_priority_on_thread(hmd->log_level, "Blubur S1 HMD"); 466 466 + #endif // XRT_OS_LINUX 467 467 + 468 468 + int result = 0; 469 469 + 470 470 + os_thread_helper_lock(&hmd->thread); 471 471 + 472 472 + while (os_thread_helper_is_running_locked(&hmd->thread) && result >= 0) { 473 473 + os_thread_helper_unlock(&hmd->thread); 474 474 + 475 475 + result = blubur_s1_hmd_tick(hmd); 476 476 + 477 477 + os_thread_helper_lock(&hmd->thread); 234 478 } 479 479 + 480 480 + os_thread_helper_unlock(&hmd->thread); 481 481 + 482 482 + return NULL; 235 483 } 236 484 237 485 struct blubur_s1_hmd * 238 486 blubur_s1_hmd_create(struct os_hid_device *dev, const char *serial) 239 487 { 488 488 + int ret; 489 489 + 240 490 struct blubur_s1_hmd *hmd = 241 491 U_DEVICE_ALLOCATE(struct blubur_s1_hmd, U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE, 1, 0); 242 492 if (hmd == NULL) { 243 493 return NULL; 244 494 } 495 495 + 496 496 + hmd->log_level = debug_get_log_option_blubur_s1_log(); 497 497 + hmd->dev = dev; 245 498 246 499 hmd->base.destroy = blubur_s1_hmd_destroy; 247 500 hmd->base.name = XRT_DEVICE_GENERIC_HMD; ··· 307 560 hmd->base.get_tracked_pose = blubur_s1_hmd_get_tracked_pose; 308 561 hmd->base.get_presence = blubur_s1_hmd_get_presence; 309 562 hmd->base.get_view_poses = blubur_s1_hmd_get_view_poses; 563 563 + 564 564 + // Initialize IMU fusion for 3DOF tracking 565 565 + m_imu_3dof_init(&hmd->fusion_3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); 566 566 + 567 567 + m_relation_history_create(&hmd->relation_history); 568 568 + if (hmd->relation_history == NULL) { 569 569 + S1_ERROR(hmd, "Failed to create relation history!"); 570 570 + blubur_s1_hmd_destroy(&hmd->base); 571 571 + return NULL; 572 572 + } 573 573 + 574 574 + ret = os_thread_helper_init(&hmd->thread); 575 575 + if (ret < 0) { 576 576 + S1_ERROR(hmd, "Failed to init thread helper!"); 577 577 + blubur_s1_hmd_destroy(&hmd->base); 578 578 + return NULL; 579 579 + } 580 580 + 581 581 + ret = os_thread_helper_start(&hmd->thread, blubur_s1_hmd_thread, hmd); 582 582 + if (ret < 0) { 583 583 + S1_ERROR(hmd, "Failed to start thread!"); 584 584 + blubur_s1_hmd_destroy(&hmd->base); 585 585 + return NULL; 586 586 + } 310 587 311 588 return hmd; 312 589 }

+22

src/xrt/drivers/blubur_s1/blubur_s1_internal.h

··· 9 9 10 10 #pragma once 11 11 12 12 + #include "os/os_hid.h" 13 13 + #include "os/os_threading.h" 14 14 + 12 15 #include "util/u_distortion_mesh.h" 16 16 + #include "util/u_logging.h" 17 17 + 18 18 + #include "math/m_imu_3dof.h" 19 19 + #include "math/m_relation_history.h" 13 20 14 21 #include "blubur_s1_interface.h" 15 22 ··· 18 25 { 19 26 struct xrt_device base; 20 27 28 28 + enum u_logging_level log_level; 29 29 + 21 30 struct u_poly_3k_eye_values poly_3k_values[2]; 31 31 + 32 32 + struct os_hid_device *dev; 33 33 + struct os_thread_helper thread; 34 34 + 35 35 + struct m_imu_3dof fusion_3dof; 36 36 + 37 37 + uint16_t last_remote_timestamp_ms; 38 38 + timepoint_ns last_remote_timestamp_ns; 39 39 + 40 40 + time_duration_ns hw2mono; 41 41 + int hw2mono_samples; 42 42 + 43 43 + struct m_relation_history *relation_history; 22 44 };

+6 -2

src/xrt/drivers/blubur_s1/blubur_s1_prober.c

··· 29 29 30 30 // TODO: figure out how to get the actual serial number of the device, since the official driver has multiple 31 31 // methods, and the string descriptor doesn't work on the unit used for development. 32 32 - char serial[32] = {0}; 33 33 - snprintf(serial, sizeof(serial), "%04x:%04x", dev->vendor_id, dev->product_id); 32 32 + char serial[41] = {0}; 33 33 + result = xrt_prober_get_string_descriptor(xp, dev, XRT_PROBER_STRING_SERIAL_NUMBER, (unsigned char *)serial, 34 34 + sizeof(serial)); 35 35 + if (result <= 0 || strlen(serial) == 0) { 36 36 + snprintf(serial, sizeof(serial), "%04x:%04x", dev->vendor_id, dev->product_id); 37 37 + } 34 38 35 39 struct blubur_s1_hmd *hmd = blubur_s1_hmd_create(hid, serial); 36 40 if (hmd == NULL) {

+47

src/xrt/drivers/blubur_s1/blubur_s1_protocol.h

··· 1 1 + // Copyright 2025, Beyley Cardellio 2 2 + // SPDX-License-Identifier: BSL-1.0 3 3 + /*! 4 4 + * @file 5 5 + * @brief Blubur S1 protocol definitions. 6 6 + * @author Beyley Cardellio <ep1cm1n10n123@gmail.com> 7 7 + * @ingroup drv_blubur_s1 8 8 + */ 9 9 + 10 10 + #pragma once 11 11 + 12 12 + #include "xrt/xrt_defines.h" 13 13 + 14 14 + 15 15 + #pragma pack(push, 1) 16 16 + 17 17 + struct blubur_s1_report_header 18 18 + { 19 19 + uint8_t id; 20 20 + }; 21 21 + 22 22 + struct blubur_s1_report_sensor_data 23 23 + { 24 24 + uint8_t data[0x20]; 25 25 + }; 26 26 + 27 27 + enum blubur_s1_status_bits 28 28 + { 29 29 + BLUBUR_S1_STATUS_DISPLAY_CONNECTION = 0x01, 30 30 + BLUBUR_S1_STATUS_DISPLAY_ON = 0x04, 31 31 + BLUBUR_S1_STATUS_BUTTON = 0x20, 32 32 + BLUBUR_S1_STATUS_PRESENCE = 0x80, 33 33 + }; 34 34 + 35 35 + struct blubur_s1_report_0x83 36 36 + { 37 37 + uint8_t status; //< blubur_s1_status_bits 38 38 + uint16_t timestamp; //< in milliseconds 39 39 + uint16_t unk; 40 40 + uint8_t unk2; 41 41 + int8_t unk3; //< float, ((int32_t)unk3 + 570) / 10.0f 42 42 + int32_t unkValues[4]; 43 43 + struct blubur_s1_report_sensor_data sensor; 44 44 + uint8_t padding[8]; 45 45 + }; 46 46 + 47 47 + #pragma pack(pop)