Merge pull request #2788 from abernste/windows_hid_support

D435i windows IMU support
IntelRealSense · Nov 28, 2018 · bffc18e · bffc18e
2 parents 46e6aa8 + da040ac
commit bffc18e
Show file tree

Hide file tree

Showing 15 changed files with 526 additions and 137 deletions.
diff --git a/src/backend.h b/src/backend.h
@@ -287,6 +287,7 @@ namespace librealsense
             std::string pid;
             std::string unique_id;
             std::string device_path;
+            std::string serial_number;
 
             operator std::string()
             {
@@ -384,6 +385,7 @@ namespace librealsense
             value
         };
 
+#pragma pack(push, 1)
         struct hid_sensor_data
         {
             short x;
@@ -395,6 +397,7 @@ namespace librealsense
             uint32_t ts_low;
             uint32_t ts_high;
         };
+#pragma pack(pop)
 
         typedef std::function<void(const sensor_data&)> hid_callback;
 
@@ -668,6 +671,12 @@ namespace librealsense
             virtual std::shared_ptr<time_service> create_time_service() const = 0;
 
             virtual std::shared_ptr<device_watcher> create_device_watcher() const = 0;
+
+            virtual std::string get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid) const
+            {
+                std::string empty_str;
+                return empty_str;
+            }
 
             virtual ~backend() = default;
         };

diff --git a/src/context.cpp b/src/context.cpp
@@ -484,18 +484,33 @@ namespace librealsense
     }
 
     std::vector<std::pair<std::vector<platform::uvc_device_info>, std::vector<platform::hid_device_info>>> group_devices_and_hids_by_unique_id(
+        std::shared_ptr<context> ctx,
         const std::vector<std::vector<platform::uvc_device_info>>& devices,
         const std::vector<platform::hid_device_info>& hids)
     {
         std::vector<std::pair<std::vector<platform::uvc_device_info>, std::vector<platform::hid_device_info>>> results;
+        uint16_t vid;
+        uint16_t pid;
+
         for (auto&& dev : devices)
         {
             std::vector<platform::hid_device_info> hid_group;
             auto unique_id = dev.front().unique_id;
             for (auto&& hid : hids)
             {
-                if (hid.unique_id == unique_id || hid.unique_id == "*")
-                    hid_group.push_back(hid);
+                if (hid.unique_id != "")
+                {
+                    std::stringstream(hid.vid) >> std::hex >> vid;
+                    std::stringstream(hid.pid) >> std::hex >> pid;
+                    auto&& backend = ctx->get_backend();
+                    auto device_serial = backend.get_device_serial(vid, pid, unique_id);
+
+                    if ((hid.unique_id == unique_id) || // Linux check
+                        ((hid.unique_id == "*") && (hid.serial_number == device_serial))) // Windows check
+                    {
+                        hid_group.push_back(hid);
+                    }
+                }
             }
             results.push_back(std::make_pair(dev, hid_group));
         }

diff --git a/src/context.h b/src/context.h
@@ -176,6 +176,7 @@ namespace librealsense
     // Helper functions for device list manipulation:
     std::vector<platform::uvc_device_info> filter_by_product(const std::vector<platform::uvc_device_info>& devices, const std::set<uint16_t>& pid_list);
     std::vector<std::pair<std::vector<platform::uvc_device_info>, std::vector<platform::hid_device_info>>> group_devices_and_hids_by_unique_id(
+        std::shared_ptr<context> ctx,
         const std::vector<std::vector<platform::uvc_device_info>>& devices,
         const std::vector<platform::hid_device_info>& hids);
     std::vector<std::vector<platform::uvc_device_info>> group_devices_by_unique_id(const std::vector<platform::uvc_device_info>& devices);

diff --git a/src/ds5/ds5-factory.cpp b/src/ds5/ds5-factory.cpp
@@ -430,7 +430,6 @@ namespace librealsense
             tags.push_back({ RS2_STREAM_COLOR, -1, width, height, RS2_FORMAT_RGB8, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
             tags.push_back({ RS2_STREAM_DEPTH, -1, width, height, RS2_FORMAT_Z16, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
             tags.push_back({ RS2_STREAM_INFRARED, -1, width, height, RS2_FORMAT_Y8, fps, 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, 63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
 
@@ -516,7 +515,7 @@ namespace librealsense
         std::vector<std::shared_ptr<device_info>> results;
 
         auto valid_pid = filter_by_product(group.uvc_devices, ds::rs400_sku_pid);
-        auto group_devices = group_devices_and_hids_by_unique_id(group_devices_by_unique_id(valid_pid), group.hid_devices);
+        auto group_devices = group_devices_and_hids_by_unique_id(ctx, group_devices_by_unique_id(valid_pid), group.hid_devices);
 
         for (auto& g : group_devices)
         {

diff --git a/src/ds5/ds5-motion.h b/src/ds5/ds5-motion.h
@@ -43,26 +43,40 @@ namespace librealsense
 
         lazy<std::vector<uint8_t>>      _fisheye_calibration_table_raw;
 
-        // Bandwidth parameters from BOSCH BMI 055 spec. used by D435i
+#ifdef _WIN32
+        // Bandwidth parameters from BOSCH BMI 055 spec'
         std::vector<std::pair<std::string, stream_profile>> sensor_name_and_hid_profiles =
-            {{"gyro_3d",  {RS2_STREAM_GYRO,  0, 1, 1, 200,  RS2_FORMAT_MOTION_RAW}},
-             {"gyro_3d",  {RS2_STREAM_GYRO,  0, 1, 1, 400,  RS2_FORMAT_MOTION_RAW}},
-             {"gyro_3d",  {RS2_STREAM_GYRO,  0, 1, 1, 200,  RS2_FORMAT_MOTION_XYZ32F}},
-             {"gyro_3d",  {RS2_STREAM_GYRO,  0, 1, 1, 400,  RS2_FORMAT_MOTION_XYZ32F}},
-             {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 63,  RS2_FORMAT_MOTION_RAW}},
-             {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250,  RS2_FORMAT_MOTION_RAW}},
-             {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 63,  RS2_FORMAT_MOTION_XYZ32F}},
-             {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250,  RS2_FORMAT_MOTION_XYZ32F}},
-             {"HID Sensor Class Device: Gyroscope",     { RS2_STREAM_GYRO,  0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F}},
-             {"HID Sensor Class Device: Accelerometer", { RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F}},
-             {"HID Sensor Class Device: Custom",        { RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F}}};
+        {{ "HID Sensor Class Device: Gyroscope",     {RS2_STREAM_GYRO,  0, 1, 1, 200, RS2_FORMAT_MOTION_XYZ32F}},
+         { "HID Sensor Class Device: Gyroscope",     {RS2_STREAM_GYRO,  0, 1, 1, 400, RS2_FORMAT_MOTION_XYZ32F}},
+         { "HID Sensor Class Device: Accelerometer", {RS2_STREAM_ACCEL, 0, 1, 1, 63, RS2_FORMAT_MOTION_XYZ32F}},
+         { "HID Sensor Class Device: Accelerometer", {RS2_STREAM_ACCEL, 0, 1, 1, 250, RS2_FORMAT_MOTION_XYZ32F}}};
+
+        // Translate frequency to SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL
+        std::map<rs2_stream, std::map<unsigned, unsigned>> fps_and_sampling_frequency_per_rs2_stream =
+                                                           {{RS2_STREAM_ACCEL,{{63,   1000},
+                                                                               {250,  400}}},
+                                                            {RS2_STREAM_GYRO, {{200,  500},
+                                                                               {400,  250}}}};
+
+#else                                                                  
+        // Bandwidth parameters from BOSCH BMI 055 spec'
+        std::vector<std::pair<std::string, stream_profile>> sensor_name_and_hid_profiles =
+        {{"gyro_3d",  {RS2_STREAM_GYRO,  0, 1, 1, 200,  RS2_FORMAT_MOTION_RAW}},
+         {"gyro_3d",  {RS2_STREAM_GYRO,  0, 1, 1, 400,  RS2_FORMAT_MOTION_RAW}},
+         {"gyro_3d",  {RS2_STREAM_GYRO,  0, 1, 1, 200,  RS2_FORMAT_MOTION_XYZ32F}},
+         {"gyro_3d",  {RS2_STREAM_GYRO,  0, 1, 1, 400,  RS2_FORMAT_MOTION_XYZ32F}},
+         {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 63,  RS2_FORMAT_MOTION_RAW}},
+         {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250,  RS2_FORMAT_MOTION_RAW}},
+         {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 63,  RS2_FORMAT_MOTION_XYZ32F}},
+         {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250,  RS2_FORMAT_MOTION_XYZ32F}}};
 
         // The frequency selector is vendor and model-specific
         std::map<rs2_stream, std::map<unsigned, unsigned>> fps_and_sampling_frequency_per_rs2_stream =
                                                          {{RS2_STREAM_ACCEL, {{63,   1},
                                                                               {250,  3}}},
                                                           {RS2_STREAM_GYRO,  {{200,  2},
                                                                               {400,  4}}}};
+#endif
 
     protected:
         std::shared_ptr<stream_interface> _fisheye_stream;

diff --git a/src/ds5/ds5-private.h b/src/ds5/ds5-private.h
@@ -528,10 +528,10 @@ namespace librealsense
             { hot_laser_power_reduce,       "Laser hot - power reduce" },
             { hot_laser_disable,            "Laser hot - disabled" },
             { flag_B_laser_disable,         "Flag B - laser disabled" },
-            { stereo_module_not_connected,  "Stered Module is not connected" },
+            { stereo_module_not_connected,  "Stereo Module is not connected" },
             { eeprom_corrupted,             "EEPROM corrupted" },
             { calibration_corrupted,        "Calibration corrupted" },
-            { mm_upd_fail,                  "Moton Module update failed" },
+            { mm_upd_fail,                  "Motion Module update failed" },
             { isp_upd_fail,                 "ISP update failed" },
             { mm_force_pause,               "Motion Module force pause" },
             { mm_failure,                   "Motion Module failure" },

diff --git a/src/ds5/ds5-timestamp.cpp b/src/ds5/ds5-timestamp.cpp
@@ -166,7 +166,7 @@ namespace librealsense
         {
             auto timestamp = *((uint64_t*)((const uint8_t*)fo.metadata));
             // The FW timestamps for HID are converted to Nanosec in Linux kernel. This may produce conflicts with MS API.
-            return static_cast<rs2_time_t>(timestamp) * TIMESTAMP_NSEC_TO_MSEC;
+            return static_cast<rs2_time_t>(timestamp) * HID_TIMESTAMP_MULTIPLIER;
         }
 
         if (!started)

diff --git a/src/types.h b/src/types.h
@@ -50,6 +50,14 @@ namespace librealsense
     const double TIMESTAMP_USEC_TO_MSEC = 0.001;
     const double TIMESTAMP_NSEC_TO_MSEC = 0.000001;
 
+#ifdef _WIN32
+/* The FW timestamps for HID are converted to Usec in Windows kernel */
+#define HID_TIMESTAMP_MULTIPLIER TIMESTAMP_USEC_TO_MSEC
+#else
+/* The FW timestamps for HID are converted to Nanosec in Linux kernel */
+#define HID_TIMESTAMP_MULTIPLIER TIMESTAMP_NSEC_TO_MSEC
+#endif // define HID_TIMESTAMP_MULTIPLIER
+
     ///////////////////////////////////
     // Utility types for general use //
     ///////////////////////////////////
@@ -484,7 +492,8 @@ namespace librealsense
             (a.height == b.height) &&
             (a.fps == b.fps) &&
             (a.format == b.format) &&
-            (a.index == b.index);
+            (a.index == b.index) &&
+            (a.stream == b.stream);
     }
 
     struct stream_descriptor

diff --git a/src/win/win-backend.cpp b/src/win/win-backend.cpp
@@ -23,7 +23,7 @@ namespace librealsense
     {
         wmf_backend::wmf_backend()
         {
-            CoInitializeEx(nullptr, COINIT_APARTMENTTHREADED);
+            CoInitializeEx(nullptr, COINIT_MULTITHREADED); // when using COINIT_APARTMENTTHREADED, calling _pISensor->SetEventSink(NULL) to stop sensor can take several seconds
             MFStartup(MF_VERSION, MFSTARTUP_NOSOCKET);
         }
 
@@ -83,7 +83,7 @@ namespace librealsense
                 {
                     std::string path(id.begin(), id.end());
                     uint16_t vid, pid, mi; std::string unique_id;
-                    if (!parse_usb_path(vid, pid, mi, unique_id, path)) continue;
+                    if (!parse_usb_path_multiple_interface(vid, pid, mi, unique_id, path)) continue;
 
                     usb_device_info info{ path, vid, pid, mi, unique_id, usb_undefined };
 
@@ -94,22 +94,27 @@ namespace librealsense
             return result;
         }
 
-        std::shared_ptr<hid_device> wmf_backend::create_hid_device(hid_device_info info) const
+        wmf_hid_device::wmf_hid_device(const hid_device_info& info)
         {
-            std::shared_ptr<hid_device> result = nullptr;
+            bool found = false;
 
-            auto action = [&result, &info](const hid_device_info& i, CComPtr<ISensor> ptr)
-            {
-                if (info.device_path == i.device_path)
+            wmf_hid_device::foreach_hid_device([&](const hid_device_info& hid_dev_info, CComPtr<ISensor> sensor) {
+                if (hid_dev_info.unique_id == info.unique_id)
                 {
-                    result = std::make_shared<wmf_hid_device>(ptr);
+                    _connected_sensors.push_back(std::make_shared<wmf_hid_sensor>(hid_dev_info, sensor));
+                    found = true;
                 }
-            };
+            });
 
-            wmf_hid_device::foreach_hid_device(action);
+            if (!found)
+            {
+                LOG_ERROR("hid device is no longer connected!");
+            }
+        }
 
-            if (result.get()) return result;
-            throw std::runtime_error("Device no longer found!");
+        std::shared_ptr<hid_device> wmf_backend::create_hid_device(hid_device_info info) const
+        {
+            return std::make_shared<wmf_hid_device>(info);
         }
 
         std::vector<hid_device_info> wmf_backend::query_hid_devices() const
@@ -335,6 +340,13 @@ namespace librealsense
         {
             return std::make_shared<win_event_device_watcher>(this);
         }
+
+        std::string wmf_backend::get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid) const
+        {
+            std::string device_serial = "";
+            platform::get_device_serial(device_vid, device_pid, device_uid, device_serial);
+            return device_serial;
+        }
     }
 }
 

diff --git a/src/win/win-backend.h b/src/win/win-backend.h
@@ -25,6 +25,8 @@ namespace librealsense
             std::vector<hid_device_info> query_hid_devices() const override;
             virtual std::shared_ptr<time_service> create_time_service() const override;
             std::shared_ptr<device_watcher> create_device_watcher() const override;
+            std::string get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid) const override;
+
         private:
             std::chrono::high_resolution_clock::time_point _start_time;
         };