The open source OpenXR runtime
d/vive: Refactor how acceleration and gyro is scaled and biased
Results are the same.
authored by
Christoph Haag
and committed by
Jakob Bornecrantz
ea65f001
63866610
+51
-8
+51
-8
src/xrt/drivers/vive/vive_device.c
+51
-8
src/xrt/drivers/vive/vive_device.c
···
365
365
(int16_t)__le16_to_cpu(sample->acc[2]),
366
366
};
367
367
368
+
double acc_scale[3] = {
369
+
d->config.imu.acc_scale.x,
370
+
d->config.imu.acc_scale.y,
371
+
d->config.imu.acc_scale.z,
372
+
};
373
+
374
+
double acc_bias[3] = {
375
+
d->config.imu.acc_bias.x,
376
+
d->config.imu.acc_bias.y,
377
+
d->config.imu.acc_bias.z,
378
+
};
379
+
368
380
scale = (double)d->config.imu.acc_range / 32768.0f;
369
381
struct xrt_vec3 acceleration = {
370
-
scale * d->config.imu.acc_scale.x * acc[0] - d->config.imu.acc_bias.x,
371
-
scale * d->config.imu.acc_scale.y * acc[1] - d->config.imu.acc_bias.y,
372
-
scale * d->config.imu.acc_scale.z * acc[2] - d->config.imu.acc_bias.z,
382
+
scale * acc_scale[0] * acc[0] - acc_bias[0],
383
+
scale * acc_scale[1] * acc[1] - acc_bias[1],
384
+
scale * acc_scale[2] * acc[2] - acc_bias[2],
373
385
};
374
386
387
+
VIVE_TRACE(d, "ACC %f %f %f (%f - %f, %f - %f, %f - %f)", //
388
+
acceleration.x, //
389
+
acceleration.y, //
390
+
acceleration.z, //
391
+
scale * acc_scale[0] * acc[0], //
392
+
acc_bias[0], //
393
+
scale * acc_scale[1] * acc[1], //
394
+
acc_bias[1], //
395
+
scale * acc_scale[2] * acc[2], //
396
+
acc_bias[2]); //
397
+
375
398
int16_t gyro[3] = {
376
399
(int16_t)__le16_to_cpu(sample->gyro[0]),
377
400
(int16_t)__le16_to_cpu(sample->gyro[1]),
378
401
(int16_t)__le16_to_cpu(sample->gyro[2]),
379
402
};
380
403
404
+
double gyro_scale[3] = {
405
+
d->config.imu.gyro_scale.x,
406
+
d->config.imu.gyro_scale.y,
407
+
d->config.imu.gyro_scale.z,
408
+
};
409
+
410
+
double gyro_bias[3] = {
411
+
d->config.imu.gyro_bias.x,
412
+
d->config.imu.gyro_bias.y,
413
+
d->config.imu.gyro_bias.z,
414
+
};
415
+
381
416
scale = (double)d->config.imu.gyro_range / 32768.0f;
382
417
struct xrt_vec3 angular_velocity = {
383
-
scale * d->config.imu.gyro_scale.x * gyro[0] - d->config.imu.gyro_bias.x,
384
-
scale * d->config.imu.gyro_scale.y * gyro[1] - d->config.imu.gyro_bias.y,
385
-
scale * d->config.imu.gyro_scale.z * gyro[2] - d->config.imu.gyro_bias.z,
418
+
scale * gyro_scale[0] * gyro[0] - gyro_bias[0],
419
+
scale * gyro_scale[1] * gyro[1] - gyro_bias[1],
420
+
scale * gyro_scale[2] * gyro[2] - gyro_bias[2],
386
421
};
387
422
388
-
VIVE_TRACE(d, "ACC %f %f %f", acceleration.x, acceleration.y, acceleration.z);
423
+
VIVE_TRACE(d, "GYRO %f %f %f (%f - %f, %f - %f, %f - %f)", //
424
+
angular_velocity.x, //
425
+
angular_velocity.y, //
426
+
angular_velocity.z, //
427
+
scale * gyro_scale[0] * gyro[0], //
428
+
gyro_bias[0], //
429
+
scale * gyro_scale[1] * gyro[1], //
430
+
gyro_bias[1], //
431
+
scale * gyro_scale[2] * gyro[2], //
432
+
gyro_bias[2]); //
389
433
390
-
VIVE_TRACE(d, "GYRO %f %f %f", angular_velocity.x, angular_velocity.y, angular_velocity.z);
391
434
392
435
switch (d->config.variant) {
393
436
case VIVE_VARIANT_VIVE: