The open source OpenXR runtime
m/3dof: Add manual way to set the gyro bias
authored by
Jakob Bornecrantz
and committed by
Jakob Bornecrantz
5befa76f
f51851d0
+43
-7
+36
-7
src/xrt/auxiliary/math/m_imu_3dof.c
+36
-7
src/xrt/auxiliary/math/m_imu_3dof.c
···
69
69
u_var_add_ro_vec3_f32(root, &f->grav.error_axis, tmp);
70
70
snprintf(tmp, sizeof(tmp), "%sgrav.error_angle", prefix);
71
71
u_var_add_ro_f32(root, &f->grav.error_angle, tmp);
72
+
73
+
snprintf(tmp, sizeof(tmp), "%sgyro_bias.value", prefix);
74
+
u_var_add_ro_vec3_f32(root, &f->gyro_bias.value, tmp);
75
+
snprintf(tmp, sizeof(tmp), "%sgyro_bias.manually_fire", prefix);
76
+
u_var_add_bool(root, &f->gyro_bias.manually_fire, tmp);
72
77
}
73
78
74
79
static void
···
171
176
}
172
177
}
173
178
179
+
static void
180
+
gyro_biasing(struct m_imu_3dof *f, uint64_t timestamp_ns)
181
+
{
182
+
if (!f->gyro_bias.manually_fire) {
183
+
return;
184
+
}
185
+
186
+
f->gyro_bias.manually_fire = false;
187
+
188
+
uint64_t dur_ns = DUR_300MS_IN_NS;
189
+
190
+
struct xrt_vec3 gyro_mean = XRT_VEC3_ZERO;
191
+
m_ff_vec3_f32_filter(f->gyro_ff, // Filter
192
+
timestamp_ns - dur_ns, // Start time
193
+
timestamp_ns, // End time
194
+
&gyro_mean); // Results
195
+
196
+
f->gyro_bias.value = gyro_mean;
197
+
}
198
+
174
199
void
175
200
m_imu_3dof_update(struct m_imu_3dof *f,
176
201
uint64_t timestamp_ns,
···
202
227
m_ff_vec3_f32_push(f->word_accel_ff, &world_accel, timestamp_ns);
203
228
m_ff_vec3_f32_push(f->gyro_ff, gyro, timestamp_ns);
204
229
205
-
float gyro_length = m_vec3_len(*gyro);
230
+
struct xrt_vec3 gyro_biased = m_vec3_sub(*gyro, f->gyro_bias.value);
231
+
float gyro_biased_length = m_vec3_len(gyro_biased);
206
232
207
-
if (gyro_length > 0.0001f) {
233
+
if (gyro_biased_length > 0.0001f) {
208
234
#if 0
209
235
math_quat_integrate_velocity(&f->rot, gyro, dt, &f->rot);
210
236
#else
211
237
struct xrt_vec3 rot_axis = {
212
-
gyro->x / gyro_length,
213
-
gyro->y / gyro_length,
214
-
gyro->z / gyro_length,
238
+
gyro_biased.x / gyro_biased_length,
239
+
gyro_biased.y / gyro_biased_length,
240
+
gyro_biased.z / gyro_biased_length,
215
241
};
216
242
217
-
float rot_angle = gyro_length * dt;
243
+
float rot_angle = gyro_biased_length * dt;
218
244
219
245
struct xrt_quat delta_orient;
220
246
math_quat_from_angle_vector(rot_angle, &rot_axis, &delta_orient);
···
224
250
}
225
251
226
252
// Gravity correction.
227
-
gravity_correction(f, timestamp_ns, accel, gyro, dt, gyro_length);
253
+
gravity_correction(f, timestamp_ns, accel, &gyro_biased, dt, gyro_biased_length);
254
+
255
+
// Gyro bias calculations.
256
+
gyro_biasing(f, timestamp_ns);
228
257
229
258
/*
230
259
* Mitigate drift due to floating point