mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-04 06:06:17 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

t_imu: Add param for accelerometer output

Browse source
This commit is contained in:
Ryan Pavlik 2019-11-12 11:43:17 -06:00 committed by Jakob Bornecrantz
parent a17413744b
commit 21352ca338
3 changed files with 20 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -11,6 +11,7 @@
#include "t_imu.h" #include "t_imu.h"
#include "t_imu_fusion.h" #include "t_imu_fusion.h"
#include "math/m_eigen_interop.h" #include "math/m_eigen_interop.h"
#include "util/u_misc.h"
#include <memory> #include <memory>
@ -69,13 +70,17 @@ int
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
uint64_t timestamp_ns, uint64_t timestamp_ns,
struct xrt_vec3 const *accel, struct xrt_vec3 const *accel,
struct xrt_vec3 const *accel_variance) struct xrt_vec3 const *accel_variance,
struct xrt_vec3 *out_world_accel)
{ {
try { try {
assert(fusion); assert(fusion);
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);
}
fusion->simple_fusion.handleAccel( fusion->simple_fusion.handleAccel(
map_vec3(*accel).cast<double>(), timestamp_ns); map_vec3(*accel).cast<double>(), timestamp_ns);
@ -162,7 +167,8 @@ imu_fusion_incorporate_gyros_and_accelerometer(
struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel,
struct xrt_vec3 const *ang_vel_variance, struct xrt_vec3 const *ang_vel_variance,
struct xrt_vec3 const *accel, struct xrt_vec3 const *accel,
struct xrt_vec3 const *accel_variance) struct xrt_vec3 const *accel_variance,
struct xrt_vec3 *out_world_accel)
{ {
try { try {
assert(fusion); assert(fusion);
@ -171,6 +177,9 @@ 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>();

10
src/xrt/auxiliary/tracking/t_imu.h
View file

@ -84,6 +84,8 @@ imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
* assumed to be +y when aligned with the world. * assumed to be +y when aligned with the world.
* @param accel_variance The variance of the accelerometer measurements: part of * @param accel_variance The variance of the accelerometer measurements: part of
* the characteristics of the IMU being used. * the characteristics of the IMU being used.
* @param out_world_accel Optional output parameter: will contain the
* non-gravity acceleration in the world frame.
* *
* @public @memberof imu_fusion * @public @memberof imu_fusion
* @ingroup aux_tracking * @ingroup aux_tracking
@ -92,7 +94,8 @@ int
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion, imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
uint64_t timestamp_ns, uint64_t timestamp_ns,
struct xrt_vec3 const *accel, struct xrt_vec3 const *accel,
struct xrt_vec3 const *accel_variance); struct xrt_vec3 const *accel_variance,
struct xrt_vec3 *out_world_accel);
/*! /*!
* Predict and correct fusion with a simultaneous accelerometer and gyroscope * Predict and correct fusion with a simultaneous accelerometer and gyroscope
@ -112,6 +115,8 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
* assumed to be +y when aligned with the world. * assumed to be +y when aligned with the world.
* @param accel_variance The variance of the accelerometer measurements: part of * @param accel_variance The variance of the accelerometer measurements: part of
* the characteristics of the IMU being used. * the characteristics of the IMU being used.
* @param out_world_accel Optional output parameter: will contain the
* non-gravity acceleration in the world frame.
* *
* @public @memberof imu_fusion * @public @memberof imu_fusion
* @ingroup aux_tracking * @ingroup aux_tracking
@ -123,7 +128,8 @@ imu_fusion_incorporate_gyros_and_accelerometer(
struct xrt_vec3 const *ang_vel, struct xrt_vec3 const *ang_vel,
struct xrt_vec3 const *ang_vel_variance, struct xrt_vec3 const *ang_vel_variance,
struct xrt_vec3 const *accel, struct xrt_vec3 const *accel,
struct xrt_vec3 const *accel_variance); struct xrt_vec3 const *accel_variance,
struct xrt_vec3 *out_world_accel);
/*! /*!
* Get the predicted state. Does not advance the internal state clock. * Get the predicted state. Does not advance the internal state clock.

2
src/xrt/drivers/psmv/psmv_driver.c
View file

@ -651,7 +651,7 @@ update_fusion(struct psmv_device *psmv,
imu_fusion_incorporate_gyros_and_accelerometer( imu_fusion_incorporate_gyros_and_accelerometer(
psmv->fusion.fusion, timestamp_ns, &psmv->read.gyro, psmv->fusion.fusion, timestamp_ns, &psmv->read.gyro,
&psmv->fusion.variance.gyro, &psmv->read.accel, &psmv->fusion.variance.gyro, &psmv->read.accel,
&psmv->fusion.variance.accel); &psmv->fusion.variance.accel, NULL);
struct xrt_vec3 angvel_dummy; struct xrt_vec3 angvel_dummy;
imu_fusion_get_prediction(psmv->fusion.fusion, timestamp_ns, imu_fusion_get_prediction(psmv->fusion.fusion, timestamp_ns,
&psmv->fusion.rot, &angvel_dummy); &psmv->fusion.rot, &angvel_dummy);