From 95f8b28eaca1ba529cfc75e55f85e374805c8569 Mon Sep 17 00:00:00 2001 From: abernste Date: Tue, 6 Nov 2018 13:47:39 +0200 Subject: [PATCH 1/9] Add HID support for D435i --- src/ds5/ds5-motion.cpp | 2 +- src/ds5/ds5-motion.h | 42 +++++++++------- src/ds5/ds5-private.h | 4 +- src/win/win-hid.cpp | 106 ++++++++++++----------------------------- 4 files changed, 58 insertions(+), 96 deletions(-) diff --git a/src/ds5/ds5-motion.cpp b/src/ds5/ds5-motion.cpp index fcc44fe801..401eac3074 100644 --- a/src/ds5/ds5-motion.cpp +++ b/src/ds5/ds5-motion.cpp @@ -189,7 +189,7 @@ namespace librealsense std::copy(custom_sensor_profiles.begin(), custom_sensor_profiles.end(), std::back_inserter(sensor_name_and_hid_profiles)); } - auto hid_ep = std::make_shared(this, ctx->get_backend().create_hid_device(all_hid_infos.front()), + auto hid_ep = std::make_shared(this, ctx->get_backend().create_hid_device(all_hid_infos[1]), std::unique_ptr(new ds5_iio_hid_timestamp_reader()), std::unique_ptr(new ds5_custom_hid_timestamp_reader()), fps_and_sampling_frequency_per_rs2_stream, diff --git a/src/ds5/ds5-motion.h b/src/ds5/ds5-motion.h index 89d8247ada..5d15210318 100644 --- a/src/ds5/ds5-motion.h +++ b/src/ds5/ds5-motion.h @@ -44,25 +44,33 @@ namespace librealsense lazy> _fisheye_calibration_table_raw; //std::shared_ptr> _depth_to_fisheye; + +#ifdef _WIN32 + // Bandwidth parameters from BOSCH BMI 055 spec' + std::vector> sensor_name_and_hid_profiles = + { { "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 } } }; +#else // Bandwidth parameters from BOSCH BMI 055 spec' std::vector> 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, 1000, 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}}, - {"gyro_3d", {RS2_STREAM_GYRO, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F}}, - {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 125, 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, 500, RS2_FORMAT_MOTION_RAW}}, - {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_RAW}}, - {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 125, RS2_FORMAT_MOTION_XYZ32F}}, - {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250, RS2_FORMAT_MOTION_XYZ32F}}, - {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 500, RS2_FORMAT_MOTION_XYZ32F}}, - {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 1000, 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}}}; + { { "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, 1000, 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 } }, + { "gyro_3d",{ RS2_STREAM_GYRO, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F } }, + { "accel_3d",{ RS2_STREAM_ACCEL, 0, 1, 1, 125, 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, 500, RS2_FORMAT_MOTION_RAW } }, + { "accel_3d",{ RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_RAW } }, + { "accel_3d",{ RS2_STREAM_ACCEL, 0, 1, 1, 125, RS2_FORMAT_MOTION_XYZ32F } }, + { "accel_3d",{ RS2_STREAM_ACCEL, 0, 1, 1, 250, RS2_FORMAT_MOTION_XYZ32F } }, + { "accel_3d",{ RS2_STREAM_ACCEL, 0, 1, 1, 500, RS2_FORMAT_MOTION_XYZ32F } }, + { "accel_3d",{ RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F } } }; +#endif + + std::map> fps_and_sampling_frequency_per_rs2_stream = {{RS2_STREAM_ACCEL, {{125, 1}, diff --git a/src/ds5/ds5-private.h b/src/ds5/ds5-private.h index 366f1e8f6a..de175aa5ae 100644 --- 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/win/win-hid.cpp b/src/win/win-hid.cpp index 7132923420..236b7a07ba 100644 --- a/src/win/win-hid.cpp +++ b/src/win/win-hid.cpp @@ -138,7 +138,7 @@ namespace librealsense data.z = rawZ; data.ts_low = customTimestampLow; data.ts_high = customTimestampHigh; - d.sensor.name = ""; + d.sensor.name = "HID Sensor Class Device: Gyroscope"; d.fo.pixels = &data; d.fo.metadata = NULL; @@ -153,7 +153,7 @@ namespace librealsense { HRESULT hr = S_OK; - // Peform any housekeeping tasks for the sensor that is leaving. + // Perform any housekeeping tasks for the sensor that is leaving. // For example, if you have maintained a reference to the sensor, // release it now and set the pointer to NULL. @@ -238,65 +238,68 @@ namespace librealsense void wmf_hid_device::foreach_hid_device(std::function)> action) { - return; // HID devices aren't supported on Windows OS + //return; + /* Enumerate all HID devices and run action function on each device */ try { CComPtr pSensorManager = nullptr; - CComPtr pSensorColl = nullptr; + CComPtr pSensorCollection = nullptr; CComPtr pSensor = nullptr; - ULONG uCount{}; + ULONG sensorCount = 0; HRESULT res{}; - CComPtr ppDataReport; + CHECK_HR(CoCreateInstance(CLSID_SensorManager, NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&pSensorManager))); - CHECK_HR(CoCreateInstance(CLSID_SensorManager, NULL, - CLSCTX_INPROC_SERVER, - IID_PPV_ARGS(&pSensorManager))); - - LOG_HR(res=pSensorManager->GetSensorsByCategory(SENSOR_CATEGORY_ALL, &pSensorColl)); + /* Retrieves a collection containing all sensors associated with category SENSOR_CATEGORY_ALL */ + LOG_HR(res=pSensorManager->GetSensorsByCategory(SENSOR_CATEGORY_ALL, &pSensorCollection)); if (SUCCEEDED(res)) { - CHECK_HR(pSensorColl->GetCount(&uCount)); + /* Retrieves the count of sensors in the collection */ + CHECK_HR(pSensorCollection->GetCount(&sensorCount)); - for (ULONG i = 0; i < uCount; i++) + for (ULONG i = 0; i < sensorCount; i++) { - if (SUCCEEDED(pSensorColl->GetAt(i, &pSensor.p))) + /* Retrieves the sensor at the specified index in the collection */ + if (SUCCEEDED(pSensorCollection->GetAt(i, &pSensor.p))) { + /* Retrieve SENSOR_PROPERTY_FRIENDLY_NAME which is the sensor name that is intended to be seen by the user */ BSTR fName{}; LOG_HR(res = pSensor->GetFriendlyName(&fName)); if (FAILED(res)) fName= L"Unidentified HID sensor"; + /* Retrieve SENSOR_PROPERTY_PERSISTENT_UNIQUE_ID which is a GUID that uniquely identifies the sensor on the current computer */ SENSOR_ID id{}; CHECK_HR(pSensor->GetID(&id)); + /* Retrieve sensor type - Sensor types are more specific groupings than sensor categories. Sensor type IDs are GUIDs that are defined in Sensors.h */ SENSOR_TYPE_ID type{}; CHECK_HR(pSensor->GetType(&type)); CComPtr pValues = nullptr; // Output - hid_device_info info{}; + /* Retrieves multiple sensor properties */ auto hr = pSensor->GetProperties(nullptr, &pValues); - DWORD cVals = 0; // Count of returned properties. if (SUCCEEDED(hr)) { - // Get the number of values returned. - hr = pValues->GetCount(&cVals); + /* Get the number of property returned */ + DWORD propertyCount = 0; + hr = pValues->GetCount(&propertyCount); if (SUCCEEDED(hr)) { - PROPERTYKEY pk; // Keys - PROPVARIANT pv = {}; // Values + PROPERTYKEY propertyKey; + PROPVARIANT propertyValue = {}; - // Loop through the values - for (DWORD j = 0; j < cVals; j++) + /* Loop through the properties */ + for (DWORD properyIndex = 0; properyIndex < propertyCount; properyIndex++) { // Get the value at the current index. - hr = pValues->GetAt(j, &pk, &pv); + hr = pValues->GetAt(properyIndex, &propertyKey, &propertyValue); if (SUCCEEDED(hr)) { - if (IsEqualPropertyKey(pk, SENSOR_PROPERTY_DEVICE_PATH)) + if (IsEqualPropertyKey(propertyKey, SENSOR_PROPERTY_DEVICE_PATH)) { - info.device_path = std::string(pv.pwszVal, pv.pwszVal + wcslen(pv.pwszVal)); + info.device_path = std::string(propertyValue.pwszVal, propertyValue.pwszVal + wcslen(propertyValue.pwszVal)); info.id = std::string(fName, fName + wcslen(fName)); uint16_t vid, pid, mi; @@ -308,64 +311,15 @@ namespace librealsense info.vid = to_string() << std::hex << vid; } } - - //if (IsEqualPropertyKey(pk, SENSOR_PROPERTY_MODEL)) - //{ - // info.pid = std::string(pv.pwszVal, pv.pwszVal + wcslen(pv.pwszVal)); - //} } - PropVariantClear(&pv); + + PropVariantClear(&propertyValue); } } } action(info, pSensor); - //if (wcsstr(fName, L"Accelerometer") != NULL) - //if (wcsstr(fName, L"Gyroscope") != NULL) - //{ - // auto callback = new SensorEvents(); - // ISensorEvents* sensorEvents; - // HRESULT hr = callback->QueryInterface(IID_PPV_ARGS(&sensorEvents)); - // hr = pSensor->SetEventSink(sensorEvents); - // //for (int i = 0; i < 10000; i++) - // //{ - // // hr = pSensor->GetData(&ppDataReport); - // // if (ppDataReport != NULL) - // // { - // // SYSTEMTIME time; - // // ppDataReport->GetTimestamp(&time); - - // // printf("%d.%d.%d %d:%d:%d.%d\n", time.wDay, time.wMonth, time.wYear, time.wHour, time.wMinute, time.wSecond, time.wMilliseconds); - - // // IPortableDeviceValues* values; - // // ppDataReport->GetSensorValues(NULL, &values); - - // // DWORD valuesCount = 0; - // // hr = values->GetCount(&valuesCount); - // // if (SUCCEEDED(hr)) - // // { - // // PROPERTYKEY pk; // Keys - // // PROPVARIANT pv = {}; // Values - - // // for (DWORD i = 0; i < valuesCount; i++) - // // { - // // // Get the value at the current index. - // // hr = values->GetAt(i, &pk, &pv); - // // if (SUCCEEDED(hr)) - // // { - // // if (IsEqualPropertyKey(pk, SENSOR_DATA_TYPE_CUSTOM_USAGE)) - // // { - // // wprintf_s(L"\Acceleration X: %lu\n", pv.ulVal); - // // } - // // } - // // } - // // } - // // //SENSOR_DATA_TYPE_ACCELERATION_X_G - // // } - // //} - // //printf("\n"); - //} SysFreeString(fName); } } From d4ca87f0bda786f5002c503c4877dc4e9b7a7e83 Mon Sep 17 00:00:00 2001 From: abernste Date: Wed, 21 Nov 2018 11:03:45 +0200 Subject: [PATCH 2/9] D435i IMU support --- src/backend.h | 1 + src/context.cpp | 16 ++- src/ds5/ds5-factory.cpp | 4 +- src/ds5/ds5-motion.h | 2 +- src/types.h | 3 +- src/win/win-backend.cpp | 29 +++-- src/win/win-helpers.cpp | 277 +++++++++++++++++++++++++++++++++++++++- src/win/win-helpers.h | 8 +- src/win/win-hid.cpp | 57 ++++++++- src/win/win-hid.h | 51 +++++++- src/win/win-uvc.cpp | 2 +- 11 files changed, 410 insertions(+), 40 deletions(-) diff --git a/src/backend.h b/src/backend.h index 2c0388a9b0..80c788569f 100644 --- 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() { diff --git a/src/context.cpp b/src/context.cpp index b739f36394..645c03b15a 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -19,6 +19,7 @@ #include "stream.h" #include "environment.h" #include "context.h" +#include "win/win-helpers.h" #ifdef WITH_TRACKING #include "tm2/tm-info.h" @@ -494,8 +495,19 @@ namespace librealsense 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::string device_serial; + uint16_t vid = std::stoi(hid.vid, nullptr, 16); + uint16_t pid = std::stoi(hid.pid, nullptr, 16); + + platform::get_device_serial(vid, pid, unique_id, device_serial); + + if ((hid.unique_id == unique_id) || ((hid.unique_id == "*") && (hid.serial_number == device_serial))) + { + hid_group.push_back(hid); + } + } } results.push_back(std::make_pair(dev, hid_group)); } diff --git a/src/ds5/ds5-factory.cpp b/src/ds5/ds5-factory.cpp index dcdc56a01e..aba08bc8ae 100644 --- a/src/ds5/ds5-factory.cpp +++ b/src/ds5/ds5-factory.cpp @@ -432,8 +432,8 @@ namespace librealsense tags.push_back({ RS2_STREAM_INFRARED, -1, width, height, RS2_FORMAT_Y8, fps, profile_tag::PROFILE_TAG_SUPERSET }); // TODO - Check which IMU shall be activated by default - //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, 125, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 1000, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 1000, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); return tags; }; diff --git a/src/ds5/ds5-motion.h b/src/ds5/ds5-motion.h index 9c16eb2758..3f5c56e063 100644 --- a/src/ds5/ds5-motion.h +++ b/src/ds5/ds5-motion.h @@ -48,7 +48,7 @@ namespace librealsense std::vector> sensor_name_and_hid_profiles = { { "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: Custom", { RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F } }*/ }; #else // Bandwidth parameters from BOSCH BMI 055 spec' std::vector> sensor_name_and_hid_profiles = diff --git a/src/types.h b/src/types.h index 1289bf1929..a7a5b01970 100644 --- a/src/types.h +++ b/src/types.h @@ -484,7 +484,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 index 8d00a3612b..96cba50e35 100644 --- 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 takes too much time 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 wmf_backend::create_hid_device(hid_device_info info) const + wmf_hid_device::wmf_hid_device(const hid_device_info& info) { - std::shared_ptr result = nullptr; + bool found = false; - auto action = [&result, &info](const hid_device_info& i, CComPtr ptr) - { - if (info.device_path == i.device_path) + wmf_hid_device::foreach_hid_device([&](const hid_device_info& hid_dev_info, CComPtr sensor) { + if (hid_dev_info.unique_id == info.unique_id) { - result = std::make_shared(ptr); + _connected_sensors.push_back(std::make_shared(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 wmf_backend::create_hid_device(hid_device_info info) const + { + return std::make_shared(info); } std::vector wmf_backend::query_hid_devices() const diff --git a/src/win/win-helpers.cpp b/src/win/win-helpers.cpp index 7e979a9ab5..84491b248b 100644 --- a/src/win/win-helpers.cpp +++ b/src/win/win-helpers.cpp @@ -113,7 +113,14 @@ namespace librealsense } } - bool parse_usb_path(uint16_t & vid, uint16_t & pid, uint16_t & mi, std::string & unique_id, const std::string & path) + + // Parse the following USB path format = \?usb#vid_vvvv&pid_pppp&mi_ii#aaaaaaaaaaaaaaaa#{gggggggg-gggg-gggg-gggg-gggggggggggg} + // vvvv = USB vendor ID represented in 4 hexadecimal characters. + // pppp = USB product ID represented in 4 hexadecimal characters. + // ii = USB interface number. + // aaaaaaaaaaaaaaaa = unique Windows-generated string based on things such as the physical USB port address and/or interface number. + // gggggggg-gggg-gggg-gggg-gggggggggggg = device interface GUID assigned in the driver or driver INF file and is used to link applications to device with specific drivers loaded. + bool parse_usb_path_multiple_interface(uint16_t & vid, uint16_t & pid, uint16_t & mi, std::string & unique_id, const std::string & path) { auto name = path; std::transform(begin(name), end(name), begin(name), ::tolower); @@ -159,6 +166,41 @@ namespace librealsense return true; } + // Parse the following USB path format = \?usb#vid_vvvv&pid_pppp#ssss#{gggggggg-gggg-gggg-gggg-gggggggggggg} + // vvvv = USB vendor ID represented in 4 hexadecimal characters. + // pppp = USB product ID represented in 4 hexadecimal characters. + // ssss = USB serial string represented in n characters. + // gggggggg-gggg-gggg-gggg-gggggggggggg = device interface GUID assigned in the driver or driver INF file and is used to link applications to device with specific drivers loaded. + bool parse_usb_path_single_interface(uint16_t & vid, uint16_t & pid, std::string & serial, const std::string & path) + { + auto name = path; + std::transform(begin(name), end(name), begin(name), ::tolower); + auto tokens = tokenize(name, '#'); + if (tokens.size() < 1 || (tokens[0] != R"(\\?\usb)" && tokens[0] != R"(\\?\hid)")) return false; // Not a USB device + if (tokens.size() < 3) + { + LOG_ERROR("malformed usb device path: " << name); + return false; + } + + auto ids = tokenize(tokens[1], '&'); + if (ids[0].size() != 8 || ids[0].substr(0, 4) != "vid_" || !(std::istringstream(ids[0].substr(4, 4)) >> std::hex >> vid)) + { + LOG_ERROR("malformed vid string: " << tokens[1]); + return false; + } + + if (ids[1].size() != 8 || ids[1].substr(0, 4) != "pid_" || !(std::istringstream(ids[1].substr(4, 4)) >> std::hex >> pid)) + { + LOG_ERROR("malformed pid string: " << tokens[1]); + return false; + } + + serial = tokens[2]; + + return true; + } + bool parse_usb_path_from_device_id(uint16_t & vid, uint16_t & pid, uint16_t & mi, std::string & unique_id, const std::string & device_id) { auto name = device_id; @@ -311,9 +353,238 @@ namespace librealsense return res; } + bool get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid, std::string& serial) + { + SP_DEVINFO_DATA devInfo = { sizeof(SP_DEVINFO_DATA) }; + std::vector guids = { GUID_DEVINTERFACE_IMAGE, GUID_DEVINTERFACE_CAMERA }; + + for (auto guid : guids) + { + // build a device info represent all imaging devices. + HDEVINFO device_info = SetupDiGetClassDevsEx(static_cast(&guid), + nullptr, + nullptr, + DIGCF_PRESENT, + nullptr, + nullptr, + nullptr); + if (device_info == INVALID_HANDLE_VALUE) + return false; + + auto di = std::shared_ptr(device_info, SetupDiDestroyDeviceInfoList); + + // enumerate all imaging devices. + for (int member_index = 0; ; ++member_index) + { + SP_DEVICE_INTERFACE_DATA interfaceData = { sizeof(SP_DEVICE_INTERFACE_DATA) }; + unsigned long buf_size = 0; + + if (SetupDiEnumDeviceInfo(device_info, member_index, &devInfo) == FALSE) + { + if (GetLastError() == ERROR_NO_MORE_ITEMS) break; // stop when none left + continue; // silently ignore other errors + } + + // get the device ID of current device. + if (CM_Get_Device_ID_Size(&buf_size, devInfo.DevInst, 0) != CR_SUCCESS) + { + LOG_ERROR("CM_Get_Device_ID_Size failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + auto alloc = std::malloc(buf_size * sizeof(WCHAR) + sizeof(WCHAR)); + if (!alloc) + { + LOG_ERROR("malloc call failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + auto pInstID = std::shared_ptr(reinterpret_cast(alloc), std::free); + if (CM_Get_Device_ID(devInfo.DevInst, pInstID.get(), buf_size * sizeof(WCHAR) + sizeof(WCHAR), 0) != CR_SUCCESS) + { + LOG_ERROR("CM_Get_Device_ID failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + if (pInstID == nullptr) continue; + + // Check if this is our device + uint16_t usb_vid, usb_pid, usb_mi; std::string usb_unique_id; + if (!parse_usb_path_from_device_id(usb_vid, usb_pid, usb_mi, usb_unique_id, std::string(win_to_utf(pInstID.get())))) continue; + if (usb_vid != device_vid || usb_pid != device_pid || /* usb_mi != device->mi || */ usb_unique_id != device_uid) continue; + + // get parent (composite device) instance + DEVINST instance; + if (CM_Get_Parent(&instance, devInfo.DevInst, 0) != CR_SUCCESS) + { + LOG_ERROR("CM_Get_Parent failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + // get composite device instance id + if (CM_Get_Device_ID_Size(&buf_size, instance, 0) != CR_SUCCESS) + { + LOG_ERROR("CM_Get_Device_ID_Size failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + alloc = std::malloc(buf_size * sizeof(WCHAR) + sizeof(WCHAR)); + if (!alloc) + { + LOG_ERROR("malloc fail"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + pInstID = std::shared_ptr(reinterpret_cast(alloc), std::free); + if (CM_Get_Device_ID(instance, pInstID.get(), buf_size * sizeof(WCHAR) + sizeof(WCHAR), 0) != CR_SUCCESS) { + LOG_ERROR("CM_Get_Device_ID failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + // upgrade to DEVINFO_DATA for SetupDiGetDeviceRegistryProperty + device_info = SetupDiGetClassDevs(nullptr, pInstID.get(), nullptr, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE | DIGCF_ALLCLASSES); + if (device_info == INVALID_HANDLE_VALUE) { + LOG_ERROR("SetupDiGetClassDevs failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + interfaceData = { sizeof(SP_DEVICE_INTERFACE_DATA) }; + if (SetupDiEnumDeviceInterfaces(device_info, nullptr, &GUID_DEVINTERFACE_USB_DEVICE, 0, &interfaceData) == FALSE) + { + LOG_ERROR("SetupDiEnumDeviceInterfaces failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + // get the SP_DEVICE_INTERFACE_DETAIL_DATA object, and also grab the SP_DEVINFO_DATA object for the device + buf_size = 0; + SetupDiGetDeviceInterfaceDetail(device_info, &interfaceData, nullptr, 0, &buf_size, nullptr); + if (GetLastError() != ERROR_INSUFFICIENT_BUFFER) + { + LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + alloc = std::malloc(buf_size); + if (!alloc) + { + LOG_ERROR("malloc fail"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + auto detail_data = std::shared_ptr(reinterpret_cast(alloc), std::free); + detail_data->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA); + SP_DEVINFO_DATA parent_data = { sizeof(SP_DEVINFO_DATA) }; + if (!SetupDiGetDeviceInterfaceDetail(device_info, &interfaceData, detail_data.get(), buf_size, nullptr, &parent_data)) + { + LOG_ERROR("SetupDiGetDeviceInterfaceDetail failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + uint16_t vid, pid; + std::wstring ws(detail_data.get()->DevicePath); + std::string path(ws.begin(), ws.end()); + parse_usb_path_single_interface(vid, pid, serial, path); + + // get driver key for composite device + buf_size = 0; + SetupDiGetDeviceRegistryProperty(device_info, &parent_data, SPDRP_DRIVER, nullptr, nullptr, 0, &buf_size); + if (GetLastError() != ERROR_INSUFFICIENT_BUFFER) + { + LOG_ERROR("SetupDiGetDeviceRegistryProperty failed in an unexpected manner"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + alloc = std::malloc(buf_size); + if (!alloc) + { + LOG_ERROR("malloc fail"); + return false; + } + auto driver_key = std::shared_ptr(reinterpret_cast(alloc), std::free); + if (!SetupDiGetDeviceRegistryProperty(device_info, &parent_data, SPDRP_DRIVER, nullptr, driver_key.get(), buf_size, nullptr)) + { + LOG_ERROR("SetupDiGetDeviceRegistryProperty failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; + } + + // contains composite device key + std::wstring targetKey(reinterpret_cast(driver_key.get())); + + // recursively check all hubs, searching for composite device + std::wstringstream buf; + for (int i = 0;; i++) + { + buf << "\\\\.\\HCD" << i; + std::wstring hcd = buf.str(); + + // grab handle + HANDLE h = CreateFile(hcd.c_str(), GENERIC_WRITE | GENERIC_READ, FILE_SHARE_READ | FILE_SHARE_WRITE, nullptr, OPEN_EXISTING, 0, nullptr); + auto h_gc = std::shared_ptr(h, CloseHandle); + if (h == INVALID_HANDLE_VALUE) + { + LOG_ERROR("CreateFile failed"); + break; + } + else + { + USB_ROOT_HUB_NAME name; + + // get required space + if (!DeviceIoControl(h, IOCTL_USB_GET_ROOT_HUB_NAME, nullptr, 0, &name, sizeof(name), nullptr, nullptr)) { + LOG_ERROR("DeviceIoControl failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; // alt: fail silently and hope its on a different root hub + } + + // alloc space + alloc = std::malloc(name.ActualLength); + if (!alloc) + { + LOG_ERROR("malloc fail"); + return false; + } + auto pName = std::shared_ptr(reinterpret_cast(alloc), std::free); + + // get name + if (!DeviceIoControl(h, IOCTL_USB_GET_ROOT_HUB_NAME, nullptr, 0, pName.get(), name.ActualLength, nullptr, nullptr)) { + LOG_ERROR("DeviceIoControl failed"); + SetupDiDestroyDeviceInfoList(device_info); + return false; // alt: fail silently and hope its on a different root hub + } + + // return location if device is connected under this root hub, also provide the port USB spec/speed + auto usb_res = handle_usb_hub(targetKey, std::wstring(pName->RootHubName)); + if (std::get<0>(usb_res) != "") + { + SetupDiDestroyDeviceInfoList(device_info); + //location = std::get<0>(usb_res); + //spec = std::get<1>(usb_res); + return true; + } + } + } + } + + SetupDiDestroyDeviceInfoList(device_info); + } + + LOG_ERROR("could not find camera in windows device tree"); + return false; + } + // Provides Port Id and the USB Specification (USB type) - bool get_usb_descriptors(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid, - std::string& location, usb_spec& spec) + bool get_usb_descriptors(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid, std::string& location, usb_spec& spec) { SP_DEVINFO_DATA devInfo = { sizeof(SP_DEVINFO_DATA) }; diff --git a/src/win/win-helpers.h b/src/win/win-helpers.h index 82c1baf632..8b6a5a2a64 100644 --- a/src/win/win-helpers.h +++ b/src/win/win-helpers.h @@ -27,10 +27,10 @@ namespace librealsense std::vector tokenize(std::string string, char separator); - bool parse_usb_path(uint16_t & vid, uint16_t & pid, uint16_t & mi, std::string & unique_id, const std::string & path); - - bool get_usb_descriptors(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid, - std::string& location, usb_spec& spec); + bool parse_usb_path_multiple_interface(uint16_t & vid, uint16_t & pid, uint16_t & mi, std::string & unique_id, const std::string & path); + bool parse_usb_path_single_interface(uint16_t & vid, uint16_t & pid, uint16_t & mi, std::string & unique_id, const std::string & path); + bool get_usb_descriptors(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid, std::string& location, usb_spec& spec); + bool get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid, std::string& serial); class event_base { diff --git a/src/win/win-hid.cpp b/src/win/win-hid.cpp index 236b7a07ba..314f2fb35c 100644 --- a/src/win/win-hid.cpp +++ b/src/win/win-hid.cpp @@ -103,6 +103,7 @@ namespace librealsense return E_INVALIDARG; } + BSTR fName{}; SYSTEMTIME time; report->GetTimestamp(&time); @@ -138,7 +139,9 @@ namespace librealsense data.z = rawZ; data.ts_low = customTimestampLow; data.ts_high = customTimestampHigh; - d.sensor.name = "HID Sensor Class Device: Gyroscope"; + + pSensor->GetFriendlyName(&fName); + d.sensor.name = CW2A(fName); d.fo.pixels = &data; d.fo.metadata = NULL; @@ -193,15 +196,46 @@ namespace librealsense void wmf_hid_device::open(const std::vector&iio_profiles) { + try + { + for (auto& sensor_to_open : iio_profiles) + { + for (auto& connected_sensor : _connected_sensors) + { + if (sensor_to_open.sensor_name == connected_sensor->get_sensor_name()) + { + _opened_sensors.push_back(connected_sensor); + } + } + } + } + catch (...) + { + for (auto& connected_sensor : _connected_sensors) + { + connected_sensor.reset(); + } + _connected_sensors.clear(); + LOG_ERROR("Hid device is busy!"); + throw; + } } void wmf_hid_device::close() { + for (auto& open_sensor : _opened_sensors) + { + open_sensor.reset(); + } + _opened_sensors.clear(); } void wmf_hid_device::stop_capture() { - _sensor->SetEventSink(NULL); + for (auto& sensor : _opened_sensors) + { + sensor->stop_capture(); + } _cb = nullptr; } @@ -212,7 +246,11 @@ namespace librealsense _cb = new sensor_events(callback); ISensorEvents* sensorEvents = nullptr; CHECK_HR(_cb->QueryInterface(IID_PPV_ARGS(&sensorEvents))); - CHECK_HR(_sensor->SetEventSink(sensorEvents)); + + for (auto& sensor : _opened_sensors) + { + CHECK_HR(sensor->start_capture(sensorEvents)); + } } std::vector wmf_hid_device::get_sensors() @@ -221,10 +259,11 @@ namespace librealsense HRESULT res = S_OK; BSTR fName{}; - LOG_HR(res = _sensor->GetFriendlyName(&fName)); - if (FAILED(res)) fName = L"Unidentified HID Sensor"; - sensors.push_back({ std::string(fName, fName + wcslen(fName)) }); + for (auto& sensor : _opened_sensors) + { + sensors.push_back({ sensor->get_sensor_name() }); + } SysFreeString(fName); @@ -304,13 +343,17 @@ namespace librealsense uint16_t vid, pid, mi; std::string uid; - if (parse_usb_path(vid, pid, mi, uid, info.device_path)) + if (parse_usb_path_multiple_interface(vid, pid, mi, uid, info.device_path)) { info.unique_id = "*"; info.pid = to_string() << std::hex << pid; info.vid = to_string() << std::hex << vid; } } + if (IsEqualPropertyKey(propertyKey, SENSOR_PROPERTY_SERIAL_NUMBER)) + { + info.serial_number = std::string(propertyValue.pwszVal, propertyValue.pwszVal + wcslen(propertyValue.pwszVal)); + } } PropVariantClear(&propertyValue); diff --git a/src/win/win-hid.h b/src/win/win-hid.h index 5e12a94ba3..4dc449f2e1 100644 --- a/src/win/win-hid.h +++ b/src/win/win-hid.h @@ -12,24 +12,61 @@ namespace librealsense { namespace platform { + + class wmf_hid_sensor { + public: + wmf_hid_sensor(const hid_device_info& device_info, CComPtr pISensor) : + _hid_device_info(device_info), _pISensor(pISensor) + { + BSTR fName{}; + + auto res = pISensor->GetFriendlyName(&fName); + if (FAILED(res)) + { + fName = L"Unidentified HID Sensor"; + } + + _name = CW2A(fName); + }; + + const std::string& get_sensor_name() const { return _name; } + + HRESULT start_capture(ISensorEvents* sensorEvents) + { + return _pISensor->SetEventSink(sensorEvents); + } + + HRESULT stop_capture() + { + return _pISensor->SetEventSink(NULL); + } + + private: + + hid_device_info _hid_device_info; + CComPtr _pISensor; + std::string _name; + }; + class wmf_hid_device : public hid_device { public: static void foreach_hid_device(std::function)> action); - - explicit wmf_hid_device(CComPtr sensor) : _sensor(sensor) {} + wmf_hid_device(const hid_device_info& info); void open(const std::vector&iio_profiles) override; void close() override; void stop_capture() override; void start_capture(hid_callback callback) override; - std::vector get_sensors() override; - std::vector get_custom_report_data(const std::string& custom_sensor_name, - const std::string& report_name, - custom_sensor_report_field report_field) override; + std::vector get_sensors() override; // Get opened sensors + std::vector get_custom_report_data(const std::string& custom_sensor_name, const std::string& report_name, custom_sensor_report_field report_field) override; private: - CComPtr _sensor = nullptr; + + std::vector> _connected_sensors; // Vector of all connected sensors of this device + std::vector> _opened_sensors; // Vector of all opened sensors of this device (subclass of _connected_sensors) + std::vector> _streaming_sensors; // Vector of all streaming sensors of this device (subclass of _connected_sensors) + CComPtr _cb = nullptr; }; } diff --git a/src/win/win-uvc.cpp b/src/win/win-uvc.cpp index 47d44429e0..3def40746e 100644 --- a/src/win/win-uvc.cpp +++ b/src/win/win-uvc.cpp @@ -733,7 +733,7 @@ namespace librealsense CoTaskMemFree(wchar_name); uint16_t vid, pid, mi; std::string unique_id; - if (!parse_usb_path(vid, pid, mi, unique_id, name)) continue; + if (!parse_usb_path_multiple_interface(vid, pid, mi, unique_id, name)) continue; uvc_device_info info; info.vid = vid; From 21df97eaf1095c91cab950fe461ea907374dd363 Mon Sep 17 00:00:00 2001 From: abernste Date: Wed, 21 Nov 2018 14:50:58 +0200 Subject: [PATCH 3/9] Linux compilation fixes --- src/backend.h | 13 +++++++++++++ src/context.cpp | 6 ++---- src/linux/backend-v4l2.cpp | 13 +++++++++++++ src/win/win-backend.cpp | 13 +++++++++++++ src/win/win-helpers.cpp | 1 - src/win/win-helpers.h | 2 +- 6 files changed, 42 insertions(+), 6 deletions(-) diff --git a/src/backend.h b/src/backend.h index 80c788569f..032c46944e 100644 --- a/src/backend.h +++ b/src/backend.h @@ -64,6 +64,19 @@ namespace librealsense namespace platform { + class hid_serial_info + { + public: + hid_serial_info(uint16_t vid, uint16_t pid, const std::string& unique_id); + std::string get_serial(void); + + private: + uint16_t _vid; + uint16_t _pid; + std::string _unique_id; + std::string _device_serial; + }; + struct control_range { control_range() diff --git a/src/context.cpp b/src/context.cpp index 645c03b15a..6448fb9b00 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -19,7 +19,6 @@ #include "stream.h" #include "environment.h" #include "context.h" -#include "win/win-helpers.h" #ifdef WITH_TRACKING #include "tm2/tm-info.h" @@ -497,13 +496,12 @@ namespace librealsense { if (hid.unique_id != "") { - std::string device_serial; uint16_t vid = std::stoi(hid.vid, nullptr, 16); uint16_t pid = std::stoi(hid.pid, nullptr, 16); - platform::get_device_serial(vid, pid, unique_id, device_serial); + platform::hid_serial_info hid_data(vid, pid, unique_id); - if ((hid.unique_id == unique_id) || ((hid.unique_id == "*") && (hid.serial_number == device_serial))) + if ((hid.unique_id == unique_id) || ((hid.unique_id == "*") && (hid.serial_number == hid_data.get_serial()))) { hid_group.push_back(hid); } diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 7d990141fc..0e0fcbeb52 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -1738,6 +1738,19 @@ namespace librealsense { return std::make_shared(); } + + hid_serial_info::hid_serial_info(uint16_t vid, uint16_t pid, const std::string& unique_id) + { + _vid = vid; + _pid = pid; + _unique_id = unique_id; + _device_serial = ""; + } + + std::string hid_serial_info::get_serial(void) + { + return _device_serial; + } } } diff --git a/src/win/win-backend.cpp b/src/win/win-backend.cpp index 96cba50e35..a7a77bcd7c 100644 --- a/src/win/win-backend.cpp +++ b/src/win/win-backend.cpp @@ -340,6 +340,19 @@ namespace librealsense { return std::make_shared(this); } + + hid_serial_info::hid_serial_info(uint16_t vid, uint16_t pid, const std::string& unique_id) + { + _vid = vid; + _pid = pid; + _unique_id = unique_id; + get_device_serial(_vid, _pid, _unique_id, _device_serial); + } + + std::string hid_serial_info::get_serial(void) + { + return _device_serial; + } } } diff --git a/src/win/win-helpers.cpp b/src/win/win-helpers.cpp index 84491b248b..3fad9e962d 100644 --- a/src/win/win-helpers.cpp +++ b/src/win/win-helpers.cpp @@ -586,7 +586,6 @@ namespace librealsense // Provides Port Id and the USB Specification (USB type) bool get_usb_descriptors(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid, std::string& location, usb_spec& spec) { - SP_DEVINFO_DATA devInfo = { sizeof(SP_DEVINFO_DATA) }; std::vector guids = { GUID_DEVINTERFACE_IMAGE, GUID_DEVINTERFACE_CAMERA }; diff --git a/src/win/win-helpers.h b/src/win/win-helpers.h index 8b6a5a2a64..a050cd22bf 100644 --- a/src/win/win-helpers.h +++ b/src/win/win-helpers.h @@ -28,7 +28,7 @@ namespace librealsense std::vector tokenize(std::string string, char separator); bool parse_usb_path_multiple_interface(uint16_t & vid, uint16_t & pid, uint16_t & mi, std::string & unique_id, const std::string & path); - bool parse_usb_path_single_interface(uint16_t & vid, uint16_t & pid, uint16_t & mi, std::string & unique_id, const std::string & path); + bool parse_usb_path_single_interface(uint16_t & vid, uint16_t & pid, std::string & serial, const std::string & path); bool get_usb_descriptors(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid, std::string& location, usb_spec& spec); bool get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid, std::string& serial); From ddead1bd36ace4401a8652b472ee1dae1ad35ca4 Mon Sep 17 00:00:00 2001 From: abernste Date: Wed, 21 Nov 2018 16:46:24 +0200 Subject: [PATCH 4/9] 1. Linux compilation fix 2. HID HW timestamp in Metadata added 3. use HID_TIMESTAMP_MULTIPLIER for Linux/Windows different timestamp calculations --- src/backend.h | 2 ++ src/context.cpp | 10 +++++++--- src/ds5/ds5-timestamp.cpp | 2 +- src/types.h | 6 ++++++ src/win/win-hid.cpp | 5 ++++- 5 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/backend.h b/src/backend.h index 032c46944e..e068784776 100644 --- a/src/backend.h +++ b/src/backend.h @@ -398,6 +398,7 @@ namespace librealsense value }; +#pragma pack(push, 1) struct hid_sensor_data { short x; @@ -409,6 +410,7 @@ namespace librealsense uint32_t ts_low; uint32_t ts_high; }; +#pragma pack(pop) typedef std::function hid_callback; diff --git a/src/context.cpp b/src/context.cpp index 6448fb9b00..5f20c8456f 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -488,6 +488,9 @@ namespace librealsense const std::vector& hids) { std::vector, std::vector>> results; + uint16_t vid; + uint16_t pid; + for (auto&& dev : devices) { std::vector hid_group; @@ -496,12 +499,13 @@ namespace librealsense { if (hid.unique_id != "") { - uint16_t vid = std::stoi(hid.vid, nullptr, 16); - uint16_t pid = std::stoi(hid.pid, nullptr, 16); + std::stringstream(hid.vid) >> std::hex >> vid; + std::stringstream(hid.pid) >> std::hex >> pid; platform::hid_serial_info hid_data(vid, pid, unique_id); - if ((hid.unique_id == unique_id) || ((hid.unique_id == "*") && (hid.serial_number == hid_data.get_serial()))) + if ((hid.unique_id == unique_id) || // Linux + ((hid.unique_id == "*") && (hid.serial_number == hid_data.get_serial()))) // Windows { hid_group.push_back(hid); } diff --git a/src/ds5/ds5-timestamp.cpp b/src/ds5/ds5-timestamp.cpp index 23483b1820..ed6e790c62 100644 --- 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(timestamp) * TIMESTAMP_NSEC_TO_MSEC; + return static_cast(timestamp) * HID_TIMESTAMP_MULTIPLIER; } if (!started) diff --git a/src/types.h b/src/types.h index a7a5b01970..75398ddac8 100644 --- a/src/types.h +++ b/src/types.h @@ -50,6 +50,12 @@ namespace librealsense const double TIMESTAMP_USEC_TO_MSEC = 0.001; const double TIMESTAMP_NSEC_TO_MSEC = 0.000001; +#ifdef _WIN32 +#define HID_TIMESTAMP_MULTIPLIER TIMESTAMP_USEC_TO_MSEC +#else +#define HID_TIMESTAMP_MULTIPLIER TIMESTAMP_NSEC_TO_MSEC +#endif // define HID_TIMESTAMP_MULTIPLIER + /////////////////////////////////// // Utility types for general use // /////////////////////////////////// diff --git a/src/win/win-hid.cpp b/src/win/win-hid.cpp index 314f2fb35c..884d2154fa 100644 --- a/src/win/win-hid.cpp +++ b/src/win/win-hid.cpp @@ -29,6 +29,8 @@ #pragma comment(lib, "Sensorsapi.lib") #pragma comment(lib, "PortableDeviceGuids.lib") +const uint8_t HID_METADATA_SIZE = 8; // bytes + namespace librealsense { namespace platform @@ -144,7 +146,8 @@ namespace librealsense d.sensor.name = CW2A(fName); d.fo.pixels = &data; - d.fo.metadata = NULL; + d.fo.metadata = &data.ts_low; + d.fo.metadata_size = HID_METADATA_SIZE; d.fo.frame_size = sizeof(data); _callback(d); From 45f9cdabdc9d523355bc8ec408dca72743d471e1 Mon Sep 17 00:00:00 2001 From: abernste Date: Thu, 22 Nov 2018 19:37:08 +0200 Subject: [PATCH 5/9] 1. Fix backend compilation on all OSs 2. Update Accel/Gyro rates --- src/backend.h | 19 ++++++------------- src/context.cpp | 15 ++++++--------- src/context.h | 1 + src/ds5/ds5-factory.cpp | 8 +++----- src/ds5/ds5-motion.h | 7 +++++-- src/linux/backend-v4l2.cpp | 13 ------------- src/win/win-backend.cpp | 14 ++++---------- src/win/win-backend.h | 2 ++ 8 files changed, 27 insertions(+), 52 deletions(-) diff --git a/src/backend.h b/src/backend.h index e068784776..b3d592f46e 100644 --- a/src/backend.h +++ b/src/backend.h @@ -64,19 +64,6 @@ namespace librealsense namespace platform { - class hid_serial_info - { - public: - hid_serial_info(uint16_t vid, uint16_t pid, const std::string& unique_id); - std::string get_serial(void); - - private: - uint16_t _vid; - uint16_t _pid; - std::string _unique_id; - std::string _device_serial; - }; - struct control_range { control_range() @@ -684,6 +671,12 @@ namespace librealsense virtual std::shared_ptr create_time_service() const = 0; virtual std::shared_ptr 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 index 5f20c8456f..d9687e5377 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -484,13 +484,11 @@ namespace librealsense } std::vector, std::vector>> group_devices_and_hids_by_unique_id( + std::shared_ptr ctx, const std::vector>& devices, const std::vector& hids) { std::vector, std::vector>> results; - uint16_t vid; - uint16_t pid; - for (auto&& dev : devices) { std::vector hid_group; @@ -499,13 +497,12 @@ namespace librealsense { if (hid.unique_id != "") { - std::stringstream(hid.vid) >> std::hex >> vid; - std::stringstream(hid.pid) >> std::hex >> pid; - - platform::hid_serial_info hid_data(vid, pid, unique_id); + uint16_t vid = std::stoi(hid.vid, nullptr, 16); + uint16_t pid = std::stoi(hid.pid, nullptr, 16); + auto&& backend = ctx->get_backend(); + auto device_serial = backend.get_device_serial(vid, pid, unique_id); - if ((hid.unique_id == unique_id) || // Linux - ((hid.unique_id == "*") && (hid.serial_number == hid_data.get_serial()))) // Windows + if ((hid.unique_id == unique_id) || ((hid.unique_id == "*") && (hid.serial_number == device_serial))) { hid_group.push_back(hid); } diff --git a/src/context.h b/src/context.h index d044bbe51a..dfd1594fff 100644 --- a/src/context.h +++ b/src/context.h @@ -176,6 +176,7 @@ namespace librealsense // Helper functions for device list manipulation: std::vector filter_by_product(const std::vector& devices, const std::set& pid_list); std::vector, std::vector>> group_devices_and_hids_by_unique_id( + std::shared_ptr ctx, const std::vector>& devices, const std::vector& hids); std::vector> group_devices_by_unique_id(const std::vector& devices); diff --git a/src/ds5/ds5-factory.cpp b/src/ds5/ds5-factory.cpp index aba08bc8ae..794b8941e7 100644 --- a/src/ds5/ds5-factory.cpp +++ b/src/ds5/ds5-factory.cpp @@ -430,10 +430,8 @@ 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 }); - - // TODO - Check which IMU shall be activated by default - tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 1000, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); - tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 1000, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 400, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 250, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); return tags; }; @@ -516,7 +514,7 @@ namespace librealsense std::vector> 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 index 3f5c56e063..e5f7f5fd1f 100644 --- a/src/ds5/ds5-motion.h +++ b/src/ds5/ds5-motion.h @@ -46,8 +46,11 @@ namespace librealsense #ifdef _WIN32 // Bandwidth parameters from BOSCH BMI 055 spec' std::vector> sensor_name_and_hid_profiles = - { { "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: 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} }, /*{ "HID Sensor Class Device: Custom", { RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F } }*/ }; #else // Bandwidth parameters from BOSCH BMI 055 spec' diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 0e0fcbeb52..7d990141fc 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -1738,19 +1738,6 @@ namespace librealsense { return std::make_shared(); } - - hid_serial_info::hid_serial_info(uint16_t vid, uint16_t pid, const std::string& unique_id) - { - _vid = vid; - _pid = pid; - _unique_id = unique_id; - _device_serial = ""; - } - - std::string hid_serial_info::get_serial(void) - { - return _device_serial; - } } } diff --git a/src/win/win-backend.cpp b/src/win/win-backend.cpp index a7a77bcd7c..6e3fc9d566 100644 --- a/src/win/win-backend.cpp +++ b/src/win/win-backend.cpp @@ -341,17 +341,11 @@ namespace librealsense return std::make_shared(this); } - hid_serial_info::hid_serial_info(uint16_t vid, uint16_t pid, const std::string& unique_id) + std::string wmf_backend::get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid) const { - _vid = vid; - _pid = pid; - _unique_id = unique_id; - get_device_serial(_vid, _pid, _unique_id, _device_serial); - } - - std::string hid_serial_info::get_serial(void) - { - return _device_serial; + 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 index 147c4cda7c..4dd9ff3481 100644 --- a/src/win/win-backend.h +++ b/src/win/win-backend.h @@ -25,6 +25,8 @@ namespace librealsense std::vector query_hid_devices() const override; virtual std::shared_ptr create_time_service() const override; std::shared_ptr 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; }; From 11dbd49cd7b0e06b29697784bc69bd12ea214781 Mon Sep 17 00:00:00 2001 From: abernste Date: Thu, 22 Nov 2018 20:35:59 +0200 Subject: [PATCH 6/9] Travis fix --- src/context.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/context.cpp b/src/context.cpp index d9687e5377..0e316dc9f5 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -489,6 +489,9 @@ namespace librealsense const std::vector& hids) { std::vector, std::vector>> results; + uint16_t vid; + uint16_t pid; + for (auto&& dev : devices) { std::vector hid_group; @@ -497,8 +500,8 @@ namespace librealsense { if (hid.unique_id != "") { - uint16_t vid = std::stoi(hid.vid, nullptr, 16); - uint16_t pid = std::stoi(hid.pid, nullptr, 16); + 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); From a8cf50fba721f36daa67598f06dd4b9990ca4635 Mon Sep 17 00:00:00 2001 From: abernste Date: Wed, 28 Nov 2018 09:01:43 +0200 Subject: [PATCH 7/9] Support sensor FPS change --- src/ds5/ds5-motion.h | 14 ++++++++++++-- src/win/win-hid.cpp | 46 +++++++++++++++++++++++++++++++------------- src/win/win-hid.h | 1 + 3 files changed, 46 insertions(+), 15 deletions(-) diff --git a/src/ds5/ds5-motion.h b/src/ds5/ds5-motion.h index e5f7f5fd1f..e017dc88de 100644 --- a/src/ds5/ds5-motion.h +++ b/src/ds5/ds5-motion.h @@ -52,6 +52,17 @@ namespace librealsense { "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} }, /*{ "HID Sensor Class Device: Custom", { RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F } }*/ }; + + // Translate frequency to SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL + std::map> fps_and_sampling_frequency_per_rs2_stream = + {{RS2_STREAM_ACCEL,{{63, 1000}, + {125, 800}, + {250, 400 }, + {500, 200 }}}, + {RS2_STREAM_GYRO, {{100, 1000}, + {200, 500}, + {400, 250}}}}; + #else // Bandwidth parameters from BOSCH BMI 055 spec' std::vector> sensor_name_and_hid_profiles = @@ -69,8 +80,6 @@ namespace librealsense {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 125, RS2_FORMAT_MOTION_XYZ32F}}, {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250, RS2_FORMAT_MOTION_XYZ32F}}, {"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 500, RS2_FORMAT_MOTION_XYZ32F}}}; -#endif - // The frequency selector is vendor and model-specific std::map> fps_and_sampling_frequency_per_rs2_stream = @@ -81,6 +90,7 @@ namespace librealsense {RS2_STREAM_GYRO, {{100, 1}, {200, 2}, {400, 4}}}}; +#endif protected: std::shared_ptr _fisheye_stream; diff --git a/src/win/win-hid.cpp b/src/win/win-hid.cpp index 884d2154fa..a47e9616c4 100644 --- a/src/win/win-hid.cpp +++ b/src/win/win-hid.cpp @@ -201,13 +201,34 @@ namespace librealsense { try { - for (auto& sensor_to_open : iio_profiles) + for (auto& profile_to_open : iio_profiles) { for (auto& connected_sensor : _connected_sensors) { - if (sensor_to_open.sensor_name == connected_sensor->get_sensor_name()) + if (profile_to_open.sensor_name == connected_sensor->get_sensor_name()) { - _opened_sensors.push_back(connected_sensor); + /* Set SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL sensor property to profile */ + HRESULT hr = S_OK; + IPortableDeviceValues* pPropsToSet = NULL; // Input + IPortableDeviceValues* pPropsReturn = NULL; // Output + + /* Create the input object */ + CHECK_HR(CoCreateInstance(__uuidof(PortableDeviceValues), NULL, CLSCTX_INPROC_SERVER, IID_PPV_ARGS(&pPropsToSet))); + + /* Add the current report interval property */ + hr = pPropsToSet->SetUnsignedIntegerValue(SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL, profile_to_open.frequency); + if (SUCCEEDED(hr)) + { + // Setting a single property + hr = connected_sensor->get_sensor()->SetProperties(pPropsToSet, &pPropsReturn); + if (SUCCEEDED(hr)) + { + _opened_sensors.push_back(connected_sensor); + pPropsReturn->Release(); + } + } + + pPropsToSet->Release(); } } } @@ -233,16 +254,6 @@ namespace librealsense _opened_sensors.clear(); } - void wmf_hid_device::stop_capture() - { - for (auto& sensor : _opened_sensors) - { - sensor->stop_capture(); - } - _cb = nullptr; - - } - void wmf_hid_device::start_capture(hid_callback callback) { // Hack, start default profile @@ -256,6 +267,15 @@ namespace librealsense } } + void wmf_hid_device::stop_capture() + { + for (auto& sensor : _opened_sensors) + { + sensor->stop_capture(); + } + _cb = nullptr; + } + std::vector wmf_hid_device::get_sensors() { std::vector sensors; diff --git a/src/win/win-hid.h b/src/win/win-hid.h index 4dc449f2e1..ef57d25efc 100644 --- a/src/win/win-hid.h +++ b/src/win/win-hid.h @@ -30,6 +30,7 @@ namespace librealsense }; const std::string& get_sensor_name() const { return _name; } + const CComPtr& get_sensor() const { return _pISensor; } HRESULT start_capture(ISensorEvents* sensorEvents) { From e4be0cf1f6ada48e4abd87c1deac5510d640cb19 Mon Sep 17 00:00:00 2001 From: abernste Date: Wed, 28 Nov 2018 11:09:36 +0200 Subject: [PATCH 8/9] Linux compilation fix --- src/ds5/ds5-motion.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/ds5/ds5-motion.h b/src/ds5/ds5-motion.h index 2dec2dd5ad..9f43d9dd0a 100644 --- a/src/ds5/ds5-motion.h +++ b/src/ds5/ds5-motion.h @@ -61,14 +61,14 @@ namespace librealsense #else // Bandwidth parameters from BOSCH BMI 055 spec' std::vector> 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}}, + {{"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> fps_and_sampling_frequency_per_rs2_stream = From 2979926e2a41867618ecbe947247b5be000be823 Mon Sep 17 00:00:00 2001 From: abernste Date: Wed, 28 Nov 2018 11:31:19 +0200 Subject: [PATCH 9/9] 1. Add some Comments 2. small tweaks --- src/context.cpp | 3 ++- src/ds5/ds5-motion.cpp | 2 +- src/types.h | 2 ++ src/win/win-backend.cpp | 2 +- 4 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/context.cpp b/src/context.cpp index 0e316dc9f5..b27ad91928 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -505,7 +505,8 @@ namespace librealsense auto&& backend = ctx->get_backend(); auto device_serial = backend.get_device_serial(vid, pid, unique_id); - if ((hid.unique_id == unique_id) || ((hid.unique_id == "*") && (hid.serial_number == device_serial))) + if ((hid.unique_id == unique_id) || // Linux check + ((hid.unique_id == "*") && (hid.serial_number == device_serial))) // Windows check { hid_group.push_back(hid); } diff --git a/src/ds5/ds5-motion.cpp b/src/ds5/ds5-motion.cpp index 02a2f20a92..97627c31f8 100644 --- a/src/ds5/ds5-motion.cpp +++ b/src/ds5/ds5-motion.cpp @@ -178,7 +178,7 @@ namespace librealsense static const char* custom_sensor_fw_ver = "5.6.0.0"; - auto hid_ep = std::make_shared(this, ctx->get_backend().create_hid_device(all_hid_infos[1]), + auto hid_ep = std::make_shared(this, ctx->get_backend().create_hid_device(all_hid_infos.front()), std::unique_ptr(new ds5_iio_hid_timestamp_reader()), std::unique_ptr(new ds5_custom_hid_timestamp_reader()), fps_and_sampling_frequency_per_rs2_stream, diff --git a/src/types.h b/src/types.h index 67c493fb6e..8a73963081 100644 --- a/src/types.h +++ b/src/types.h @@ -51,8 +51,10 @@ namespace librealsense 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 diff --git a/src/win/win-backend.cpp b/src/win/win-backend.cpp index 6e3fc9d566..7f1400b08d 100644 --- 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_MULTITHREADED); // when using COINIT_APARTMENTTHREADED, calling _pISensor->SetEventSink(NULL) to stop sensor takes too much time + CoInitializeEx(nullptr, COINIT_MULTITHREADED); // when using COINIT_APARTMENTTHREADED, calling _pISensor->SetEventSink(NULL) to stop sensor can take several seconds MFStartup(MF_VERSION, MFSTARTUP_NOSOCKET); }