mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-01 12:46:12 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/wmr: Hook up motion controller gyro and accel

Browse source 
Basic IMU setup with fusion. Reading and applying config callibration data from controller is still work in progress
This commit is contained in:
nima01 2021-12-08 00:43:50 +00:00
parent 98982bd25b
commit d5d1695438
4 changed files with 151 additions and 71 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

74
src/xrt/drivers/wmr/wmr_bt_controller.c
View file

@ -62,18 +62,26 @@ read_packets(struct wmr_bt_controller *d)
} else if (size == 0) {
WMR_TRACE(d, "WMR Controller (Bluetooth): No data to read from device");
return true; // No more messages, return.
} else {
WMR_DEBUG(d, "WMR Controller (Bluetooth): Read %u bytes from device", size);
}
WMR_TRACE(d, "WMR Controller (Bluetooth): Read %u bytes from device", size);
switch (buffer[0]) {
case WMR_BT_MOTION_CONTROLLER_MSG:
os_mutex_lock(&d->lock);
// Note: skipping msg type byte
if (!wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->input, d->log_level)) {
bool b = wmr_controller_packet_parse(&buffer[1], (size_t)size - 1, &d->input, d->log_level);
if (b) {
m_imu_3dof_update(&d->fusion, d->input.imu.timestamp_ticks, &d->input.imu.gyro,
&d->input.imu.acc);
} else {
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i",
buffer[0], size);
os_mutex_unlock(&d->lock);
return false;
}
os_mutex_unlock(&d->lock);
break;
default:
WMR_DEBUG(d, "WMR Controller (Bluetooth): Unknown message type: %02x, size: %i", buffer[0], size);
@ -97,21 +105,45 @@ wmr_bt_controller_get_tracked_pose(struct xrt_device *xdev,
uint64_t at_timestamp_ns,
struct xrt_space_relation *out_relation)
{
// struct wmr_bt_controller *d = wmr_bt_controller(xdev);
// Todo: implement
struct wmr_bt_controller *d = wmr_bt_controller(xdev);
// Variables needed for prediction.
uint64_t last_imu_timestamp_ns = 0;
struct xrt_space_relation relation = {0};
relation.relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
struct xrt_pose pose = {{0, 0, 0, 1}, {0, 1.2, -0.5}};
if (xdev->device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) {
pose.position.x = -0.2;
} else {
pose.position.x = 0.2;
}
relation.pose = pose;
out_relation->pose = pose;
out_relation->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
// Copy data while holding the lock.
os_mutex_lock(&d->lock);
relation.pose.orientation = d->fusion.rot;
relation.angular_velocity = d->last_angular_velocity;
last_imu_timestamp_ns = d->last_imu_timestamp_ns * WMR_MOTION_CONTROLLER_NS_PER_TICK;
os_mutex_unlock(&d->lock);
// No prediction needed.
if (at_timestamp_ns < last_imu_timestamp_ns) {
*out_relation = relation;
return;
}
uint64_t prediction_ns = at_timestamp_ns - last_imu_timestamp_ns;
double prediction_s = time_ns_to_s(prediction_ns);
m_predict_relation(&relation, prediction_s, out_relation);
}
static void
wmr_bt_controller_update_inputs(struct xrt_device *xdev)
{
@ -119,7 +151,7 @@ wmr_bt_controller_update_inputs(struct xrt_device *xdev)
struct xrt_input *inputs = d->base.inputs;
//! @todo Mutex protect the input struct.
os_mutex_lock(&d->lock);
inputs[WMR_INDEX_MENU_CLICK].value.boolean = d->input.menu;
inputs[WMR_INDEX_SQUEEZE_CLICK].value.boolean = d->input.squeeze;
@ -129,6 +161,8 @@ wmr_bt_controller_update_inputs(struct xrt_device *xdev)
inputs[WMR_INDEX_TRACKPAD_CLICK].value.boolean = d->input.trackpad.click;
inputs[WMR_INDEX_TRACKPAD_TOUCH].value.boolean = d->input.trackpad.touch;
inputs[WMR_INDEX_TRACKPAD].value.vec2 = d->input.trackpad.values;
os_mutex_unlock(&d->lock);
}
static void *
@ -136,7 +170,6 @@ wmr_bt_controller_run_thread(void *ptr)
{
struct wmr_bt_controller *d = wmr_bt_controller(ptr);
os_thread_helper_lock(&d->controller_thread);
while (os_thread_helper_is_running_locked(&d->controller_thread)) {
os_thread_helper_unlock(&d->controller_thread);
@ -164,12 +197,13 @@ wmr_bt_controller_destroy(struct xrt_device *xdev)
// Destroy the thread object.
os_thread_helper_destroy(&d->controller_thread);
if (d->controller_hid != NULL) {
os_hid_destroy(d->controller_hid);
d->controller_hid = NULL;
}
os_mutex_destroy(&d->lock);
// Destroy the fusion.
m_imu_3dof_close(&d->fusion);
@ -260,6 +294,8 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
d->base.position_tracking_supported = false;
d->base.hand_tracking_supported = true;
d->input.imu.timestamp_ticks = 0;
m_imu_3dof_init(&d->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
@ -268,6 +304,13 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
// Todo: Read config file from controller if possible.
ret = os_mutex_init(&d->lock);
if (ret != 0) {
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed to init mutex!");
wmr_bt_controller_destroy(&d->base);
return NULL;
}
// Thread and other state.
ret = os_thread_helper_init(&d->controller_thread);
if (ret != 0) {
@ -289,8 +332,11 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
u_var_add_root(d, d->base.str, true);
u_var_add_bool(d, &d->input.menu, "input.menu");
u_var_add_bool(d, &d->input.home, "input.home");
u_var_add_bool(d, &d->input.bt_pairing, "input.bt_pairing");
u_var_add_bool(d, &d->input.squeeze, "input.squeeze");
u_var_add_f32(d, &d->input.trigger, "input.trigger");
u_var_add_u8(d, &d->input.battery, "input.battery");
u_var_add_bool(d, &d->input.thumbstick.click, "input.thumbstick.click");
u_var_add_f32(d, &d->input.thumbstick.values.x, "input.thumbstick.values.y");
u_var_add_f32(d, &d->input.thumbstick.values.y, "input.thumbstick.values.x");
@ -298,6 +344,10 @@ wmr_bt_controller_create(struct os_hid_device *controller_hid,
u_var_add_bool(d, &d->input.trackpad.touch, "input.trackpad.touch");
u_var_add_f32(d, &d->input.trackpad.values.x, "input.trackpad.values.x");
u_var_add_f32(d, &d->input.trackpad.values.y, "input.trackpad.values.y");
u_var_add_ro_vec3_f32(d, &d->input.imu.acc, "imu.acc");
u_var_add_ro_vec3_f32(d, &d->input.imu.gyro, "imu.gyro");
u_var_add_i32(d, &d->input.imu.temperature, "imu.temperature");
return &d->base;
}

23
src/xrt/drivers/wmr/wmr_bt_controller.h
View file

@ -24,7 +24,7 @@ extern "C" {
/*!
* Indices where each input is in the input list.
* Indices in input list of each input.
*/
enum wmr_bt_input_index
{
@ -52,23 +52,20 @@ struct wmr_bt_controller
struct os_hid_device *controller_hid;
struct os_thread_helper controller_thread;
struct os_mutex lock;
//! The last decoded package of IMU and button data
struct wmr_controller_input input;
//! Time of last IMU sample, in CPU time.
uint64_t last_imu_timestamp_ns;
//! Main fusion calculator.
struct m_imu_3dof fusion;
struct
{
struct xrt_vec3 acc;
struct xrt_vec3 gyro;
} last;
struct xrt_quat rot_filtered;
//! The last angular velocity from the IMU, for prediction.
struct xrt_vec3 last_angular_velocity;
enum u_logging_level log_level;
uint32_t last_ticks;
struct wmr_controller_input input;
};

109
src/xrt/drivers/wmr/wmr_controller_protocol.c
View file

@ -19,6 +19,29 @@
*
*/
static inline void
vec3_from_wmr_controller_accel(int32_t sample[3], struct xrt_vec3 *out_vec)
{
// Reverb G1: 1g approximately equivalent to 490,000
// float g = sqrtf(sample[0]*sample[0] + sample[1]*sample[1] + sample[2]*sample[2]);
// U_LOG_IFL_D(log_level, "g: %f", g);
out_vec->x = (float)sample[0] * 0.001f * 1.0f;
out_vec->y = (float)sample[1] * 0.001f * 1.0f;
out_vec->z = (float)sample[2] * 0.001f * 1.0f;
}
static inline void
vec3_from_wmr_controller_gyro(int32_t sample[3], struct xrt_vec3 *out_vec)
{
out_vec->x = (float)sample[0] * 0.001f * 1.0f;
out_vec->y = (float)sample[1] * 0.001f * 1.0f;
out_vec->z = (float)sample[2] * 0.001f * 1.0f;
}
bool
wmr_controller_packet_parse(const unsigned char *buffer,
size_t len,
@ -30,39 +53,24 @@ wmr_controller_packet_parse(const unsigned char *buffer,
return false;
}
/*
U_LOG_IFL_D(log_level,
"%02x %02x %02x %02x %02x %02x %02x %02x | " // buttons and inputs, battery
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // accel
"%02x %02x | " // temp
"%02x %02x %02x %02x %02x %02x %02x %02x %02x | " // gyro
"%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x | " // timestamp and more?
"%02x %02x %02x %02x %02x %02x", // device run state, status and more?
buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5], buffer[6], buffer[7],
buffer[8], buffer[9], buffer[10], buffer[11], buffer[12], buffer[13], buffer[14], buffer[15], buffer[16],
buffer[17], buffer[18], buffer[19], buffer[20], buffer[21], buffer[22], buffer[23],
buffer[24], buffer[25], buffer[26], buffer[27], buffer[28], buffer[29], buffer[30], buffer[31], buffer[32],
buffer[33], buffer[34], buffer[35], buffer[36], buffer[37], buffer[38], buffer[39],
buffer[40], buffer[41], buffer[42], buffer[43]);
*/
const unsigned char *p = buffer;
// Read buttons
unsigned char buttons = read8(&p);
uint8_t buttons = read8(&p);
decoded_input->thumbstick.click = buttons & 0x01;
// decoded_input->home = buttons & 0x02;
decoded_input->home = buttons & 0x02;
decoded_input->menu = buttons & 0x04;
decoded_input->squeeze = buttons & 0x08; // squeeze-click
decoded_input->trackpad.click = buttons & 0x10;
// decoded_input->bt_pairing = buttons & 0x20;
decoded_input->bt_pairing = buttons & 0x20;
decoded_input->trackpad.touch = buttons & 0x40;
// Read thumbstick coordinates (12 bit resolution)
signed int stick_x = read8(&p);
unsigned char nipples = read8(&p);
stick_x += ((nipples & 0x0F) << 8);
signed int stick_y = (nipples >> 4);
int16_t stick_x = read8(&p);
uint8_t nibbles = read8(&p);
stick_x += ((nibbles & 0x0F) << 8);
int16_t stick_y = (nibbles >> 4);
stick_y += (read8(&p) << 4);
decoded_input->thumbstick.values.x = (float)(stick_x - 0x07FF) / 0x07FF;
@ -75,39 +83,50 @@ wmr_controller_packet_parse(const unsigned char *buffer,
decoded_input->thumbstick.values.y = 1.0f;
}
// Read trigger value (0x00 - 0xFF)
decoded_input->trigger = (float)read8(&p) / 0xFF;
U_LOG_IFL_D(log_level, "thumbstick: x %f, y %f, trigger: %f", decoded_input->thumbstick.values.x,
decoded_input->thumbstick.values.y, decoded_input->trigger);
// Read trackpad coordinates (0x00 - 0x64. Both are 0xFF when untouched)
unsigned char trackpad_x = read8(&p);
unsigned char trackpad_y = read8(&p);
uint8_t trackpad_x = read8(&p);
uint8_t trackpad_y = read8(&p);
decoded_input->trackpad.values.x = (trackpad_x == 0xFF) ? 0.0f : (float)(trackpad_x - 0x32) / 0x32;
decoded_input->trackpad.values.y = (trackpad_y == 0xFF) ? 0.0f : (float)(trackpad_y - 0x32) / 0x32;
U_LOG_IFL_D(log_level, "touchpad: x %f, y %f", decoded_input->trackpad.values.x,
decoded_input->trackpad.values.y);
decoded_input->battery = read8(&p);
int32_t acc[3];
acc[0] = read24(&p); // x
acc[1] = read24(&p); // y
acc[2] = read24(&p); // z
vec3_from_wmr_controller_accel(acc, &decoded_input->imu.acc);
decoded_input->imu.temperature = read16(&p);
int32_t gyro[3];
gyro[0] = read24(&p);
gyro[1] = read24(&p);
gyro[2] = read24(&p);
vec3_from_wmr_controller_accel(gyro, &decoded_input->imu.gyro);
uint32_t prev_ticks = decoded_input->imu.timestamp_ticks & 0xFFFFFFFFUL;
// Write the new ticks value into the lower half of timestamp_ticks
decoded_input->imu.timestamp_ticks &= (0xFFFFFFFFUL << 32);
decoded_input->imu.timestamp_ticks += (uint32_t)read32(&p);
if ((decoded_input->imu.timestamp_ticks & 0xFFFFFFFFUL) < prev_ticks) {
// Timer overflow, so increment the upper half of timestamp_ticks
decoded_input->imu.timestamp_ticks += (0x1UL << 32);
}
/* Todo: More decoding here
unsigned char battery = read8(&p);
unsigned int accel_x = read24(&p);
unsigned int accel_y = read24(&p);
unsigned int accel_z = read24(&p);
unsigned int temp = read16(&p);
unsigned int gyro_x = read24(&p);
unsigned int gyro_y = read24(&p);
unsigned int gyro_z = read24(&p);
unsigned int timestamp = read32(&p); // Maybe only part of timestamp.
read16(&p); // Unknown. Seems to depend on controller orientation.
read32(&p); // Unknown.
read16(&p); // Unknown. Seems to depend on controller orientation.
read32(&p); // Unknown.
read16(&p); // Unknown. Device state, etc.
read16(&p);
read16(&p);

16
src/xrt/drivers/wmr/wmr_controller_protocol.h
View file

@ -27,6 +27,7 @@ extern "C" {
// Todo: Is this enough?
#define WMR_MOTION_CONTROLLER_MSG_BUFFER_SIZE 256
#define WMR_MOTION_CONTROLLER_NS_PER_TICK 100
// Messages types specific to Bluetooth connected WMR motion controllers
@ -35,8 +36,12 @@ extern "C" {
struct wmr_controller_input
{
// buttons clicked
bool menu;
bool home;
bool bt_pairing;
bool squeeze; // Actually a "squeeze" click
float trigger;
struct
@ -50,8 +55,17 @@ struct wmr_controller_input
bool touch;
struct xrt_vec2 values;
} trackpad;
};
uint8_t battery;
struct
{
uint64_t timestamp_ticks;
struct xrt_vec3 acc;
struct xrt_vec3 gyro;
int32_t temperature;
} imu;
};
/*!
* @}