The open source OpenXR runtime
d/wmr: Hook up motion controller gyro and accel Basic IMU setup with fusion. Reading and applying config callibration data from controller is still work in progress
+149
-69
+62
-12
src/xrt/drivers/wmr/wmr_bt_controller.c
+62
-12
src/xrt/drivers/wmr/wmr_bt_controller.c
···
62
62
} else if (size == 0) {
63
63
WMR_TRACE(d, "WMR Controller (Bluetooth): No data to read from device");
64
64
return true; // No more messages, return.
65
-
} else {
66
-
WMR_DEBUG(d, "WMR Controller (Bluetooth): Read %u bytes from device", size);
67
65
}
66
+
67
+
WMR_TRACE(d, "WMR Controller (Bluetooth): Read %u bytes from device", size);
68
+
68
69
69
70
switch (buffer[0]) {
70
71
case WMR_BT_MOTION_CONTROLLER_MSG:
72
+
os_mutex_lock(&d->lock);
71
73
// Note: skipping msg type byte
72
-
if (!wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->input, d->log_level)) {
74
+
bool b = wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->input, d->log_level);
75
+
if (b) {
76
+
m_imu_3dof_update(&d->fusion, d->input.imu.timestamp_ticks, &d->input.imu.gyro,
77
+
&d->input.imu.acc);
78
+
} else {
73
79
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i",
74
80
buffer[0], size);
81
+
os_mutex_unlock(&d->lock);
75
82
return false;
76
83
}
84
+
os_mutex_unlock(&d->lock);
77
85
break;
78
86
default:
79
87
WMR_DEBUG(d, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], size);
···
97
105
uint64_t at_timestamp_ns,
98
106
struct xrt_space_relation *out_relation)
99
107
{
100
-
// struct wmr_bt_controller *d = wmr_bt_controller(xdev);
101
-
// Todo: implement
108
+
struct wmr_bt_controller *d = wmr_bt_controller(xdev);
109
+
110
+
// Variables needed for prediction.
111
+
uint64_t last_imu_timestamp_ns = 0;
112
+
struct xrt_space_relation relation = {0};
113
+
relation.relation_flags = (enum xrt_space_relation_flags)(
114
+
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
115
+
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
116
+
117
+
102
118
struct xrt_pose pose = {{0, 0, 0, 1}, {0, 1.2, -0.5}};
103
119
if (xdev->device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
104
120
pose.position.x = -0.2;
105
121
} else {
106
122
pose.position.x = 0.2;
107
123
}
124
+
relation.pose = pose;
108
125
109
-
out_relation->pose = pose;
110
-
out_relation->relation_flags = (enum xrt_space_relation_flags)(
111
-
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
112
-
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
126
+
// Copy data while holding the lock.
127
+
os_mutex_lock(&d->lock);
128
+
relation.pose.orientation = d->fusion.rot;
129
+
relation.angular_velocity = d->last_angular_velocity;
130
+
last_imu_timestamp_ns = d->last_imu_timestamp_ns * WMR_MOTION_CONTROLLER_NS_PER_TICK;
131
+
os_mutex_unlock(&d->lock);
132
+
133
+
// No prediction needed.
134
+
if (at_timestamp_ns < last_imu_timestamp_ns) {
135
+
*out_relation = relation;
136
+
return;
137
+
}
138
+
139
+
uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns;
140
+
double prediction_s = time_ns_to_s(prediction_ns);
141
+
142
+
m_predict_relation(&relation, prediction_s, out_relation);
113
143
}
144
+
145
+
114
146
115
147
static void
116
148
wmr_bt_controller_update_inputs(struct xrt_device *xdev)
···
119
151
120
152
struct xrt_input *inputs = d->base.inputs;
121
153
122
-
//! @todo Mutex protect the input struct.
154
+
os_mutex_lock(&d->lock);
123
155
124
156
inputs[WMR_INDEX_MENU_CLICK].value.boolean = d->input.menu;
125
157
inputs[WMR_INDEX_SQUEEZE_CLICK].value.boolean = d->input.squeeze;
···
129
161
inputs[WMR_INDEX_TRACKPAD_CLICK].value.boolean = d->input.trackpad.click;
130
162
inputs[WMR_INDEX_TRACKPAD_TOUCH].value.boolean = d->input.trackpad.touch;
131
163
inputs[WMR_INDEX_TRACKPAD].value.vec2 = d->input.trackpad.values;
164
+
165
+
os_mutex_unlock(&d->lock);
132
166
}
133
167
134
168
static void *
135
169
wmr_bt_controller_run_thread(void *ptr)
136
170
{
137
171
struct wmr_bt_controller *d = wmr_bt_controller(ptr);
138
-
139
172
140
173
os_thread_helper_lock(&d->controller_thread);
141
174
while (os_thread_helper_is_running_locked(&d->controller_thread)) {
···
163
196
164
197
// Destroy the thread object.
165
198
os_thread_helper_destroy(&d->controller_thread);
166
-
167
199
168
200
if (d->controller_hid != NULL) {
169
201
os_hid_destroy(d->controller_hid);
170
202
d->controller_hid = NULL;
171
203
}
172
204
205
+
os_mutex_destroy(&d->lock);
206
+
173
207
// Destroy the fusion.
174
208
m_imu_3dof_close(&d->fusion);
175
209
···
260
294
d->base.position_tracking_supported = false;
261
295
d->base.hand_tracking_supported = true;
262
296
297
+
298
+
d->input.imu.timestamp_ticks = 0;
263
299
m_imu_3dof_init(&d->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
264
300
265
301
···
268
304
269
305
// Todo: Read config file from controller if possible.
270
306
307
+
ret = os_mutex_init(&d->lock);
308
+
if (ret != 0) {
309
+
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to init mutex!");
310
+
wmr_bt_controller_destroy(&d->base);
311
+
return NULL;
312
+
}
313
+
271
314
// Thread and other state.
272
315
ret = os_thread_helper_init(&d->controller_thread);
273
316
if (ret != 0) {
···
289
332
290
333
u_var_add_root(d, d->base.str, true);
291
334
u_var_add_bool(d, &d->input.menu, "input.menu");
335
+
u_var_add_bool(d, &d->input.home, "input.home");
336
+
u_var_add_bool(d, &d->input.bt_pairing, "input.bt_pairing");
292
337
u_var_add_bool(d, &d->input.squeeze, "input.squeeze");
293
338
u_var_add_f32(d, &d->input.trigger, "input.trigger");
339
+
u_var_add_u8(d, &d->input.battery, "input.battery");
294
340
u_var_add_bool(d, &d->input.thumbstick.click, "input.thumbstick.click");
295
341
u_var_add_f32(d, &d->input.thumbstick.values.x, "input.thumbstick.values.y");
296
342
u_var_add_f32(d, &d->input.thumbstick.values.y, "input.thumbstick.values.x");
···
298
344
u_var_add_bool(d, &d->input.trackpad.touch, "input.trackpad.touch");
299
345
u_var_add_f32(d, &d->input.trackpad.values.x, "input.trackpad.values.x");
300
346
u_var_add_f32(d, &d->input.trackpad.values.y, "input.trackpad.values.y");
347
+
u_var_add_ro_vec3_f32(d, &d->input.imu.acc, "imu.acc");
348
+
u_var_add_ro_vec3_f32(d, &d->input.imu.gyro, "imu.gyro");
349
+
u_var_add_i32(d, &d->input.imu.temperature, "imu.temperature");
350
+
301
351
302
352
return &d->base;
303
353
}
+10
-13
src/xrt/drivers/wmr/wmr_bt_controller.h
+10
-13
src/xrt/drivers/wmr/wmr_bt_controller.h
···
24
24
25
25
26
26
/*!
27
-
* Indices where each input is in the input list.
27
+
* Indices in input list of each input.
28
28
*/
29
29
enum wmr_bt_input_index
30
30
{
···
52
52
53
53
struct os_hid_device *controller_hid;
54
54
struct os_thread_helper controller_thread;
55
+
56
+
55
57
struct os_mutex lock;
56
58
59
+
//! The last decoded package of IMU and button data
60
+
struct wmr_controller_input input;
61
+
//! Time of last IMU sample, in CPU time.
62
+
uint64_t last_imu_timestamp_ns;
63
+
//! Main fusion calculator.
57
64
struct m_imu_3dof fusion;
58
-
59
-
struct
60
-
{
61
-
struct xrt_vec3 acc;
62
-
struct xrt_vec3 gyro;
63
-
} last;
64
-
65
-
struct xrt_quat rot_filtered;
65
+
//! The last angular velocity from the IMU, for prediction.
66
+
struct xrt_vec3 last_angular_velocity;
66
67
67
68
enum u_logging_level log_level;
68
-
69
-
uint32_t last_ticks;
70
-
71
-
struct wmr_controller_input input;
72
69
};
73
70
74
71
+62
-43
src/xrt/drivers/wmr/wmr_controller_protocol.c
+62
-43
src/xrt/drivers/wmr/wmr_controller_protocol.c
···
19
19
*
20
20
*/
21
21
22
+
static inline void
23
+
vec3_from_wmr_controller_accel(int32_t sample[3], struct xrt_vec3 *out_vec)
24
+
{
25
+
26
+
// Reverb G1: 1g approximately equivalent to 490,000
27
+
// float g = sqrtf(sample[0]*sample[0] + sample[1]*sample[1] + sample[2]*sample[2]);
28
+
// U_LOG_IFL_D(log_level, "g: %f", g);
29
+
30
+
out_vec->x = (float)sample[0] * 0.001f * 1.0f;
31
+
out_vec->y = (float)sample[1] * 0.001f * 1.0f;
32
+
out_vec->z = (float)sample[2] * 0.001f * 1.0f;
33
+
}
34
+
35
+
36
+
static inline void
37
+
vec3_from_wmr_controller_gyro(int32_t sample[3], struct xrt_vec3 *out_vec)
38
+
{
39
+
out_vec->x = (float)sample[0] * 0.001f * 1.0f;
40
+
out_vec->y = (float)sample[1] * 0.001f * 1.0f;
41
+
out_vec->z = (float)sample[2] * 0.001f * 1.0f;
42
+
}
43
+
44
+
22
45
bool
23
46
wmr_controller_packet_parse(const unsigned char *buffer,
24
47
size_t len,
···
30
53
return false;
31
54
}
32
55
33
-
/*
34
-
U_LOG_IFL_D(log_level,
35
-
"%02x %02x %02x %02x %02x %02x %02x %02x | " // buttons and inputs, battery
36
-
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // accel
37
-
"%02x %02x | " // temp
38
-
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // gyro
39
-
"%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | " // timestamp and more?
40
-
"%02x %02x %02x %02x %02x %02x", // device run state, status and more?
41
-
buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7],
42
-
buffer[8], buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15], buffer[16],
43
-
buffer[17], buffer[18], buffer[19], buffer[20], buffer[21], buffer[22], buffer[23],
44
-
buffer[24], buffer[25], buffer[26], buffer[27], buffer[28], buffer[29], buffer[30], buffer[31], buffer[32],
45
-
buffer[33], buffer[34], buffer[35], buffer[36], buffer[37], buffer[38], buffer[39],
46
-
buffer[40], buffer[41], buffer[42], buffer[43]);
47
-
*/
48
56
const unsigned char *p = buffer;
49
57
50
58
// Read buttons
51
-
unsigned char buttons = read8(&p);
59
+
uint8_t buttons = read8(&p);
52
60
decoded_input->thumbstick.click = buttons & 0x01;
53
-
// decoded_input->home = buttons & 0x02;
61
+
decoded_input->home = buttons & 0x02;
54
62
decoded_input->menu = buttons & 0x04;
55
63
decoded_input->squeeze = buttons & 0x08; // squeeze-click
56
64
decoded_input->trackpad.click = buttons & 0x10;
57
-
// decoded_input->bt_pairing = buttons & 0x20;
65
+
decoded_input->bt_pairing = buttons & 0x20;
58
66
decoded_input->trackpad.touch = buttons & 0x40;
59
67
60
68
61
69
// Read thumbstick coordinates (12 bit resolution)
62
-
signed int stick_x = read8(&p);
63
-
unsigned char nipples = read8(&p);
64
-
stick_x += ((nipples & 0x0F) << 8);
65
-
signed int stick_y = (nipples >> 4);
70
+
int16_t stick_x = read8(&p);
71
+
uint8_t nibbles = read8(&p);
72
+
stick_x += ((nibbles & 0x0F) << 8);
73
+
int16_t stick_y = (nibbles >> 4);
66
74
stick_y += (read8(&p) << 4);
67
75
68
76
decoded_input->thumbstick.values.x = (float)(stick_x - 0x07FF) / 0x07FF;
···
74
82
if (decoded_input->thumbstick.values.y > 1.0f) {
75
83
decoded_input->thumbstick.values.y = 1.0f;
76
84
}
77
-
78
85
79
86
// Read trigger value (0x00 - 0xFF)
80
87
decoded_input->trigger = (float)read8(&p) / 0xFF;
81
88
82
-
U_LOG_IFL_D(log_level, "thumbstick: x %f, y %f, trigger: %f", decoded_input->thumbstick.values.x,
83
-
decoded_input->thumbstick.values.y, decoded_input->trigger);
84
-
85
-
86
89
// Read trackpad coordinates (0x00 - 0x64. Both are 0xFF when untouched)
87
-
unsigned char trackpad_x = read8(&p);
88
-
unsigned char trackpad_y = read8(&p);
90
+
uint8_t trackpad_x = read8(&p);
91
+
uint8_t trackpad_y = read8(&p);
89
92
decoded_input->trackpad.values.x = (trackpad_x == 0xFF) ? 0.0f : (float)(trackpad_x - 0x32) / 0x32;
90
93
decoded_input->trackpad.values.y = (trackpad_y == 0xFF) ? 0.0f : (float)(trackpad_y - 0x32) / 0x32;
91
94
92
-
U_LOG_IFL_D(log_level, "touchpad: x %f, y %f", decoded_input->trackpad.values.x,
93
-
decoded_input->trackpad.values.y);
94
95
96
+
decoded_input->battery = read8(&p);
95
97
96
-
/* Todo: More decoding here
97
98
98
-
unsigned char battery = read8(&p);
99
-
unsigned int accel_x = read24(&p);
100
-
unsigned int accel_y = read24(&p);
101
-
unsigned int accel_z = read24(&p);
102
-
unsigned int temp = read16(&p);
103
-
unsigned int gyro_x = read24(&p);
104
-
unsigned int gyro_y = read24(&p);
105
-
unsigned int gyro_z = read24(&p);
99
+
int32_t acc[3];
100
+
acc[0] = read24(&p); // x
101
+
acc[1] = read24(&p); // y
102
+
acc[2] = read24(&p); // z
103
+
vec3_from_wmr_controller_accel(acc, &decoded_input->imu.acc);
106
104
107
-
unsigned int timestamp = read32(&p); // Maybe only part of timestamp.
108
-
read16(&p); // Unknown. Seems to depend on controller orientation.
109
-
read32(&p); // Unknown.
105
+
106
+
decoded_input->imu.temperature = read16(&p);
107
+
108
+
109
+
int32_t gyro[3];
110
+
gyro[0] = read24(&p);
111
+
gyro[1] = read24(&p);
112
+
gyro[2] = read24(&p);
113
+
vec3_from_wmr_controller_accel(gyro, &decoded_input->imu.gyro);
110
114
115
+
116
+
uint32_t prev_ticks = decoded_input->imu.timestamp_ticks & 0xFFFFFFFFUL;
117
+
118
+
// Write the new ticks value into the lower half of timestamp_ticks
119
+
decoded_input->imu.timestamp_ticks &= (0xFFFFFFFFUL << 32);
120
+
decoded_input->imu.timestamp_ticks += (uint32_t)read32(&p);
121
+
122
+
if ((decoded_input->imu.timestamp_ticks & 0xFFFFFFFFUL) < prev_ticks) {
123
+
// Timer overflow, so increment the upper half of timestamp_ticks
124
+
decoded_input->imu.timestamp_ticks += (0x1UL << 32);
125
+
}
126
+
127
+
/* Todo: More decoding here
128
+
read16(&p); // Unknown. Seems to depend on controller orientation.
129
+
read32(&p); // Unknown.
111
130
read16(&p); // Unknown. Device state, etc.
112
131
read16(&p);
113
132
read16(&p);
+15
-1
src/xrt/drivers/wmr/wmr_controller_protocol.h
+15
-1
src/xrt/drivers/wmr/wmr_controller_protocol.h
···
27
27
28
28
// Todo: Is this enough?
29
29
#define WMR_MOTION_CONTROLLER_MSG_BUFFER_SIZE 256
30
+
#define WMR_MOTION_CONTROLLER_NS_PER_TICK 100
30
31
31
32
32
33
// Messages types specific to Bluetooth connected WMR motion controllers
···
35
36
36
37
struct wmr_controller_input
37
38
{
39
+
// buttons clicked
38
40
bool menu;
41
+
bool home;
42
+
bool bt_pairing;
39
43
bool squeeze; // Actually a "squeeze" click
44
+
40
45
float trigger;
41
46
42
47
struct
···
50
55
bool touch;
51
56
struct xrt_vec2 values;
52
57
} trackpad;
53
-
};
58
+
59
+
uint8_t battery;
54
60
61
+
struct
62
+
{
63
+
uint64_t timestamp_ticks;
64
+
struct xrt_vec3 acc;
65
+
struct xrt_vec3 gyro;
66
+
int32_t temperature;
67
+
} imu;
68
+
};
55
69
56
70
/*!
57
71
* @}