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: Forward IMU samples to wmr_source

Browse source
This commit is contained in:
Mateo de Mayo 2021-12-06 08:56:56 -03:00 committed by Jakob Bornecrantz
parent 41f42e0b3a
commit d3bde043bd
3 changed files with 59 additions and 11 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

34
src/xrt/drivers/wmr/wmr_hmd.c
View file

@ -272,32 +272,44 @@ hololens_sensors_read_packets(struct wmr_hmd *wh)
struct xrt_vec3 raw_gyro[4];
struct xrt_vec3 raw_accel[4];
struct xrt_vec3 calib_gyro[4];
struct xrt_vec3 calib_accel[4];
for (int i = 0; i < 4; i++) {
struct xrt_vec3 raw_sample, sample;
vec3_from_hololens_gyro(wh->packet.gyro, i, &raw_sample);
math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, &raw_sample, &sample);
math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, &sample);
math_quat_rotate_vec3(&wh->gyro_to_centerline.orientation, &sample, &raw_gyro[i]);
struct xrt_vec3 *rg = &raw_gyro[i];
struct xrt_vec3 *cg = &calib_gyro[i];
vec3_from_hololens_gyro(wh->packet.gyro, i, rg);
math_matrix_3x3_transform_vec3(&wh->config.sensors.gyro.mix_matrix, rg, cg);
math_vec3_accum(&wh->config.sensors.gyro.bias_offsets, cg);
math_quat_rotate_vec3(&wh->gyro_to_centerline.orientation, cg, cg);
vec3_from_hololens_accel(wh->packet.accel, i, &raw_sample);
math_matrix_3x3_transform_vec3(&wh->config.sensors.accel.mix_matrix, &raw_sample, &sample);
math_vec3_accum(&wh->config.sensors.accel.bias_offsets, &sample);
math_quat_rotate_vec3(&wh->accel_to_centerline.orientation, &sample, &raw_accel[i]);
struct xrt_vec3 *ra = &raw_accel[i];
struct xrt_vec3 *ca = &calib_accel[i];
vec3_from_hololens_accel(wh->packet.accel, i, ra);
math_matrix_3x3_transform_vec3(&wh->config.sensors.accel.mix_matrix, ra, ca);
math_vec3_accum(&wh->config.sensors.accel.bias_offsets, ca);
math_quat_rotate_vec3(&wh->accel_to_centerline.orientation, ca, ca);
}
// Fusion tracking
os_mutex_lock(&wh->fusion.mutex);
for (int i = 0; i < 4; i++) {
m_imu_3dof_update( //
&wh->fusion.i3dof, //
wh->packet.gyro_timestamp[i] * WMR_MS_HOLOLENS_NS_PER_TICK, //
&raw_accel[i], //
&raw_gyro[i]); //
&calib_accel[i], //
&calib_gyro[i]); //
}
wh->fusion.last_imu_timestamp_ns = now_ns;
wh->fusion.last_angular_velocity = raw_gyro[3];
os_mutex_unlock(&wh->fusion.mutex);
// SLAM tracking
//! @todo For now we are using raw_samples here because the centerline fix
//! in the calibrated samples breaks SLAM systems. Handling of coordinate
//! systems needs to be revisited.
wmr_source_push_imu_packet(wh->slam.source, wh->packet.gyro_timestamp, raw_accel, raw_gyro);
break;
}
case WMR_MS_HOLOLENS_MSG_BT_IFACE: //

31
src/xrt/drivers/wmr/wmr_source.c
View file

@ -304,3 +304,34 @@ wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev
return xfs;
}
void
wmr_source_push_imu_packet(struct xrt_fs *xfs, uint64_t ts[4], struct xrt_vec3 accels[4], struct xrt_vec3 gyros[4])
{
struct wmr_source *ws = wmr_source_from_xfs(xfs);
if (ws->average_imus) {
struct xrt_vec3 a = {0, 0, 0};
struct xrt_vec3 g = {0, 0, 0};
for (int i = 0; i < 4; i++) {
math_vec3_accum(&accels[i], &a);
math_vec3_accum(&gyros[i], &g);
}
math_vec3_scalar_mul(1.0f / 4, &a);
math_vec3_scalar_mul(1.0f / 4, &g);
timepoint_ns t = (ts[3] + ts[0]) / 2 * WMR_MS_HOLOLENS_NS_PER_TICK;
struct xrt_vec3_f64 accel = {a.x, a.y, a.z};
struct xrt_vec3_f64 gyro = {g.x, g.y, g.z};
struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = accel, .gyro_rad_secs = gyro};
xrt_sink_push_imu(&ws->imu_sink, &sample);
} else {
for (int i = 0; i < 4; i++) {
timepoint_ns t = ts[i] * WMR_MS_HOLOLENS_NS_PER_TICK;
struct xrt_vec3_f64 accel = {accels[i].x, accels[i].y, accels[i].z};
struct xrt_vec3_f64 gyro = {gyros[i].x, gyros[i].y, gyros[i].z};
struct xrt_imu_sample sample = {.timestamp_ns = t, .accel_m_s2 = accel, .gyro_rad_secs = gyro};
xrt_sink_push_imu(&ws->imu_sink, &sample);
}
}
}

5
src/xrt/drivers/wmr/wmr_source.h
View file

@ -29,6 +29,11 @@ extern "C" {
struct xrt_fs *
wmr_source_create(struct xrt_frame_context *xfctx, struct xrt_prober_device *dev_holo, struct wmr_hmd_config cfg);
//! @todo IMU data should be generated from within the data source, but right
//! now we need this function because it is being generated from wmr_hmd
void
wmr_source_push_imu_packet(struct xrt_fs *xfs, uint64_t ts[4], struct xrt_vec3 accels[4], struct xrt_vec3 gyros[4]);
/*!
* @}
*/