From 01c3c7151689852938c868ea45abfe4b263f990c Mon Sep 17 00:00:00 2001
From: KalmanSLightech <kalmanxiong@slightech.com>
Date: Sat, 21 Jul 2018 17:09:47 +0800
Subject: [PATCH] fix tool bug

---
 src/internal/channels.cc |  4 ++++
 src/internal/motions.cc  | 13 ++++++++++---
 tools/dataset/dataset.cc | 11 +++++------
 3 files changed, 19 insertions(+), 9 deletions(-)

diff --git a/src/internal/channels.cc b/src/internal/channels.cc
index 88558ef..0afbbba 100644
--- a/src/internal/channels.cc
+++ b/src/internal/channels.cc
@@ -298,6 +298,10 @@ void Channels::DoImuTrack() {
     return;
   }
 
+  if (res_packet.packets.back().count == 0) {
+    return;
+  }
+
   VLOG(2) << "Imu req sn: " << imu_sn_ << ", res count: " << []() {
     std::size_t n = 0;
     for (auto &&packet : res_packet.packets) {
diff --git a/src/internal/motions.cc b/src/internal/motions.cc
index 881ac5f..b091f72 100644
--- a/src/internal/motions.cc
+++ b/src/internal/motions.cc
@@ -51,15 +51,22 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
         imu->flag = seg.flag;
         imu->temperature = seg.temperature / 326.8f + 25;
 
-        if((imu->flag) & 0x01 != 0) {
+        if(imu->flag == 1) {
           imu->accel[0] = seg.aceel_or_gyro[0] * 8.f / 0x10000;
           imu->accel[1] = seg.aceel_or_gyro[1] * 8.f / 0x10000;
           imu->accel[2] = seg.aceel_or_gyro[2] * 8.f / 0x10000;
-        } 
-        if((imu->flag) & 0x02 != 0) {
+          imu->gyro[0] = 0;
+          imu->gyro[1] = 0;
+          imu->gyro[2] = 0;
+        } else if(imu->flag == 2) {
+          imu->accel[0] = 0;
+          imu->accel[1] = 0;
+          imu->accel[2] = 0;
           imu->gyro[0] = seg.aceel_or_gyro[0] * 1000.f / 0x10000;
           imu->gyro[1] = seg.aceel_or_gyro[1] * 1000.f / 0x10000;
           imu->gyro[2] = seg.aceel_or_gyro[2] * 1000.f / 0x10000;
+        } else {
+          imu->Reset();
         }
         
         std::lock_guard<std::mutex> _(mtx_datas_);
diff --git a/tools/dataset/dataset.cc b/tools/dataset/dataset.cc
index d156c37..1fea9c9 100644
--- a/tools/dataset/dataset.cc
+++ b/tools/dataset/dataset.cc
@@ -79,12 +79,11 @@ void Dataset::SaveStreamData(
 void Dataset::SaveMotionData(const device::MotionData &data) {
   auto &&writer = GetMotionWriter();
   auto seq = motion_count_;
-  writer->ofs << seq << ", " << data.imu->frame_id << ", "
-              << data.imu->timestamp << ", " << data.imu->accel[0] << ", "
-              << data.imu->accel[1] << ", " << data.imu->accel[2] << ", "
-              << data.imu->gyro[0] << ", " << data.imu->gyro[1] << ", "
-              << data.imu->gyro[2] << ", " << data.imu->temperature
-              << std::endl;
+  writer->ofs << seq << ", " << data.imu->timestamp << ", "
+              << data.imu->accel[0] << ", " << data.imu->accel[1] << ", "
+              << data.imu->accel[2] << ", " << data.imu->gyro[0] << ", "
+              << data.imu->gyro[1] << ", " <<  data.imu->gyro[2] << ", "
+              << data.imu->temperature << std::endl;           
   ++motion_count_;
 }