Added support for IMU in l500

src/context.cpp

@@ -333,7 +333,7 @@ namespace librealsense
             std::copy(begin(ds5_devices), end(ds5_devices), std::back_inserter(list));
         }
 
-        auto l500_devices = l500_info::pick_l500_devices(ctx, devices.uvc_devices, devices.usb_devices);
+        auto l500_devices = l500_info::pick_l500_devices(ctx, devices);
         std::copy(begin(l500_devices), end(l500_devices), std::back_inserter(list));
 
         if (mask & RS2_PRODUCT_LINE_SR300)

src/ds5/ds5-motion.cpp

@@ -165,7 +165,7 @@ namespace librealsense
         static const char* custom_sensor_fw_ver = "5.6.0.0";
 
         auto hid_ep = std::make_shared<ds5_hid_sensor>(this, ctx->get_backend().create_hid_device(all_hid_infos.front()),
-                                                        std::unique_ptr<frame_timestamp_reader>(new ds5_iio_hid_timestamp_reader()),
+                                                        std::unique_ptr<frame_timestamp_reader>(new iio_hid_timestamp_reader()),
                                                         std::unique_ptr<frame_timestamp_reader>(new ds5_custom_hid_timestamp_reader()),
                                                         fps_and_sampling_frequency_per_rs2_stream,
                                                         sensor_name_and_hid_profiles);

src/ds5/ds5-timestamp.cpp

@@ -15,8 +15,6 @@
 
 namespace librealsense
 {
-    static const double TIMESTAMP_USEC_TO_MSEC = 0.001;
-
     ds5_timestamp_reader_from_metadata::ds5_timestamp_reader_from_metadata(std::unique_ptr<frame_timestamp_reader> backup_timestamp_reader)
         :_backup_timestamp_reader(std::move(backup_timestamp_reader)), _has_metadata(pins), one_time_note(false)
     {
@@ -144,78 +142,6 @@ namespace librealsense
         return RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME;
     }
 
-    ds5_iio_hid_timestamp_reader::ds5_iio_hid_timestamp_reader()
-    {
-        counter.resize(sensors);
-        reset();
-    }
-
-    void ds5_iio_hid_timestamp_reader::reset()
-    {
-        std::lock_guard<std::recursive_mutex> lock(_mtx);
-        started = false;
-        for (auto i = 0; i < sensors; ++i)
-        {
-            counter[i] = 0;
-        }
-    }
-
-    rs2_time_t ds5_iio_hid_timestamp_reader::get_frame_timestamp(const request_mapping& mode, const platform::frame_object& fo)
-    {
-        std::lock_guard<std::recursive_mutex> lock(_mtx);
-
-        if(has_metadata(mode, fo.metadata, fo.metadata_size))
-        {
-            //  The timestamps conversions path comprise of:
-            // FW TS (32bit) ->    USB Phy Layer (no changes)  -> Host Driver TS (Extend to 64bit) ->  LRS read as 64 bit
-            // The flow introduces discrepancy with UVC stream which timestamps aer not extended to 64 bit by host driver both for Win and v4l backends.
-            // In order to allow for hw timestamp-based synchronization of Depth and IMU streams the latter will be trimmed to 32 bit.
-            // To revert to the extended 64 bit TS uncomment the next line instead
-            //auto timestamp = *((uint64_t*)((const uint8_t*)fo.metadata));
-            auto timestamp = *((uint32_t*)((const uint8_t*)fo.metadata));
-
-            // HID timestamps are aligned to FW Default - usec units
-            return static_cast<rs2_time_t>(timestamp * TIMESTAMP_USEC_TO_MSEC);
-        }
-
-        if (!started)
-        {
-            LOG_WARNING("HID timestamp not found! please apply HID patch.");
-            started = true;
-        }
-
-        return std::chrono::duration<rs2_time_t, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
-    }
-
-    bool ds5_iio_hid_timestamp_reader::has_metadata(const request_mapping& mode, const void * metadata, size_t metadata_size) const
-    {
-        if(metadata != nullptr && metadata_size > 0)
-        {
-            return true;
-        }
-        return false;
-    }
-
-    unsigned long long ds5_iio_hid_timestamp_reader::get_frame_counter(const request_mapping & mode, const platform::frame_object& fo) const
-    {
-        std::lock_guard<std::recursive_mutex> lock(_mtx);
-        if (nullptr == mode.pf) return 0;                   // Windows support is limited
-        int index = 0;
-        if (mode.pf->fourcc == rs_fourcc('G','Y','R','O'))
-            index = 1;
-
-        return ++counter[index];
-    }
-
-    rs2_timestamp_domain ds5_iio_hid_timestamp_reader::get_frame_timestamp_domain(const request_mapping & mode, const platform::frame_object& fo) const
-    {
-        if(has_metadata(mode ,fo.metadata, fo.metadata_size))
-        {
-            return RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK;
-        }
-        return RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME;
-    }
-
     ds5_custom_hid_timestamp_reader::ds5_custom_hid_timestamp_reader()
     {
         counter.resize(sensors);

src/ds5/ds5-timestamp.h

@@ -50,26 +50,6 @@ namespace librealsense
         rs2_timestamp_domain get_frame_timestamp_domain(const request_mapping & mode, const platform::frame_object& fo) const override;
     };
 
-    class ds5_iio_hid_timestamp_reader : public frame_timestamp_reader
-    {
-        static const int sensors = 2;
-        bool started;
-        mutable std::vector<int64_t> counter;
-        mutable std::recursive_mutex _mtx;
-    public:
-        ds5_iio_hid_timestamp_reader();
-
-        void reset() override;
-
-        rs2_time_t get_frame_timestamp(const request_mapping& mode, const platform::frame_object& fo) override;
-
-        bool has_metadata(const request_mapping& mode, const void * metadata, size_t metadata_size) const;
-
-        unsigned long long get_frame_counter(const request_mapping & mode, const platform::frame_object& fo) const override;
-
-        rs2_timestamp_domain get_frame_timestamp_domain(const request_mapping & mode, const platform::frame_object& fo) const override;
-    };
-
     class ds5_custom_hid_timestamp_reader : public frame_timestamp_reader
     {
         static const int sensors = 4; // TODO: implement frame-counter for each GPIO or

src/l500/CMakeLists.txt

@@ -5,9 +5,11 @@ target_sources(${LRS_TARGET}
         "${CMAKE_CURRENT_LIST_DIR}/l500-depth.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/l500-private.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/l500-color.cpp"
+        "${CMAKE_CURRENT_LIST_DIR}/l500-motion.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/l500-factory.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/l500-depth.h"
         "${CMAKE_CURRENT_LIST_DIR}/l500-private.h"
         "${CMAKE_CURRENT_LIST_DIR}/l500-color.h"
+        "${CMAKE_CURRENT_LIST_DIR}/l500-motion.h"
         "${CMAKE_CURRENT_LIST_DIR}/l500-factory.h"
 )

src/l500/l500-factory.cpp

@@ -13,24 +13,27 @@
 #include "metadata-parser.h"
 
 #include "l500-factory.h"
-#include "l500-private.h"
 #include "l500-depth.h"
+#include "l500-motion.h"
 #include "l500-color.h"
 
 namespace librealsense
 {
     using namespace ivcam2;
 
     // l515
-    class rs515_device : public l500_depth, public l500_color
+    class rs515_device : public l500_depth,
+        public l500_color,
+        public l500_motion
     {
     public:
         rs515_device(std::shared_ptr<context> ctx,
             const platform::backend_device_group& group,
             bool register_device_notifications)
             : device(ctx, group, register_device_notifications),
             l500_depth(ctx, group),
-            l500_color(ctx, group)
+            l500_color(ctx, group),
+            l500_motion(ctx, group)
         {}
 
         std::shared_ptr<matcher> create_matcher(const frame_holder& frame) const override;
@@ -43,7 +46,8 @@ namespace librealsense
             tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 360, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
             tags.push_back({ RS2_STREAM_INFRARED, -1, 640, 360, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
             tags.push_back({ RS2_STREAM_CONFIDENCE, -1, 640, 360, RS2_FORMAT_RAW8, 30, profile_tag::PROFILE_TAG_SUPERSET });
-
+            tags.push_back({ RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
+            tags.push_back({ RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
             return tags;
         };
     };
@@ -85,32 +89,41 @@ namespace librealsense
             return std::make_shared<rs500_device>(ctx, group, register_device_notifications);
         case L515_PID:
             return std::make_shared<rs515_device>(ctx, group, register_device_notifications);
-        default:
+       default:
             throw std::runtime_error(to_string() << "Unsupported L500 model! 0x"
                 << std::hex << std::setw(4) << std::setfill('0') << (int)pid);
         }
     }
 
     std::vector<std::shared_ptr<device_info>> l500_info::pick_l500_devices(
         std::shared_ptr<context> ctx,
-        std::vector<platform::uvc_device_info>& uvc,
-        std::vector<platform::usb_device_info>& usb)
+        platform::backend_device_group& group)
     {
         std::vector<platform::uvc_device_info> chosen;
         std::vector<std::shared_ptr<device_info>> results;
 
-        auto correct_pid = filter_by_product(uvc, { L500_PID, L515_PID });
-        auto group_devices = group_devices_by_unique_id(correct_pid);
-        for (auto& group : group_devices)
+        auto correct_pid = filter_by_product(group.uvc_devices, { L500_PID, L515_PID });
+        auto group_devices = group_devices_and_hids_by_unique_id(group_devices_by_unique_id(correct_pid), group.hid_devices);
+        for (auto& g : group_devices)
         {
-            if (!group.empty() && mi_present(group, 0))
+            if (!g.first.empty() && mi_present(g.first, 0))
             {
-                auto depth = get_mi(group, 0);
+                auto depth = get_mi(g.first, 0);
                 platform::usb_device_info hwm;
-                if (!ivcam2::try_fetch_usb_device(usb, depth, hwm))
+
+                if (!ivcam2::try_fetch_usb_device(group.usb_devices, depth, hwm))
                     LOG_WARNING("try_fetch_usb_device(...) failed.");
 
-                auto info = std::make_shared<l500_info>(ctx, group, hwm);
+
+#if defined( RS2_USE_WMF_BACKEND) || defined(RS2_USE_V4L2_BACKEND)  //currently hid backend implemented only for windows and linux.
+                if (g.second.size() < 2)
+                {
+                    LOG_WARNING("Skipping L500. " << g.second.size() << " HID devices were recognized, at least 2 required");
+                    continue;
+                }
+
+#endif
+                auto info = std::make_shared<l500_info>(ctx, g.first, hwm, g.second);
                 chosen.push_back(depth);
                 results.push_back(info);
             }
@@ -120,7 +133,7 @@ namespace librealsense
             }
         }
 
-        trim_device_list(uvc, chosen);
+        trim_device_list(group.uvc_devices, chosen);
 
         return results;
     }

src/l500/l500-factory.h

@@ -3,7 +3,6 @@
 #pragma once
 
 #include "l500-private.h"
-#include "l500-depth.h"
 
 namespace librealsense
 {
@@ -15,28 +14,29 @@ namespace librealsense
 
         l500_info(std::shared_ptr<context> ctx,
             std::vector<platform::uvc_device_info> depth,
-            platform::usb_device_info hwm)
+            platform::usb_device_info hwm,
+            std::vector<platform::hid_device_info> hid)
             : device_info(ctx),
             _depth(std::move(depth)),
-            _hwm(std::move(hwm))
+            _hwm(std::move(hwm)),
+            _hid(std::move(hid))
         {}
 
         static std::vector<std::shared_ptr<device_info>> pick_l500_devices(
             std::shared_ptr<context> ctx,
-            std::vector<platform::uvc_device_info>& platform,
-            std::vector<platform::usb_device_info>& usb);
+            platform::backend_device_group& group);
 
         platform::backend_device_group get_device_data() const override
         {
             std::vector<platform::usb_device_info> usb_devices;
             if (_hwm.id != "")
                 usb_devices = { _hwm };
-            return platform::backend_device_group({ _depth }, usb_devices);
+            return platform::backend_device_group({ _depth }, usb_devices, { _hid });
         }
 
     private:
         std::vector<platform::uvc_device_info> _depth;
-
         platform::usb_device_info _hwm;
+        std::vector<platform::hid_device_info> _hid;
     };
 }