The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

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
··· 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
+7
src/xrt/auxiliary/math/m_imu_3dof.h
··· 58 58 struct xrt_vec3 error_axis; 59 59 float error_angle; 60 60 } grav; 61 + 62 + // gyro bias correction 63 + struct 64 + { 65 + struct xrt_vec3 value; 66 + bool manually_fire; 67 + } gyro_bias; 61 68 }; 62 69 63 70 void