From ed42efc5cb49640df3e3ef44c90ca0558e04ab4b Mon Sep 17 00:00:00 2001
From: Osenberg <junyangqu@slightech.com>
Date: Mon, 25 Mar 2019 18:18:54 +0800
Subject: [PATCH] fix(src): fixed imu size not matched with old version

---
 src/mynteye/device/channel/channels.cc     | 10 ++----
 src/mynteye/device/channel/file_channel.cc | 37 ++++++++++++++++++++--
 src/mynteye/device/channel/file_channel.h  |  3 ++
 3 files changed, 41 insertions(+), 9 deletions(-)

diff --git a/src/mynteye/device/channel/channels.cc b/src/mynteye/device/channel/channels.cc
index 1cadb96..2dc47da 100644
--- a/src/mynteye/device/channel/channels.cc
+++ b/src/mynteye/device/channel/channels.cc
@@ -492,13 +492,9 @@ bool Channels::GetFiles(
           if (imu_params->ok) {
             auto &&n = file_channel_.GetImuParamsFromData(
                 data + i, file_size, imu_params);
-            // TODO(Kalman): Use CHECK_EQ
-            if (n != file_size) {
-              LOG(WARNING) << "Wrong sizes of imu params";
-            }
-            // CHECK_EQ(n, file_size)
-            //  << "The firmware not support getting device info, you could "
-            //     "upgrade to latest";
+            CHECK_EQ(n, file_size)
+              << "The firmware not support getting device info, you could "
+                 "upgrade to latest";
           }
         } break;
         default:
diff --git a/src/mynteye/device/channel/file_channel.cc b/src/mynteye/device/channel/file_channel.cc
index 6f31d0d..4dba195 100644
--- a/src/mynteye/device/channel/file_channel.cc
+++ b/src/mynteye/device/channel/file_channel.cc
@@ -443,9 +443,9 @@ std::size_t ImuParamsParser::GetFromData(
 std::size_t ImuParamsParser::SetToData(
     const imu_params_t *imu_params, std::uint8_t *data) const {
   if (spec_version_ >= Version(1, 2)) {
-    return SetToData_new(imu_params, data);
+    return SetToData_bool_new(imu_params, data, true);
   } else {
-    return SetToData_old(imu_params, data);
+    return SetToData_bool_new(imu_params, data, false);
   }
 }
 
@@ -524,4 +524,37 @@ std::size_t ImuParamsParser::SetToData_new(
   return size + 3;
 }
 
+std::size_t ImuParamsParser::SetToData_bool_new(
+    const imu_params_t *imu_params, std::uint8_t *data, bool is_set) const {
+  std::size_t i = 3;  // skip id, size
+
+  Version version_new(1, 2);  // new version
+  Version version_raw(imu_params->version);
+
+  // version, 2
+  data[i] = version_new.major();
+  data[i + 1] = version_new.minor();
+  i += 2;
+  // set imu params with new version format
+  if (version_raw <= version_new) {
+    if (is_set) {
+      i += bytes::to_data(&imu_params->in_accel, data + i, true);
+      i += bytes::to_data(&imu_params->in_gyro, data + i, true);
+      i += bytes::to_data(&imu_params->ex_left_to_imu, data + i);
+    } else {
+      i += bytes::to_data(&imu_params->in_accel, data + i, false);
+      i += bytes::to_data(&imu_params->in_gyro, data + i, false);
+      i += bytes::to_data(&imu_params->ex_left_to_imu, data + i);
+    }
+  } else {
+    LOG(FATAL) << "Could not set imu params of version "
+        << version_raw.to_string() << ", please use latest SDK.";
+  }
+  // others
+  std::size_t size = i - 3;
+  data[0] = FID_IMU_PARAMS;
+  data[1] = static_cast<std::uint8_t>((size >> 8) & 0xFF);
+  data[2] = static_cast<std::uint8_t>(size & 0xFF);
+  return size + 3;
+}
 MYNTEYE_END_NAMESPACE
diff --git a/src/mynteye/device/channel/file_channel.h b/src/mynteye/device/channel/file_channel.h
index 3f2c334..f2636f4 100644
--- a/src/mynteye/device/channel/file_channel.h
+++ b/src/mynteye/device/channel/file_channel.h
@@ -144,6 +144,9 @@ class ImuParamsParser {
   std::size_t SetToData_new(
       const imu_params_t *imu_params, std::uint8_t *data) const;
 
+  std::size_t SetToData_bool_new(
+      const imu_params_t *imu_params, std::uint8_t *data, bool is_set) const;
+
  private:
   Version spec_version_;
 };