mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-13 17:20:09 +00:00
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:
parent
fe4f6c6eb0
commit
2d48a9bca2
src/xrt/drivers/wmr
|
@ -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.
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in a new issue