diff --git a/src/backend.h b/src/backend.h index 2c0388a9b0..b3d592f46e 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() { @@ -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 hid_callback; @@ -668,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 b739f36394..b27ad91928 100644 --- a/src/context.cpp +++ b/src/context.cpp @@ -484,18 +484,33 @@ 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; 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 index da19fc058f..6f520a83a6 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 4f998a888b..b40f21198d 100644 --- 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> 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 031ff7f5de..9f43d9dd0a 100644 --- a/src/ds5/ds5-motion.h +++ b/src/ds5/ds5-motion.h @@ -43,19 +43,32 @@ namespace librealsense lazy> _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> 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> 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> 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> fps_and_sampling_frequency_per_rs2_stream = @@ -63,6 +76,7 @@ namespace librealsense {250, 3}}}, {RS2_STREAM_GYRO, {{200, 2}, {400, 4}}}}; +#endif protected: std::shared_ptr _fisheye_stream; 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/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 2b08728902..8a73963081 100644 --- 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 index 8d00a3612b..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_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 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 @@ -335,6 +340,13 @@ namespace librealsense { return std::make_shared(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 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; }; diff --git a/src/win/win-helpers.cpp b/src/win/win-helpers.cpp index 7e979a9ab5..3fad9e962d 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,11 +353,239 @@ namespace librealsense return res; } - // 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_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) + { 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 82c1baf632..a050cd22bf 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, 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); class event_base { diff --git a/src/win/win-hid.cpp b/src/win/win-hid.cpp index 7132923420..a47e9616c4 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 @@ -103,6 +105,7 @@ namespace librealsense return E_INVALIDARG; } + BSTR fName{}; SYSTEMTIME time; report->GetTimestamp(&time); @@ -138,10 +141,13 @@ namespace librealsense data.z = rawZ; data.ts_low = customTimestampLow; data.ts_high = customTimestampHigh; - d.sensor.name = ""; + + pSensor->GetFriendlyName(&fName); + 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); @@ -153,7 +159,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. @@ -193,17 +199,59 @@ namespace librealsense void wmf_hid_device::open(const std::vector&iio_profiles) { - } + try + { + for (auto& profile_to_open : iio_profiles) + { + for (auto& connected_sensor : _connected_sensors) + { + if (profile_to_open.sensor_name == connected_sensor->get_sensor_name()) + { + /* Set SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL sensor property to profile */ + HRESULT hr = S_OK; + IPortableDeviceValues* pPropsToSet = NULL; // Input + IPortableDeviceValues* pPropsReturn = NULL; // Output - void wmf_hid_device::close() - { + /* 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(); + } + } + } + } + catch (...) + { + for (auto& connected_sensor : _connected_sensors) + { + connected_sensor.reset(); + } + _connected_sensors.clear(); + LOG_ERROR("Hid device is busy!"); + throw; + } } - void wmf_hid_device::stop_capture() + void wmf_hid_device::close() { - _sensor->SetEventSink(NULL); - _cb = nullptr; - + for (auto& open_sensor : _opened_sensors) + { + open_sensor.reset(); + } + _opened_sensors.clear(); } void wmf_hid_device::start_capture(hid_callback callback) @@ -212,7 +260,20 @@ 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)); + } + } + + void wmf_hid_device::stop_capture() + { + for (auto& sensor : _opened_sensors) + { + sensor->stop_capture(); + } + _cb = nullptr; } std::vector wmf_hid_device::get_sensors() @@ -221,10 +282,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); @@ -238,134 +300,92 @@ 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; 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(pk, SENSOR_PROPERTY_MODEL)) - //{ - // info.pid = std::string(pv.pwszVal, pv.pwszVal + wcslen(pv.pwszVal)); - //} + if (IsEqualPropertyKey(propertyKey, SENSOR_PROPERTY_SERIAL_NUMBER)) + { + info.serial_number = std::string(propertyValue.pwszVal, propertyValue.pwszVal + wcslen(propertyValue.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); } } diff --git a/src/win/win-hid.h b/src/win/win-hid.h index 5e12a94ba3..ef57d25efc 100644 --- a/src/win/win-hid.h +++ b/src/win/win-hid.h @@ -12,24 +12,62 @@ 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; } + const CComPtr& get_sensor() const { return _pISensor; } + + 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;