The open source OpenXR runtime
d/wmr: Do prediction when returning poses
+35
-7
+29
-7
src/xrt/drivers/wmr/wmr_hmd.c
+29
-7
src/xrt/drivers/wmr/wmr_hmd.c
···
20
20
#include "math/m_mathinclude.h"
21
21
#include "math/m_api.h"
22
22
#include "math/m_vec2.h"
23
+
#include "math/m_predict.h"
23
24
24
25
#include "util/u_var.h"
25
26
#include "util/u_misc.h"
···
140
141
}
141
142
142
143
switch (buffer[0]) {
143
-
case WMR_MS_HOLOLENS_MSG_SENSORS:
144
+
case WMR_MS_HOLOLENS_MSG_SENSORS: {
145
+
// Get the timing as close to reading the packet as possible.
146
+
uint64_t now_ns = os_monotonic_get_ns();
147
+
144
148
hololens_sensors_decode_packet(wh, &wh->packet, buffer, size);
145
149
146
150
for (int i = 0; i < 4; i++) {
···
148
152
vec3_from_hololens_accel(wh->packet.accel, i, &wh->raw_accel);
149
153
150
154
os_mutex_lock(&wh->fusion.mutex);
155
+
wh->fusion.last_imu_timestamp_ns = now_ns;
156
+
wh->fusion.last_angular_velocity = wh->raw_gyro;
151
157
m_imu_3dof_update( //
152
158
&wh->fusion.i3dof, //
153
159
wh->packet.gyro_timestamp[i] * WMR_MS_HOLOLENS_NS_PER_TICK, //
···
156
162
os_mutex_unlock(&wh->fusion.mutex);
157
163
}
158
164
break;
165
+
}
159
166
case WMR_MS_HOLOLENS_MSG_UNKNOWN_05:
160
167
case WMR_MS_HOLOLENS_MSG_UNKNOWN_06:
161
168
case WMR_MS_HOLOLENS_MSG_UNKNOWN_0E: //
···
370
377
return;
371
378
}
372
379
373
-
// Clear relation.
374
-
U_ZERO(out_relation);
380
+
// Variables needed for prediction.
381
+
uint64_t last_imu_timestamp_ns = 0;
382
+
struct xrt_space_relation relation = {0};
383
+
relation.relation_flags = (enum xrt_space_relation_flags)( //
384
+
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | //
385
+
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | //
386
+
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
375
387
388
+
// Get data while holding the lock.
376
389
os_mutex_lock(&wh->fusion.mutex);
377
-
out_relation->pose.orientation = wh->fusion.i3dof.rot;
390
+
relation.pose.orientation = wh->fusion.i3dof.rot;
391
+
relation.angular_velocity = wh->fusion.last_angular_velocity;
392
+
last_imu_timestamp_ns = wh->fusion.last_imu_timestamp_ns;
378
393
os_mutex_unlock(&wh->fusion.mutex);
379
394
380
-
out_relation->relation_flags = (enum xrt_space_relation_flags)( //
381
-
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | //
382
-
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
395
+
// No prediction needed.
396
+
if (at_timestamp_ns < last_imu_timestamp_ns) {
397
+
*out_relation = relation;
398
+
return;
399
+
}
400
+
401
+
uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns;
402
+
double prediction_s = time_ns_to_s(prediction_ns);
403
+
404
+
m_predict_relation(&relation, prediction_s, out_relation);
383
405
}
384
406
385
407
static void
+6
src/xrt/drivers/wmr/wmr_hmd.h
+6
src/xrt/drivers/wmr/wmr_hmd.h
···
79
79
80
80
//! Main fusion calculator.
81
81
struct m_imu_3dof i3dof;
82
+
83
+
//! The last angular velocity from the IMU, for prediction.
84
+
struct xrt_vec3 last_angular_velocity;
85
+
86
+
//! When did we get the last IMU sample, in CPU time.
87
+
uint64_t last_imu_timestamp_ns;
82
88
} fusion;
83
89
84
90
struct