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

t_imu: Actually return non-zero leftover acceleration data.

Browse source
This commit is contained in:
Ryan Pavlik 2019-11-12 12:14:24 -06:00 committed by Jakob Bornecrantz
parent 2823193302
commit e01cf5efd3
1 changed files with 12 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
src/xrt/auxiliary/tracking/t_imu.cpp
View file

@ -78,12 +78,14 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
assert(accel); assert(accel);
assert(accel_variance); assert(accel_variance);
assert(timestamp_ns != 0); assert(timestamp_ns != 0);
Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>();
fusion->simple_fusion.handleAccel(accelVec, timestamp_ns);
if (out_world_accel != NULL) { if (out_world_accel != NULL) {
U_ZERO(out_world_accel); Eigen::Vector3d worldAccel =
fusion->simple_fusion.getCorrectedWorldAccel(
accelVec);
map_vec3(*out_world_accel) = worldAccel.cast<float>();
} }
fusion->simple_fusion.handleAccel(
map_vec3(*accel).cast<double>(), timestamp_ns);
return 0; return 0;
} catch (...) { } catch (...) {
assert(false && assert(false &&
@ -177,15 +179,18 @@ imu_fusion_incorporate_gyros_and_accelerometer(
assert(accel); assert(accel);
assert(accel_variance); assert(accel_variance);
assert(timestamp_ns != 0); assert(timestamp_ns != 0);
if (out_world_accel != NULL) {
U_ZERO(out_world_accel);
}
Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>(); Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>();
Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>(); Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>();
fusion->simple_fusion.handleAccel(accelVec, timestamp_ns); fusion->simple_fusion.handleAccel(accelVec, timestamp_ns);
fusion->simple_fusion.handleGyro(angVelVec, timestamp_ns); fusion->simple_fusion.handleGyro(angVelVec, timestamp_ns);
fusion->simple_fusion.postCorrect(); fusion->simple_fusion.postCorrect();
if (out_world_accel != NULL) {
Eigen::Vector3d worldAccel =
fusion->simple_fusion.getCorrectedWorldAccel(
accelVec);
map_vec3(*out_world_accel) = worldAccel.cast<float>();
}
return 0; return 0;
} catch (...) { } catch (...) {
assert( assert(