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

m/3dof: Show more in the variable tracker

Browse source
This commit is contained in:
Jakob Bornecrantz 2021-12-05 13:38:18 +00:00
parent 6cc92c369a
commit a6b4e6a830
2 changed files with 26 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

22
src/xrt/auxiliary/math/m_imu_3dof.c
View file

@ -58,8 +58,14 @@ m_imu_3dof_add_vars(struct m_imu_3dof *f, void *root, const char *prefix)
u_var_add_ro_u64(root, &f->last.timestamp_ns, tmp);
snprintf(tmp, sizeof(tmp), "%slast.gyro", prefix);
u_var_add_ro_vec3_f32(root, &f->last.gyro, tmp);
snprintf(tmp, sizeof(tmp), "%slast.gyro_length", prefix);
u_var_add_ro_f32(root, &f->last.gyro_length, tmp);
snprintf(tmp, sizeof(tmp), "%slast.gyro_biased_length", prefix);
u_var_add_ro_f32(root, &f->last.gyro_biased_length, tmp);
snprintf(tmp, sizeof(tmp), "%slast.accel", prefix);
u_var_add_ro_vec3_f32(root, &f->last.accel, tmp);
snprintf(tmp, sizeof(tmp), "%slast.accel_length", prefix);
u_var_add_ro_f32(root, &f->last.accel_length, tmp);
snprintf(tmp, sizeof(tmp), "%slast.delta_ms", prefix);
u_var_add_ro_f32(root, &f->last.delta_ms, tmp);
@ -69,6 +75,10 @@ m_imu_3dof_add_vars(struct m_imu_3dof *f, void *root, const char *prefix)
u_var_add_ro_vec3_f32(root, &f->grav.error_axis, tmp);
snprintf(tmp, sizeof(tmp), "%sgrav.error_angle", prefix);
u_var_add_ro_f32(root, &f->grav.error_angle, tmp);
snprintf(tmp, sizeof(tmp), "%sgrav.is_accel", prefix);
u_var_add_bool(root, &f->grav.is_accel, tmp);
snprintf(tmp, sizeof(tmp), "%sgrav.is_rotating", prefix);
u_var_add_bool(root, &f->grav.is_rotating, tmp);
snprintf(tmp, sizeof(tmp), "%sgyro_bias.value", prefix);
u_var_add_ro_vec3_f32(root, &f->gyro_bias.value, tmp);
@ -102,11 +112,14 @@ gravity_correction(struct m_imu_3dof *f,
* reset the counter and start over.
*/
bool is_accel = fabsf(m_vec3_len(*accel) - 9.82f) >= gravity_tolerance;
float accel_length = m_vec3_len(*accel);
bool is_accel = fabsf(accel_length - 9.82f) >= gravity_tolerance;
bool is_rotating = gyro_length >= gyro_tolerance;
if (is_accel || is_rotating) {
f->grav.level_timestamp_ns = timestamp_ns;
}
f->grav.is_accel = is_accel;
f->grav.is_rotating = is_rotating;
/*
* Device has been level for long enough, grab mean from the
@ -229,6 +242,13 @@ m_imu_3dof_update(struct m_imu_3dof *f,
struct xrt_vec3 gyro_biased = m_vec3_sub(*gyro, f->gyro_bias.value);
float gyro_biased_length = m_vec3_len(gyro_biased);
float gyro_length = m_vec3_len(*gyro);
float accel_length = m_vec3_len(*accel);
f->last.accel_length = accel_length;
f->last.gyro_length = gyro_length;
f->last.gyro_biased_length = gyro_biased_length;
if (gyro_biased_length > 0.0001f) {
#if 0

5
src/xrt/auxiliary/math/m_imu_3dof.h
View file

@ -41,6 +41,9 @@ struct m_imu_3dof
struct xrt_vec3 gyro; //!< Angular velocity
struct xrt_vec3 accel; //!< Acceleration
float delta_ms;
float accel_length;
float gyro_length;
float gyro_biased_length;
} last;
enum m_imu_3dof_state state;
@ -57,6 +60,8 @@ struct m_imu_3dof
uint64_t level_timestamp_ns;
struct xrt_vec3 error_axis;
float error_angle;
bool is_accel;
bool is_rotating;
} grav;
// gyro bias correction