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: 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_fusion.h"
#include "math/m_eigen_interop.h"
#include "util/u_misc.h"
#include <memory>
@ -69,13 +70,17 @@ int
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
uint64_t timestamp_ns,
struct xrt_vec3 const *accel,
struct xrt_vec3 const *accel_variance)
struct xrt_vec3 const *accel_variance,
struct xrt_vec3 *out_world_accel)
{
try {
assert(fusion);
assert(accel);
assert(accel_variance);
assert(timestamp_ns != 0);
if (out_world_accel != NULL) {
U_ZERO(out_world_accel);
}
fusion->simple_fusion.handleAccel(
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_variance,
struct xrt_vec3 const *accel,
struct xrt_vec3 const *accel_variance)
struct xrt_vec3 const *accel_variance,
struct xrt_vec3 *out_world_accel)
{
try {
assert(fusion);
@ -171,6 +177,9 @@ imu_fusion_incorporate_gyros_and_accelerometer(
assert(accel);
assert(accel_variance);
assert(timestamp_ns != 0);
if (out_world_accel != NULL) {
U_ZERO(out_world_accel);
}
Eigen::Vector3d accelVec = map_vec3(*accel).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.
* @param accel_variance The variance of the accelerometer measurements: part of
* 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
* @ingroup aux_tracking
@ -92,7 +94,8 @@ int
imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
uint64_t timestamp_ns,
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
@ -112,6 +115,8 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
* assumed to be +y when aligned with the world.
* @param accel_variance The variance of the accelerometer measurements: part of
* 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
* @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_variance,
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.

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(
psmv->fusion.fusion, timestamp_ns, &psmv->read.gyro,
&psmv->fusion.variance.gyro, &psmv->read.accel,
&psmv->fusion.variance.accel);
&psmv->fusion.variance.accel, NULL);
struct xrt_vec3 angvel_dummy;
imu_fusion_get_prediction(psmv->fusion.fusion, timestamp_ns,
&psmv->fusion.rot, &angvel_dummy);