Merge pull request #6203 from doronhi/rsusb_delay

Fix bug - RSUSB messaging takes 20 msec

src/concurrency.h

@@ -273,7 +273,7 @@ class dispatcher
 
         //wait
         std::unique_lock<std::mutex> lk(_blocking_invoke_mutex);
-        while(_blocking_invoke_cv.wait_for(lk, std::chrono::milliseconds(10), [&](){ return !done && !exit_condition(); }));
+        _blocking_invoke_cv.wait(lk, [&](){ return done || exit_condition(); });
     }
 
     void start()

src/global_timestamp_reader.cpp

@@ -47,6 +47,14 @@ namespace librealsense
         calc_linear_coefs();
     }
 
+    void CLinearCoefficients::add_const_y_coefs(double dy)
+    {
+        for (auto &&sample : _last_values)
+        {
+            sample._y += dy;
+        }
+    }
+
     void CLinearCoefficients::calc_linear_coefs()
     {
         // Calculate linear coefficients, based on calculus described in: https://www.statisticshowto.datasciencecentral.com/probability-and-statistics/regression-analysis/find-a-linear-regression-equation/
@@ -117,6 +125,7 @@ namespace librealsense
         _coefs(15),
         _users_count(0),
         _is_ready(false),
+        _min_command_delay(1000),
         _active_object([this](dispatcher::cancellable_timer cancellable_timer)
             {
                 polling(cancellable_timer);
@@ -166,9 +175,14 @@ namespace librealsense
 
             double sample_hw_time = _device->get_device_time_ms();
             double system_time_finish = duration<double, std::milli>(system_clock::now().time_since_epoch()).count();
-            if (system_time_finish - system_time_start > 2.0)
-                throw io_exception("get_device_time_ms() took too long (more then 2 mSecs)");
-            double system_time((system_time_finish + system_time_start) / 2);
+            double command_delay = (system_time_finish-system_time_start)/2;
+
+            if (command_delay < _min_command_delay)
+            {
+                _coefs.add_const_y_coefs(command_delay - _min_command_delay);
+                _min_command_delay = command_delay;
+            }
+            double system_time(system_time_finish - _min_command_delay);
             if (sample_hw_time < _last_sample_hw_time)
             {
                 // A time loop happend:

src/global_timestamp_reader.h

@@ -36,6 +36,7 @@ namespace librealsense
         CLinearCoefficients(unsigned int buffer_size);
         void reset();
         void add_value(CSample val);
+        void add_const_y_coefs(double dy);
         void update_linear_coefs(double x);
         double calc_value(double x) const;
         bool is_full() const;
@@ -79,6 +80,7 @@ namespace librealsense
         mutable std::recursive_mutex _read_mtx; // Watch only 1 reader at a time.
         mutable std::recursive_mutex _enable_mtx; // Watch only 1 start/stop operation at a time.
         CLinearCoefficients _coefs;
+        double _min_command_delay;
         bool _is_ready;
     };