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

t/imu: No fancy new c++ syntax until clang-format-8 is used on CI

Browse source 
Go to your room clang-format and think about what you have done.
This commit is contained in:
Jakob Bornecrantz 2019-10-29 20:08:44 +00:00
parent 477e740568
commit 46b9d28e8d
1 changed files with 110 additions and 84 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -26,35 +26,44 @@ struct imu_fusion
* API functions
*/
struct imu_fusion *
imu_fusion_create() try
imu_fusion_create()
{
auto fusion = std::make_unique<imu_fusion>();
return fusion.release();
} catch (...) {
return NULL;
try {
auto fusion = std::make_unique<imu_fusion>();
return fusion.release();
} catch (...) {
return NULL;
}
}
void
imu_fusion_destroy(struct imu_fusion *fusion) try {
delete fusion;
} catch (...) {
assert(false && "Caught exception on destroy");
imu_fusion_destroy(struct imu_fusion *fusion)
{
try {
delete fusion;
} catch (...) {
assert(false && "Caught exception on destroy");
}
}
int
imu_fusion_incorporate_gyros(struct imu_fusion *fusion,
float dt,
struct xrt_vec3 const *ang_vel,
struct xrt_vec3 const *variance) try {
assert(fusion);
assert(ang_vel);
assert(variance);
struct xrt_vec3 const *variance)
{
try {
assert(fusion);
assert(ang_vel);
assert(variance);
fusion->simple_fusion.handleGyro(map_vec3(*ang_vel).cast<double>(), dt);
return 0;
} catch (...) {
assert(false && "Caught exception on incorporate gyros");
return -1;
fusion->simple_fusion.handleGyro(
map_vec3(*ang_vel).cast<double>(), dt);
return 0;
} catch (...) {
assert(false && "Caught exception on incorporate gyros");
return -1;
}
}
int
@ -63,72 +72,85 @@ imu_fusion_incorporate_accelerometer(struct imu_fusion *fusion,
struct xrt_vec3 const *accel,
float scale,
struct xrt_vec3 const *reference,
struct xrt_vec3 const *variance) try {
assert(fusion);
assert(accel);
assert(variance);
struct xrt_vec3 const *variance)
{
try {
assert(fusion);
assert(accel);
assert(variance);
fusion->simple_fusion.handleAccel(map_vec3(*accel).cast<double>(), dt);
return 0;
} catch (...) {
assert(false && "Caught exception on incorporate accelerometer");
return -1;
fusion->simple_fusion.handleAccel(
map_vec3(*accel).cast<double>(), dt);
return 0;
} catch (...) {
assert(false &&
"Caught exception on incorporate accelerometer");
return -1;
}
}
int
imu_fusion_get_prediction(struct imu_fusion const *fusion,
float dt,
struct xrt_quat *out_quat,
struct xrt_vec3 *out_ang_vel) try {
assert(fusion);
assert(out_quat);
assert(out_ang_vel);
if (!fusion->simple_fusion.valid()) {
return -2;
}
struct xrt_vec3 *out_ang_vel)
{
try {
assert(fusion);
assert(out_quat);
assert(out_ang_vel);
if (!fusion->simple_fusion.valid()) {
return -2;
}
map_vec3(*out_ang_vel) =
fusion->simple_fusion.getAngVel().cast<float>();
if (dt == 0) {
// No need to predict here.
map_quat(*out_quat) =
fusion->simple_fusion.getQuat().cast<float>();
map_vec3(*out_ang_vel) =
fusion->simple_fusion.getAngVel().cast<float>();
if (dt == 0) {
// No need to predict here.
map_quat(*out_quat) =
fusion->simple_fusion.getQuat().cast<float>();
return 0;
}
Eigen::Quaterniond predicted_quat =
fusion->simple_fusion.getPredictedQuat(dt);
map_quat(*out_quat) = predicted_quat.cast<float>();
return 0;
}
Eigen::Quaterniond predicted_quat =
fusion->simple_fusion.getPredictedQuat(dt);
map_quat(*out_quat) = predicted_quat.cast<float>();
return 0;
} catch (...) {
assert(false && "Caught exception on getting prediction");
return -1;
} catch (...) {
assert(false && "Caught exception on getting prediction");
return -1;
}
}
int
imu_fusion_get_prediction_rotation_vec(struct imu_fusion const *fusion,
float dt,
struct xrt_vec3 *out_rotation_vec) try {
assert(fusion);
assert(out_rotation_vec);
struct xrt_vec3 *out_rotation_vec)
{
try {
assert(fusion);
assert(out_rotation_vec);
if (!fusion->simple_fusion.valid()) {
return -2;
if (!fusion->simple_fusion.valid()) {
return -2;
}
if (dt == 0) {
// No need to predict here.
map_vec3(*out_rotation_vec) =
fusion->simple_fusion.getRotationVec()
.cast<float>();
} else {
Eigen::Quaterniond predicted_quat =
fusion->simple_fusion.getPredictedQuat(dt);
map_vec3(*out_rotation_vec) =
flexkalman::util::quat_ln(predicted_quat)
.cast<float>();
}
return 0;
} catch (...) {
assert(false && "Caught exception on getting prediction");
return -1;
}
if (dt == 0) {
// No need to predict here.
map_vec3(*out_rotation_vec) =
fusion->simple_fusion.getRotationVec().cast<float>();
} else {
Eigen::Quaterniond predicted_quat =
fusion->simple_fusion.getPredictedQuat(dt);
map_vec3(*out_rotation_vec) =
flexkalman::util::quat_ln(predicted_quat).cast<float>();
}
return 0;
} catch (...) {
assert(false && "Caught exception on getting prediction");
return -1;
}
int
@ -140,22 +162,26 @@ imu_fusion_incorporate_gyros_and_accelerometer(
struct xrt_vec3 const *accel,
float accel_scale,
struct xrt_vec3 const *accel_reference,
struct xrt_vec3 const *accel_variance) try {
assert(fusion);
assert(ang_vel);
assert(ang_vel_variance);
assert(accel);
assert(accel_reference);
assert(accel_variance);
struct xrt_vec3 const *accel_variance)
{
try {
assert(fusion);
assert(ang_vel);
assert(ang_vel_variance);
assert(accel);
assert(accel_reference);
assert(accel_variance);
Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>();
Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>();
fusion->simple_fusion.handleAccel(accelVec, dt);
fusion->simple_fusion.handleGyro(angVelVec, dt);
fusion->simple_fusion.postCorrect();
return 0;
} catch (...) {
assert(false &&
"Caught exception on incorporate gyros and accelerometer");
return -1;
Eigen::Vector3d accelVec = map_vec3(*accel).cast<double>();
Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast<double>();
fusion->simple_fusion.handleAccel(accelVec, dt);
fusion->simple_fusion.handleGyro(angVelVec, dt);
fusion->simple_fusion.postCorrect();
return 0;
} catch (...) {
assert(
false &&
"Caught exception on incorporate gyros and accelerometer");
return -1;
}
}