diff --git a/src/xrt/drivers/wmr/wmr_bt_controller.c b/src/xrt/drivers/wmr/wmr_bt_controller.c index 6aa60cf28..ae1a632c7 100644 --- a/src/xrt/drivers/wmr/wmr_bt_controller.c +++ b/src/xrt/drivers/wmr/wmr_bt_controller.c @@ -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; } diff --git a/src/xrt/drivers/wmr/wmr_bt_controller.h b/src/xrt/drivers/wmr/wmr_bt_controller.h index c78e54599..c0b941fc1 100644 --- a/src/xrt/drivers/wmr/wmr_bt_controller.h +++ b/src/xrt/drivers/wmr/wmr_bt_controller.h @@ -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; }; diff --git a/src/xrt/drivers/wmr/wmr_controller_protocol.c b/src/xrt/drivers/wmr/wmr_controller_protocol.c index b751410c9..87d226e51 100644 --- a/src/xrt/drivers/wmr/wmr_controller_protocol.c +++ b/src/xrt/drivers/wmr/wmr_controller_protocol.c @@ -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); diff --git a/src/xrt/drivers/wmr/wmr_controller_protocol.h b/src/xrt/drivers/wmr/wmr_controller_protocol.h index b284b0814..773368f9a 100644 --- a/src/xrt/drivers/wmr/wmr_controller_protocol.h +++ b/src/xrt/drivers/wmr/wmr_controller_protocol.h @@ -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; +}; /*! * @}