globalTimeStamp - going to idle

include/librealsense2/h/rs_types.h

@@ -167,6 +167,7 @@ typedef enum rs2_extension
     RS2_EXTENSION_POSE,
     RS2_EXTENSION_POSE_SENSOR,
     RS2_EXTENSION_WHEEL_ODOMETER,
+    RS2_EXTENSION_GLOBAL_TIMER,
     RS2_EXTENSION_COUNT
 } rs2_extension;
 const char* rs2_extension_type_to_string(rs2_extension type);

src/device.cpp

@@ -229,11 +229,6 @@ void device::hardware_reset()
     throw not_implemented_exception(to_string() << __FUNCTION__ << " is not implemented for this device!");
 }
 
-double device::get_device_time()
-{
-    throw not_implemented_exception(to_string() << __FUNCTION__ << " is not implemented for this device!");
-}
-
 std::shared_ptr<matcher> librealsense::device::create_matcher(const frame_holder& frame) const
 {
 

src/device.h

@@ -77,7 +77,6 @@ namespace librealsense
         virtual bool compress_while_record() const override { return true; }
 
         virtual bool contradicts(const stream_profile_interface* a, const std::vector<stream_profile>& others) const override;
-        virtual double get_device_time(); // Returns time in miliseconds.
 
     protected:
         int add_sensor(std::shared_ptr<sensor_interface> sensor_base);

src/ds5/ds5-device.cpp

@@ -341,12 +341,11 @@ namespace librealsense
 
     ds5_device::ds5_device(std::shared_ptr<context> ctx,
                            const platform::backend_device_group& group)
-        : device(ctx, group),
+        : device(ctx, group), global_time_interface(), 
           _depth_stream(new stream(RS2_STREAM_DEPTH)),
           _left_ir_stream(new stream(RS2_STREAM_INFRARED, 1)),
           _right_ir_stream(new stream(RS2_STREAM_INFRARED, 2)),
-          _device_capabilities(ds::d400_caps::CAP_UNDEFINED),
-          _tf_keeper(std::make_shared<time_diff_keeper>(this, 100))
+          _device_capabilities(ds::d400_caps::CAP_UNDEFINED)
     {
         _depth_device_idx = add_sensor(create_depth_device(ctx, group.uvc_devices));
         init(ctx, group);
@@ -618,10 +617,6 @@ namespace librealsense
             });
             notification_thread.detach();
         }
-        if (dynamic_cast<const platform::playback_backend*>(&(ctx->get_backend())) == nullptr)
-            _tf_keeper->start();
-        else
-            LOG_WARNING("playback_backend - global time_keeper unavailable.");
     }
 
     notification ds5_notification_decoder::decode(int value)
@@ -655,8 +650,14 @@ namespace librealsense
     }
 
 
-    double ds5_device::get_device_time()
+    double ds5_device::get_device_time_ms()
     {
+        // TODO: Refactor the following query with an extension.
+        if (dynamic_cast<const platform::playback_backend*>(&(get_context()->get_backend())) != nullptr)
+        {
+            throw not_implemented_exception("device time not supported for backend.");
+        }
+
         if (!_hw_monitor)
             throw wrong_api_call_sequence_exception("_hw_monitor is not initialized yet");
 

src/ds5/ds5-device.h

@@ -27,7 +27,7 @@ namespace librealsense
         const hw_monitor& _hw_monitor;
     };
 
-    class ds5_device : public virtual device, public debug_interface
+    class ds5_device : public virtual device, public debug_interface, public global_time_interface
     {
     public:
         std::shared_ptr<uvc_sensor> create_depth_device(std::shared_ptr<context> ctx,
@@ -47,7 +47,7 @@ namespace librealsense
         void create_snapshot(std::shared_ptr<debug_interface>& snapshot) const override;
         void enable_recording(std::function<void(const debug_interface&)> record_action) override;
         platform::usb_spec get_usb_spec() const;
-        double get_device_time();
+        virtual double get_device_time_ms();
 
     protected:
 
@@ -79,7 +79,6 @@ namespace librealsense
 
         std::unique_ptr<polling_error_handler> _polling_error_handler;
         std::shared_ptr<lazy<rs2_extrinsics>> _left_right_extrinsics;
-        std::shared_ptr<time_diff_keeper> _tf_keeper;
     };
 
     class ds5u_device : public ds5_device

src/global_timestamp_reader.cpp

@@ -96,11 +96,13 @@ namespace librealsense
         return y;
     }
 
-    time_diff_keeper::time_diff_keeper(device* dev, const unsigned int sampling_interval_ms) :
+    time_diff_keeper::time_diff_keeper(global_time_interface* dev, const unsigned int sampling_interval_ms) :
         _device(dev),
         _poll_intervals_ms(sampling_interval_ms),
         _last_sample_hw_time(1e+200),
         _coefs(15),
+        _users_count(0),
+        _is_ready(false),
         _active_object([this](dispatcher::cancellable_timer cancellable_timer)
             {
                 polling(cancellable_timer);
@@ -111,9 +113,28 @@ namespace librealsense
 
     void time_diff_keeper::start()
     {
+        std::lock_guard<std::recursive_mutex> lock(_enable_mtx);
+        _users_count++;
+        LOG_DEBUG("time_diff_keeper::start: _users_count = " << _users_count);
         _active_object.start();
     }
 
+    void time_diff_keeper::stop()
+    {
+        std::lock_guard<std::recursive_mutex> lock(_enable_mtx);
+        if (_users_count <= 0)
+            LOG_ERROR("time_diff_keeper users_count <= 0.");
+
+        _users_count--;
+        LOG_DEBUG("time_diff_keeper::stop: _users_count = " << _users_count);
+        if (_users_count == 0)
+        {
+            LOG_DEBUG("time_diff_keeper::stop: stop object.");
+            _active_object.stop();
+            _coefs.reset();
+        }
+    }
+
     time_diff_keeper::~time_diff_keeper()
     {
         _active_object.stop();
@@ -124,11 +145,14 @@ namespace librealsense
         using namespace std::chrono;
         try
         {
+            if (!_users_count)
+                throw wrong_api_call_sequence_exception("time_diff_keeper::update_diff_time called before object started.");
             LOG_DEBUG("time_diff_keeper::update_diff_time - in");
             std::lock_guard<std::recursive_mutex> lock(_mtx);
             LOG_DEBUG("time_diff_keeper::update_diff_time - lock");
             double system_time_start = duration<double, std::milli>(system_clock::now().time_since_epoch()).count();
-            double sample_hw_time = _device->get_device_time();
+
+            double sample_hw_time = _device->get_device_time_ms();
             double system_time_finish = duration<double, std::milli>(system_clock::now().time_since_epoch()).count();
             double system_time((system_time_finish + system_time_start) / 2);
             if (sample_hw_time < _last_sample_hw_time)
@@ -140,6 +164,7 @@ namespace librealsense
             _last_sample_hw_time = sample_hw_time;
             CSample crnt_sample(_last_sample_hw_time, system_time);
             _coefs.add_value(crnt_sample);
+            _is_ready = true;
             LOG_DEBUG("time_diff_keeper::update_diff_time - unlock");
             return true;
         }
@@ -171,7 +196,7 @@ namespace librealsense
         }
     }
 
-    double time_diff_keeper::get_system_hw_time(double crnt_hw_time)
+    double time_diff_keeper::get_system_hw_time(double crnt_hw_time, bool& is_ready)
     {
         static const double possible_loop_time(3000);
         {
@@ -184,15 +209,20 @@ namespace librealsense
             }
             LOG_DEBUG("time_diff_keeper::get_system_hw_time - unlock");
         }
-        return _coefs.calc_value(crnt_hw_time);
+        is_ready = _is_ready;
+        if (_is_ready)
+            return _coefs.calc_value(crnt_hw_time);
+        else        
+            return crnt_hw_time;
     }
 
     global_timestamp_reader::global_timestamp_reader(std::unique_ptr<frame_timestamp_reader> device_timestamp_reader, 
                                                      std::shared_ptr<time_diff_keeper> timediff,
                                                      std::shared_ptr<global_time_option> enable_option) :
         _device_timestamp_reader(std::move(device_timestamp_reader)),
         _time_diff_keeper(timediff),
-        _option_is_enabled(enable_option)
+        _option_is_enabled(enable_option),
+        _ts_is_ready(false)
     {
         LOG_DEBUG("global_timestamp_reader created");
     }
@@ -205,7 +235,7 @@ namespace librealsense
         {
             auto sp = _time_diff_keeper.lock();
             if (sp)
-                frame_time = sp->get_system_hw_time(frame_time);
+                frame_time = sp->get_system_hw_time(frame_time, _ts_is_ready);
             else
                 LOG_DEBUG("Notification: global_timestamp_reader - time_diff_keeper is being shut-down");
         }
@@ -221,11 +251,23 @@ namespace librealsense
     rs2_timestamp_domain global_timestamp_reader::get_frame_timestamp_domain(const request_mapping& mode, const platform::frame_object& fo) const
     {
         rs2_timestamp_domain ts_domain = _device_timestamp_reader->get_frame_timestamp_domain(mode, fo);
-        return (_option_is_enabled->is_true() && ts_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) ? RS2_TIMESTAMP_DOMAIN_GLOBAL_TIME : ts_domain;
+        return (_option_is_enabled->is_true() && _ts_is_ready && ts_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK) ? RS2_TIMESTAMP_DOMAIN_GLOBAL_TIME : ts_domain;
     }
 
     void global_timestamp_reader::reset()
     {
         _device_timestamp_reader->reset();
     }
+
+    global_time_interface::global_time_interface() :
+        _tf_keeper(std::make_shared<time_diff_keeper>(this, 100))
+    {}
+
+    void global_time_interface::enable_time_diff_keeper(bool is_enable)
+    {
+        if (is_enable)
+            _tf_keeper->start();
+        else
+            _tf_keeper->stop();
+    }
 }

src/global_timestamp_reader.h

@@ -4,7 +4,6 @@
 #pragma once
 
 #include "sensor.h"
-#include "device.h"
 #include "error-handling.h"
 #include <deque>
 
@@ -52,26 +51,32 @@ namespace librealsense
         mutable std::recursive_mutex _stat_mtx;
     };
 
+    class global_time_interface;
+
     class time_diff_keeper
     {
     public:
-        explicit time_diff_keeper(device* dev, const unsigned int sampling_interval_ms);
+        explicit time_diff_keeper(global_time_interface* dev, const unsigned int sampling_interval_ms);
         void start();   // must be called AFTER ALL initializations of _hw_monitor.
+        void stop();
         ~time_diff_keeper();
-        double get_system_hw_time(double crnt_hw_time);
+        double get_system_hw_time(double crnt_hw_time, bool& is_ready);
 
     private:
         bool update_diff_time();
         void polling(dispatcher::cancellable_timer cancellable_timer);
 
     private:
-        device* _device;
+        global_time_interface* _device;
         double _last_sample_hw_time;
         unsigned int _poll_intervals_ms;
+        int             _users_count;
         active_object<> _active_object;
-        mutable std::recursive_mutex _mtx;
-        mutable std::recursive_mutex _read_mtx;
+        mutable std::recursive_mutex _mtx;      // Watch the update process
+        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;
+        bool _is_ready;
     };
 
     class global_timestamp_reader : public frame_timestamp_reader
@@ -91,5 +96,21 @@ namespace librealsense
         std::weak_ptr<time_diff_keeper> _time_diff_keeper;
         mutable std::recursive_mutex _mtx;
         std::shared_ptr<global_time_option> _option_is_enabled;
+        bool _ts_is_ready;
+    };
+
+    class global_time_interface : public recordable<global_time_interface>
+    {
+    protected:
+        std::shared_ptr<time_diff_keeper> _tf_keeper;
+
+    public:
+        global_time_interface();
+        void enable_time_diff_keeper(bool is_enable);
+        virtual double get_device_time_ms() = 0; // Returns time in miliseconds.
+        virtual void create_snapshot(std::shared_ptr<global_time_interface>& snapshot) const override {}
+        virtual void enable_recording(std::function<void(const global_time_interface&)> record_action) override {}
     };
+    MAP_EXTENSION(RS2_EXTENSION_GLOBAL_TIMER, librealsense::global_time_interface);
+
 }

src/l500/l500-device.cpp

@@ -20,11 +20,10 @@ namespace librealsense
 
     l500_device::l500_device(std::shared_ptr<context> ctx,
         const platform::backend_device_group& group)
-        :device(ctx, group),
+        :device(ctx, group), global_time_interface(), 
         _depth_stream(new stream(RS2_STREAM_DEPTH)),
         _ir_stream(new stream(RS2_STREAM_INFRARED)),
-        _confidence_stream(new stream(RS2_STREAM_CONFIDENCE)),
-        _tf_keeper(std::make_shared<time_diff_keeper>(this, 100))
+        _confidence_stream(new stream(RS2_STREAM_CONFIDENCE))
     {
         _depth_device_idx = add_sensor(create_depth_device(ctx, group.uvc_devices));
         auto pid = group.uvc_devices.front().pid;
@@ -83,10 +82,6 @@ namespace librealsense
         register_info(RS2_CAMERA_INFO_DEBUG_OP_CODE, std::to_string(static_cast<int>(fw_cmd::GLD)));
         register_info(RS2_CAMERA_INFO_PHYSICAL_PORT, group.uvc_devices.front().device_path);
         register_info(RS2_CAMERA_INFO_PRODUCT_ID, pid_hex_str);
-        if (dynamic_cast<const platform::playback_backend*>(&(ctx->get_backend())) == nullptr)
-            _tf_keeper->start();
-        else
-            LOG_WARNING("playback_backend - global time_keeper unavailable.");
     }
 
     std::shared_ptr<uvc_sensor> l500_device::create_depth_device(std::shared_ptr<context> ctx,
@@ -135,8 +130,13 @@ namespace librealsense
         throw not_implemented_exception("enable_recording(...) not implemented!");
     }
 
-    double l500_device::get_device_time()
+    double l500_device::get_device_time_ms()
     {
+        if (dynamic_cast<const platform::playback_backend*>(&(get_context()->get_backend())) != nullptr)
+        {
+            throw not_implemented_exception("device time not supported for backend.");
+        }
+
         if (!_hw_monitor)
             throw wrong_api_call_sequence_exception("_hw_monitor is not initialized yet");
 

src/l500/l500-device.h

@@ -19,7 +19,7 @@
 namespace librealsense
 {
 
-    class l500_device : public virtual device, public debug_interface
+    class l500_device : public virtual device, public debug_interface, public global_time_interface
     {
     public:
         l500_device(std::shared_ptr<context> ctx,
@@ -42,7 +42,7 @@ namespace librealsense
 
         void create_snapshot(std::shared_ptr<debug_interface>& snapshot) const override;
         void enable_recording(std::function<void(const debug_interface&)> record_action) override;
-        double get_device_time();
+        double get_device_time_ms();
 
     protected:
         friend class l500_depth_sensor;
@@ -58,7 +58,6 @@ namespace librealsense
         std::shared_ptr<stream_interface> _depth_stream;
         std::shared_ptr<stream_interface> _ir_stream;
         std::shared_ptr<stream_interface> _confidence_stream;
-        std::shared_ptr<time_diff_keeper> _tf_keeper;
 
         void force_hardware_reset() const;
     };

src/rs.cpp

@@ -37,6 +37,7 @@
 #include "environment.h"
 #include "proc/temporal-filter.h"
 #include "software-device.h"
+#include "global_timestamp_reader.h"
 
 ////////////////////////
 // API implementation //
@@ -1125,6 +1126,7 @@ int rs2_is_device_extendable_to(const rs2_device* dev, rs2_extension extension,
         case RS2_EXTENSION_RECORD                : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::record_device)               != nullptr;
         case RS2_EXTENSION_PLAYBACK              : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::playback_device)             != nullptr;
         case RS2_EXTENSION_TM2                   : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::tm2_extensions)              != nullptr;
+        case RS2_EXTENSION_GLOBAL_TIMER          : return VALIDATE_INTERFACE_NO_THROW(dev->device, librealsense::global_time_interface)       != nullptr;
         default:
             return false;
     }