The open source OpenXR runtime
d/wmr: Decode and handle controller buttons, thumbstick and trackpad.
+93
-98
+1
-1
src/xrt/drivers/wmr/wmr_bt_controller.c
+1
-1
src/xrt/drivers/wmr/wmr_bt_controller.c
···
69
69
switch (buffer[0]) {
70
70
case WMR_BT_MOTION_CONTROLLER_MSG:
71
71
// Note: skipping msg type byte
72
-
if (!wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->controller_message, d->log_level)) {
72
+
if (!wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->input, d->log_level)) {
73
73
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i",
74
74
buffer[0], size);
75
75
return false;
+1
-20
src/xrt/drivers/wmr/wmr_bt_controller.h
+1
-20
src/xrt/drivers/wmr/wmr_bt_controller.h
···
54
54
struct os_thread_helper controller_thread;
55
55
struct os_mutex lock;
56
56
57
-
struct wmr_controller_message controller_message;
58
-
59
57
struct m_imu_3dof fusion;
60
58
61
59
struct
···
70
68
71
69
uint32_t last_ticks;
72
70
73
-
struct
74
-
{
75
-
bool menu;
76
-
bool squeeze;
77
-
float trigger;
78
-
79
-
struct
80
-
{
81
-
bool click;
82
-
struct xrt_vec2 values;
83
-
} thumbstick;
84
-
struct
85
-
{
86
-
bool click;
87
-
bool touch;
88
-
struct xrt_vec2 values;
89
-
} trackpad;
90
-
} input;
71
+
struct wmr_controller_input input;
91
72
};
92
73
93
74
+75
-40
src/xrt/drivers/wmr/wmr_controller_protocol.c
+75
-40
src/xrt/drivers/wmr/wmr_controller_protocol.c
···
22
22
bool
23
23
wmr_controller_packet_parse(const unsigned char *buffer,
24
24
size_t len,
25
-
struct wmr_controller_message *out_message,
25
+
struct wmr_controller_input *decoded_input,
26
26
enum u_logging_level log_level)
27
27
{
28
28
if (len != 44) {
···
30
30
return false;
31
31
}
32
32
33
-
U_LOG_IFL_D(log_level,
34
-
"%02x %02x %02x %02x %02x %02x %02x %02x | " // buttons and inputs, battery
35
-
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // accel
36
-
"%02x %02x | " // temp
37
-
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // gyro
38
-
"%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | " // timestamp and more?
39
-
"%02x %02x %02x %02x %02x %02x", // device run state, status and more?
40
-
buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7], buffer[8],
41
-
buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15], buffer[16],
42
-
buffer[17], buffer[18], buffer[19], buffer[20], buffer[21], buffer[22], buffer[23], buffer[24],
43
-
buffer[25], buffer[26], buffer[27], buffer[28], buffer[29], buffer[30], buffer[31], buffer[32],
44
-
buffer[33], buffer[34], buffer[35], buffer[36], buffer[37], buffer[38], buffer[39], buffer[40],
45
-
buffer[41], buffer[42], buffer[43]);
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
+
const unsigned char *p = buffer;
49
+
50
+
// Read buttons
51
+
unsigned char buttons = read8(&p);
52
+
decoded_input->thumbstick.click = buttons & 0x01;
53
+
// decoded_input->home = buttons & 0x02;
54
+
decoded_input->menu = buttons & 0x04;
55
+
decoded_input->squeeze = buttons & 0x08; // squeeze-click
56
+
decoded_input->trackpad.click = buttons & 0x10;
57
+
// decoded_input->bt_pairing = buttons & 0x20;
58
+
decoded_input->trackpad.touch = buttons & 0x40;
59
+
60
+
61
+
// 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);
66
+
stick_y += (read8(&p) << 4);
67
+
68
+
decoded_input->thumbstick.values.x = (float)(stick_x - 0x07FF) / 0x07FF;
69
+
if (decoded_input->thumbstick.values.x > 1.0f) {
70
+
decoded_input->thumbstick.values.x = 1.0f;
71
+
}
72
+
73
+
decoded_input->thumbstick.values.y = (float)(stick_y - 0x07FF) / 0x07FF;
74
+
if (decoded_input->thumbstick.values.y > 1.0f) {
75
+
decoded_input->thumbstick.values.y = 1.0f;
76
+
}
77
+
78
+
79
+
// Read trigger value (0x00 - 0xFF)
80
+
decoded_input->trigger = (float)read8(&p) / 0xFF;
81
+
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);
46
84
47
-
const unsigned char *p = buffer;
48
85
49
-
out_message->buttons = read8(&p);
86
+
// Read trackpad coordinates (0x00 - 0x64. Both are 0xFF when untouched)
87
+
unsigned char trackpad_x = read8(&p);
88
+
unsigned char trackpad_y = read8(&p);
89
+
decoded_input->trackpad.values.x = (trackpad_x == 0xFF) ? 0.0f : (float)(trackpad_x - 0x32) / 0x32;
90
+
decoded_input->trackpad.values.y = (trackpad_y == 0xFF) ? 0.0f : (float)(trackpad_y - 0x32) / 0x32;
50
91
51
-
// Todo: interpret analog stick data
52
-
out_message->stick_1 = read8(&p);
53
-
out_message->stick_2 = read8(&p);
54
-
out_message->stick_3 = read8(&p);
92
+
U_LOG_IFL_D(log_level, "touchpad: x %f, y %f", decoded_input->trackpad.values.x,
93
+
decoded_input->trackpad.values.y);
55
94
56
-
out_message->trigger = read8(&p); // pressure: 0x00 - 0xFF
57
95
58
-
// Touchpad coords range: 0x00 - 0x64. Both are 0xFF when untouched.
59
-
out_message->pad_x = read8(&p);
60
-
out_message->pad_y = read8(&p);
61
-
out_message->battery = read8(&p);
62
-
out_message->accel_x = read24(&p);
63
-
out_message->accel_y = read24(&p);
64
-
out_message->accel_z = read24(&p);
65
-
out_message->temp = read16(&p);
66
-
out_message->gyro_x = read24(&p);
67
-
out_message->gyro_y = read24(&p);
68
-
out_message->gyro_z = read24(&p);
96
+
/* Todo: More decoding here
69
97
70
-
out_message->timestamp = read32(&p); // Maybe only part of timestamp.
71
-
read16(&p); // Unknown. Seems to depend on controller orientation.
72
-
read32(&p); // Unknown.
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);
73
106
74
-
read16(&p); // Unknown. Device state, etc.
75
-
read16(&p);
76
-
read16(&p);
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.
77
110
78
-
U_LOG_IFL_D(log_level, "buttons: %02x, trigger: %02x, pad_x: %02x, pad_y: %02x", out_message->buttons,
79
-
out_message->trigger, out_message->pad_x, out_message->pad_y);
111
+
read16(&p); // Unknown. Device state, etc.
112
+
read16(&p);
113
+
read16(&p);
114
+
*/
80
115
81
116
return true;
82
117
}
+16
-37
src/xrt/drivers/wmr/wmr_controller_protocol.h
+16
-37
src/xrt/drivers/wmr/wmr_controller_protocol.h
···
33
33
#define WMR_BT_MOTION_CONTROLLER_MSG 0x01
34
34
35
35
36
-
struct wmr_controller_message
36
+
struct wmr_controller_input
37
37
{
38
-
// Very much still work in progress!
38
+
bool menu;
39
+
bool squeeze; // Actually a "squeeze" click
40
+
float trigger;
39
41
40
-
// HP Reverb G1 button map:
41
-
// Stick_pressed: 0x01
42
-
// Home button pressed: 0x02
43
-
// Menu button pressed: 0x04
44
-
// Grip button pressed: 0x08
45
-
// Touch-pad pressed: 0x10
46
-
// BT pairing button pressed: 0x20
47
-
// Touch-pad touched: 0x40
48
-
uint8_t buttons;
49
-
50
-
// Todo: interpret analog stick data
51
-
uint8_t stick_1;
52
-
uint8_t stick_2;
53
-
uint8_t stick_3;
54
-
55
-
uint8_t trigger; // pressure: 0x00 - 0xFF
56
-
57
-
// Touchpad coords range: 0x00 - 0x64. Both are 0xFF when untouched.
58
-
uint8_t pad_x;
59
-
uint8_t pad_y;
60
-
61
-
uint8_t battery;
62
-
63
-
int32_t accel_x;
64
-
int32_t accel_y;
65
-
int32_t accel_z;
66
-
67
-
int32_t temp;
68
-
69
-
int32_t gyro_x;
70
-
int32_t gyro_y;
71
-
int32_t gyro_z;
72
-
73
-
uint64_t timestamp;
42
+
struct
43
+
{
44
+
bool click;
45
+
struct xrt_vec2 values;
46
+
} thumbstick;
47
+
struct
48
+
{
49
+
bool click;
50
+
bool touch;
51
+
struct xrt_vec2 values;
52
+
} trackpad;
74
53
};
75
54
76
55
···
89
68
bool
90
69
wmr_controller_packet_parse(const unsigned char *buffer,
91
70
size_t len,
92
-
struct wmr_controller_message *out_message,
71
+
struct wmr_controller_input *decoded_input,
93
72
enum u_logging_level log_level);
94
73
95
74