From cad2d26132d59acea2598fb85476237a7d24d87f Mon Sep 17 00:00:00 2001
From: Adam A <51179158+krazycoder2k@users.noreply.github.com>
Date: Fri, 2 Oct 2020 13:39:04 -0400
Subject: [PATCH] Update rs-motion.cpp

On instances where the first accelerometer frame arrives before the first gyro frame the "first" flag is set to false by process_accel which causes the first call to process_gryo to calculate the wrong ts value.
---
 examples/motion/rs-motion.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/examples/motion/rs-motion.cpp b/examples/motion/rs-motion.cpp
index 3ca1db59e9..5d33663daf 100644
--- a/examples/motion/rs-motion.cpp
+++ b/examples/motion/rs-motion.cpp
@@ -117,15 +117,17 @@ class rotation_estimator
     /* alpha indicates the part that gyro and accelerometer take in computation of theta; higher alpha gives more weight to gyro, but too high
     values cause drift; lower alpha gives more weight to accelerometer, which is more sensitive to disturbances */
     float alpha = 0.98;
-    bool first = true;
+    bool firstGyro = true;
+    bool firstAccel = true;
     // Keeps the arrival time of previous gyro frame
     double last_ts_gyro = 0;
 public:
     // Function to calculate the change in angle of motion based on data from gyro
     void process_gyro(rs2_vector gyro_data, double ts)
     {
-        if (first) // On the first iteration, use only data from accelerometer to set the camera's initial position
+        if (firstGyro) // On the first iteration, use only data from accelerometer to set the camera's initial position
         {
+            firstGyro = false;
             last_ts_gyro = ts;
             return;
         }
@@ -160,9 +162,9 @@ class rotation_estimator
 
         // If it is the first iteration, set initial pose of camera according to accelerometer data (note the different handling for Y axis)
         std::lock_guard<std::mutex> lock(theta_mtx);
-        if (first)
+        if (firstAccel)
         {
-            first = false;
+            firstAccel = false;
             theta = accel_angle;
             // Since we can't infer the angle around Y axis using accelerometer data, we'll use PI as a convetion for the initial pose
             theta.y = PI;