From 2d1989fc2b6f7c8916a89da7997a929592e53474 Mon Sep 17 00:00:00 2001
From: Osenberg <junyangqu@slightech.com>
Date: Fri, 15 Mar 2019 21:01:49 +0800
Subject: [PATCH] feat(src): determines whether the imu assembly parameter is
 null

n
---
 src/mynteye/device/motions.cc | 40 +++++++++++++++++++++++++++++++++--
 src/mynteye/device/motions.h  |  1 +
 2 files changed, 39 insertions(+), 2 deletions(-)

diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc
index 6fbaea4..de708d5 100644
--- a/src/mynteye/device/motions.cc
+++ b/src/mynteye/device/motions.cc
@@ -169,7 +169,9 @@ Motions::motion_datas_t Motions::GetMotionDatas() {
 }
 
 void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
-  if (nullptr == motion_intrinsics_) return;
+  if (nullptr == motion_intrinsics_ ||
+      IsNullAssemblyOrTempDrift(ProcessMode::PROC_IMU_ASSEMBLY))
+    return;
 
   double dst[3][3] = {0};
   if (data->flag == 1) {
@@ -200,7 +202,9 @@ void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
 }
 
 void Motions::ProcImuTempDrift(std::shared_ptr<ImuData> data) const {
-  if (nullptr == motion_intrinsics_) return;
+  if (nullptr == motion_intrinsics_ ||
+      IsNullAssemblyOrTempDrift(ProcessMode::PROC_IMU_TEMP_DRIFT))
+    return;
 
   double temp = data->temperature;
   if (data->flag == 1) {
@@ -228,4 +232,36 @@ 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;
+    }
+  }
+
+  return true;
+}
+
 MYNTEYE_END_NAMESPACE
diff --git a/src/mynteye/device/motions.h b/src/mynteye/device/motions.h
index 74f42a0..6e8296e 100644
--- a/src/mynteye/device/motions.h
+++ b/src/mynteye/device/motions.h
@@ -52,6 +52,7 @@ class Motions {
  private:
   void ProcImuAssembly(std::shared_ptr<ImuData> data) const;
   void ProcImuTempDrift(std::shared_ptr<ImuData> data) const;
+  bool IsNullAssemblyOrTempDrift(const ProcessMode& mode) const;
 
   std::shared_ptr<Channels> channels_;