From 9f83e83cb6f1efe17dc62178d375a01168da22f2 Mon Sep 17 00:00:00 2001
From: TinyO <tinysilk@hotmail.com>
Date: Wed, 28 Aug 2019 17:51:53 +0800
Subject: [PATCH] fix(*): imu display blink.

---
 samples/util_cv_painter.cc | 42 ++++++++++++++++++++++++++++++++------
 1 file changed, 36 insertions(+), 6 deletions(-)

diff --git a/samples/util_cv_painter.cc b/samples/util_cv_painter.cc
index ac14c4d..ab694c9 100644
--- a/samples/util_cv_painter.cc
+++ b/samples/util_cv_painter.cc
@@ -116,18 +116,48 @@ cv::Rect CVPainter::DrawImuData(
   int sign = 1;
   if (gravity == BOTTOM_LEFT || gravity == BOTTOM_RIGHT)
     sign = -1;
-
-  Clear(ss) << "stamp: " << data.timestamp
+  static auto fmt_time = NewFormat(0, 2);
+  Clear(ss) << "stamp: " << fmt_time << (0.001f * data.timestamp)
             << ", temp: " << fmt_temp << data.temperature;
   cv::Rect rect_i = DrawText(img, ss.str(), gravity, 5);
 
-  Clear(ss) << "accel(x,y,z): " << fmt_imu << data.accel[0] << "," << fmt_imu
-            << data.accel[1] << "," << fmt_imu << data.accel[2];
+  static double accel1_s = 0.0;
+  static double accel2_s = 0.0;
+  static double accel3_s = 0.0;
+
+  if (data.accel[0] > 0.000001 ||
+      data.accel[1] > 0.000001 ||
+      data.accel[2] > 0.000001 ||
+      data.accel[0] < -0.000001 ||
+      data.accel[1] < -0.000001 ||
+      data.accel[2] < -0.000001) {
+    accel1_s = data.accel[0];
+    accel2_s = data.accel[1];
+    accel3_s = data.accel[2];
+  }
+
+  Clear(ss) << "accel(x,y,z): " << fmt_imu << accel1_s << "," << fmt_imu
+            << accel2_s << "," << fmt_imu << accel3_s;
   cv::Rect rect_a =
       DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height));
 
-  Clear(ss) << "gyro(x,y,z): " << fmt_imu << data.gyro[0] << "," << fmt_imu
-            << data.gyro[1] << "," << fmt_imu << data.gyro[2];
+  static double gyro1_s = 0.0;
+  static double gyro2_s = 0.0;
+  static double gyro3_s = 0.0;
+
+  if (data.gyro[0] > 0.01 ||
+      data.gyro[1] > 0.01 ||
+      data.gyro[2] > 0.01 ||
+      data.gyro[0] < -0.01 ||
+      data.gyro[1] < -0.01 ||
+      data.gyro[2] < -0.01 ) {
+    gyro1_s = data.gyro[0];
+    gyro2_s = data.gyro[1];
+    gyro3_s = data.gyro[2];
+  }
+
+  Clear(ss) << "gyro(x,y,z): " << fmt_imu << gyro1_s << "," << fmt_imu
+            << gyro2_s << "," << fmt_imu << gyro3_s;
   cv::Rect rect_g = DrawText(
       img, ss.str(), gravity, 5, 0,
       sign * (10 + rect_i.height + rect_a.height));