From 25d3526086a91b84e6637045f8f53a3124a0c989 Mon Sep 17 00:00:00 2001
From: John Zhao <johnzhao@slightech.com>
Date: Wed, 16 May 2018 16:37:12 +0800
Subject: [PATCH] Fix imu track period

---
 src/internal/channels.cc | 23 ++++++++++++++---------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/internal/channels.cc b/src/internal/channels.cc
index 8faecdf..58eba47 100644
--- a/src/internal/channels.cc
+++ b/src/internal/channels.cc
@@ -286,22 +286,32 @@ void Channels::StartImuTracking(imu_callback_t callback) {
     imu_sn_ = 0;
     ImuReqPacket req_packet{imu_sn_};
     ImuResPacket res_packet;
-    auto sleep_ms = [](std::intmax_t n) {
-      std::this_thread::sleep_for(std::chrono::milliseconds(n));
+    auto sleep = [](const times::system_clock::time_point &time_beg) {
+      auto &&time_elapsed_ms =
+          times::count<times::milliseconds>(times::now() - time_beg);
+      if (time_elapsed_ms < IMU_TRACK_PERIOD) {
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(IMU_TRACK_PERIOD - time_elapsed_ms));
+        VLOG(2) << "Imu track cost " << time_elapsed_ms << " ms"
+                << ", sleep " << (IMU_TRACK_PERIOD - time_elapsed_ms) << " ms";
+      }
     };
     while (!imu_track_stop_) {
       auto &&time_beg = times::now();
 
       req_packet.serial_number = imu_sn_;
       if (!XuImuWrite(req_packet)) {
+        sleep(time_beg);
         continue;
       }
 
       if (!XuImuRead(&res_packet)) {
+        sleep(time_beg);
         continue;
       }
 
       if (res_packet.packets.size() == 0) {
+        sleep(time_beg);
         continue;
       }
 
@@ -317,6 +327,7 @@ void Channels::StartImuTracking(imu_callback_t callback) {
       auto &&sn = res_packet.packets.back().serial_number;
       if (imu_sn_ == sn) {
         VLOG(2) << "New imu not ready, dropped";
+        sleep(time_beg);
         continue;
       }
       imu_sn_ = sn;
@@ -329,13 +340,7 @@ void Channels::StartImuTracking(imu_callback_t callback) {
 
       res_packet.packets.clear();
 
-      auto &&time_elapsed_ms =
-          times::count<times::milliseconds>(times::now() - time_beg);
-      if (time_elapsed_ms < IMU_TRACK_PERIOD) {
-        sleep_ms(IMU_TRACK_PERIOD - time_elapsed_ms);
-        VLOG(2) << "Imu track cost " << time_elapsed_ms << " ms"
-                << ", sleep " << (IMU_TRACK_PERIOD - time_elapsed_ms) << " ms";
-      }
+      sleep(time_beg);
     }
   });
 }