From 46b9d28e8dd4b7476033ec6713442b04a353b426 Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Tue, 29 Oct 2019 20:08:44 +0000 Subject: [PATCH] t/imu: No fancy new c++ syntax until clang-format-8 is used on CI Go to your room clang-format and think about what you have done. --- src/xrt/auxiliary/tracking/t_imu.cpp | 194 +++++++++++++++------------ 1 file changed, 110 insertions(+), 84 deletions(-) diff --git a/src/xrt/auxiliary/tracking/t_imu.cpp b/src/xrt/auxiliary/tracking/t_imu.cpp index 4e4ff875d..05c84855e 100644 --- a/src/xrt/auxiliary/tracking/t_imu.cpp +++ b/src/xrt/auxiliary/tracking/t_imu.cpp @@ -26,35 +26,44 @@ struct imu_fusion * API functions */ struct imu_fusion * -imu_fusion_create() try +imu_fusion_create() { - auto fusion = std::make_unique(); - return fusion.release(); -} catch (...) { - return NULL; + try { + auto fusion = std::make_unique(); + 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(), dt); - return 0; -} catch (...) { - assert(false && "Caught exception on incorporate gyros"); - return -1; + fusion->simple_fusion.handleGyro( + map_vec3(*ang_vel).cast(), 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(), dt); - return 0; -} catch (...) { - assert(false && "Caught exception on incorporate accelerometer"); - return -1; + fusion->simple_fusion.handleAccel( + map_vec3(*accel).cast(), 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(); - if (dt == 0) { - // No need to predict here. - map_quat(*out_quat) = - fusion->simple_fusion.getQuat().cast(); + map_vec3(*out_ang_vel) = + fusion->simple_fusion.getAngVel().cast(); + if (dt == 0) { + // No need to predict here. + map_quat(*out_quat) = + fusion->simple_fusion.getQuat().cast(); + return 0; + } + Eigen::Quaterniond predicted_quat = + fusion->simple_fusion.getPredictedQuat(dt); + map_quat(*out_quat) = predicted_quat.cast(); return 0; - } - Eigen::Quaterniond predicted_quat = - fusion->simple_fusion.getPredictedQuat(dt); - map_quat(*out_quat) = predicted_quat.cast(); - 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(); + } else { + Eigen::Quaterniond predicted_quat = + fusion->simple_fusion.getPredictedQuat(dt); + map_vec3(*out_rotation_vec) = + flexkalman::util::quat_ln(predicted_quat) + .cast(); + } + 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(); - } else { - Eigen::Quaterniond predicted_quat = - fusion->simple_fusion.getPredictedQuat(dt); - map_vec3(*out_rotation_vec) = - flexkalman::util::quat_ln(predicted_quat).cast(); - } - 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(); - Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast(); - 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(); + Eigen::Vector3d angVelVec = map_vec3(*ang_vel).cast(); + 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; + } }