diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index 41f249a..029a0bb 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -610,6 +610,8 @@ void Device::ReadAllInfos() { device_info_->name = uvc::get_name(*device_); + motions_->SetDeviceInfo(device_info_); + bool img_params_ok = false; for (auto &¶ms : all_img_params_) { auto &&img_params = params.second; diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc index de708d5..a2b5811 100644 --- a/src/mynteye/device/motions.cc +++ b/src/mynteye/device/motions.cc @@ -170,7 +170,7 @@ Motions::motion_datas_t Motions::GetMotionDatas() { void Motions::ProcImuAssembly(std::shared_ptr data) const { if (nullptr == motion_intrinsics_ || - IsNullAssemblyOrTempDrift(ProcessMode::PROC_IMU_ASSEMBLY)) + IsNullAssemblyOrTempDrift()) return; double dst[3][3] = {0}; @@ -203,7 +203,7 @@ void Motions::ProcImuAssembly(std::shared_ptr data) const { void Motions::ProcImuTempDrift(std::shared_ptr data) const { if (nullptr == motion_intrinsics_ || - IsNullAssemblyOrTempDrift(ProcessMode::PROC_IMU_TEMP_DRIFT)) + IsNullAssemblyOrTempDrift()) return; double temp = data->temperature; @@ -232,36 +232,18 @@ void Motions::EnableProcessMode(const std::int32_t& mode) { proc_mode_ = mode; } -bool Motions::IsNullAssemblyOrTempDrift(const ProcessMode& mode) const { - if (mode == ProcessMode::PROC_IMU_ASSEMBLY) { - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - if (motion_intrinsics_->accel.assembly[i][j] != 0.0) - return false; - } - } - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - if (motion_intrinsics_->gyro.assembly[i][j] != 0.0) - return false; - } - } - } else if (mode == ProcessMode::PROC_IMU_TEMP_DRIFT) { - for (int i = 0; i < 2; i++) { - if (motion_intrinsics_->accel.x[i] != 0.0 || - motion_intrinsics_->accel.y[i] != 0.0 || - motion_intrinsics_->accel.z[i] != 0.0) - return false; - } - for (int i = 0; i < 2; i++) { - if (motion_intrinsics_->gyro.x[i] != 0 || - motion_intrinsics_->gyro.y[i] != 0 || - motion_intrinsics_->gyro.z[i] != 0) - return false; - } - } +bool Motions::IsNullAssemblyOrTempDrift() const { + if (!device_info_) + return true; + + if (device_info_->spec_version >= Version(1, 2)) + return false; return true; } +void Motions::SetDeviceInfo(const std::shared_ptr& in) { + device_info_ = in; +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/motions.h b/src/mynteye/device/motions.h index 6e8296e..564e6cd 100644 --- a/src/mynteye/device/motions.h +++ b/src/mynteye/device/motions.h @@ -21,6 +21,7 @@ #include "mynteye/mynteye.h" #include "mynteye/device/callbacks.h" +#include "mynteye/device/types.h" MYNTEYE_BEGIN_NAMESPACE @@ -49,10 +50,12 @@ class Motions { void SetMotionIntrinsics(const std::shared_ptr& in); void EnableProcessMode(const std::int32_t& mode); + void SetDeviceInfo(const std::shared_ptr& in); + private: void ProcImuAssembly(std::shared_ptr data) const; void ProcImuTempDrift(std::shared_ptr data) const; - bool IsNullAssemblyOrTempDrift(const ProcessMode& mode) const; + bool IsNullAssemblyOrTempDrift() const; std::shared_ptr channels_; @@ -71,6 +74,7 @@ class Motions { std::int32_t proc_mode_; std::shared_ptr motion_intrinsics_; + std::shared_ptr device_info_; }; MYNTEYE_END_NAMESPACE