d/wmr: WMR Controller (BT) 3DoF working, based on gyro and accel.

Tested on HP Reverb G1. Reading and applying device specific
config data is still pending.
This commit is contained in:
Nima01 2021-12-09 01:23:15 +01:00 committed by Jakob Bornecrantz
parent fe4f6c6eb0
commit 2d48a9bca2
2 changed files with 24 additions and 16 deletions

View file

@ -53,7 +53,10 @@ read_packets(struct wmr_bt_controller *d)
{
unsigned char buffer[WMR_MOTION_CONTROLLER_MSG_BUFFER_SIZE];
// Do not block
// Get the timing as close to packet reception as possible.
uint64_t now_ns = os_monotonic_get_ns();
// Read must be non-blocking for reliable timing.
int size = os_hid_read(d->controller_hid, buffer, sizeof(buffer), 0);
if (size < 0) {
@ -66,15 +69,18 @@ read_packets(struct wmr_bt_controller *d)
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
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);
m_imu_3dof_update(&d->fusion, d->input.imu.timestamp_ticks * WMR_MOTION_CONTROLLER_NS_PER_TICK,
&d->input.imu.acc, &d->input.imu.gyro);
d->last_imu_timestamp_ns = now_ns;
d->last_angular_velocity = d->input.imu.gyro;
} else {
WMR_ERROR(d, "WMR Controller (Bluetooth): Failed parsing message type: %02x, size: %i",
buffer[0], size);
@ -127,7 +133,7 @@ wmr_bt_controller_get_tracked_pose(struct xrt_device *xdev,
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;
last_imu_timestamp_ns = d->last_imu_timestamp_ns;
os_mutex_unlock(&d->lock);
// No prediction needed.

View file

@ -22,23 +22,20 @@
static inline void
vec3_from_wmr_controller_accel(int32_t sample[3], struct xrt_vec3 *out_vec)
{
// Reverb G1 observation: 1g is approximately 490,000.
// 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;
out_vec->x = (float)sample[0] / (98000 / 2);
out_vec->y = (float)sample[1] / (98000 / 2);
out_vec->z = (float)sample[2] / (98000 / 2);
}
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;
out_vec->x = (float)sample[0] * 0.00001f;
out_vec->y = (float)sample[1] * 0.00001f;
out_vec->z = (float)sample[2] * 0.00001f;
}
@ -102,6 +99,11 @@ wmr_controller_packet_parse(const unsigned char *buffer,
acc[2] = read24(&p); // z
vec3_from_wmr_controller_accel(acc, &decoded_input->imu.acc);
U_LOG_IFL_T(log_level, "Accel [m/s^2] : %f",
sqrtf(decoded_input->imu.acc.x * decoded_input->imu.acc.x +
decoded_input->imu.acc.y * decoded_input->imu.acc.y +
decoded_input->imu.acc.z * decoded_input->imu.acc.z));
decoded_input->imu.temperature = read16(&p);
@ -110,7 +112,7 @@ wmr_controller_packet_parse(const unsigned char *buffer,
gyro[0] = read24(&p);
gyro[1] = read24(&p);
gyro[2] = read24(&p);
vec3_from_wmr_controller_accel(gyro, &decoded_input->imu.gyro);
vec3_from_wmr_controller_gyro(gyro, &decoded_input->imu.gyro);
uint32_t prev_ticks = decoded_input->imu.timestamp_ticks & 0xFFFFFFFFUL;