Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

D435i windows IMU support #2788

Merged
merged 16 commits into from
Nov 28, 2018
Merged

D435i windows IMU support #2788

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
9f76354
Merge pull request #38 from IntelRealSense/development
abernste Nov 5, 2018
95f8b28
Add HID support for D435i
abernste Nov 6, 2018
ec2ab3c
Merge pull request #39 from IntelRealSense/development
abernste Nov 21, 2018
0dc6d49
Merge branch 'windows_hid_support' into development
abernste Nov 21, 2018
0afb815
Merge pull request #40 from abernste/development
abernste Nov 21, 2018
d4ca87f
D435i IMU support
abernste Nov 21, 2018
21df97e
Linux compilation fixes
abernste Nov 21, 2018
ddead1b
1. Linux compilation fix
abernste Nov 21, 2018
45f9cda
1. Fix backend compilation on all OSs
abernste Nov 22, 2018
11dbd49
Travis fix
abernste Nov 22, 2018
a8cf50f
Support sensor FPS change
abernste Nov 28, 2018
22491c7
Merge branch 'windows_hid_support' into development
abernste Nov 28, 2018
8340c14
Merge pull request #43 from abernste/development
abernste Nov 28, 2018
e4be0cf
Linux compilation fix
abernste Nov 28, 2018
2979926
1. Add some Comments
abernste Nov 28, 2018
da040ac
Merge branch 'development' into windows_hid_support
abernste Nov 28, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions src/backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -287,6 +287,7 @@ namespace librealsense
std::string pid;
std::string unique_id;
std::string device_path;
std::string serial_number;

operator std::string()
{
Expand Down Expand Up @@ -384,6 +385,7 @@ namespace librealsense
value
};

#pragma pack(push, 1)
struct hid_sensor_data
{
short x;
Expand All @@ -395,6 +397,7 @@ namespace librealsense
uint32_t ts_low;
uint32_t ts_high;
};
#pragma pack(pop)

typedef std::function<void(const sensor_data&)> hid_callback;

Expand Down Expand Up @@ -668,6 +671,12 @@ namespace librealsense
virtual std::shared_ptr<time_service> create_time_service() const = 0;

virtual std::shared_ptr<device_watcher> create_device_watcher() const = 0;

virtual std::string get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid) const
{
std::string empty_str;
return empty_str;
}

virtual ~backend() = default;
};
Expand Down
19 changes: 17 additions & 2 deletions src/context.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -484,18 +484,33 @@ namespace librealsense
}

std::vector<std::pair<std::vector<platform::uvc_device_info>, std::vector<platform::hid_device_info>>> group_devices_and_hids_by_unique_id(
std::shared_ptr<context> ctx,
const std::vector<std::vector<platform::uvc_device_info>>& devices,
const std::vector<platform::hid_device_info>& hids)
{
std::vector<std::pair<std::vector<platform::uvc_device_info>, std::vector<platform::hid_device_info>>> results;
uint16_t vid;
uint16_t pid;

for (auto&& dev : devices)
{
std::vector<platform::hid_device_info> hid_group;
auto unique_id = dev.front().unique_id;
for (auto&& hid : hids)
{
if (hid.unique_id == unique_id || hid.unique_id == "*")
hid_group.push_back(hid);
if (hid.unique_id != "")
{
std::stringstream(hid.vid) >> std::hex >> vid;
std::stringstream(hid.pid) >> std::hex >> pid;
auto&& backend = ctx->get_backend();
auto device_serial = backend.get_device_serial(vid, pid, unique_id);

if ((hid.unique_id == unique_id) || // Linux check
((hid.unique_id == "*") && (hid.serial_number == device_serial))) // Windows check
{
hid_group.push_back(hid);
}
}
}
results.push_back(std::make_pair(dev, hid_group));
}
Expand Down
1 change: 1 addition & 0 deletions src/context.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -176,6 +176,7 @@ namespace librealsense
// Helper functions for device list manipulation:
std::vector<platform::uvc_device_info> filter_by_product(const std::vector<platform::uvc_device_info>& devices, const std::set<uint16_t>& pid_list);
std::vector<std::pair<std::vector<platform::uvc_device_info>, std::vector<platform::hid_device_info>>> group_devices_and_hids_by_unique_id(
std::shared_ptr<context> ctx,
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I thought the serial num can be retrieved and assigned during platform::hid_device_info initialization, so passing ctx could be avoided

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

the serial number is indeed retrieved and assigned during platform::hid_device_info in foreach_hid_device function
But we need to compare it to the UVC device serial number, and to retrieve it we need the ctx because foreach_uvc_device
can't retrieve serial number when using MFEnumDeviceSources

const std::vector<std::vector<platform::uvc_device_info>>& devices,
const std::vector<platform::hid_device_info>& hids);
std::vector<std::vector<platform::uvc_device_info>> group_devices_by_unique_id(const std::vector<platform::uvc_device_info>& devices);
Expand Down
3 changes: 1 addition & 2 deletions src/ds5/ds5-factory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 });

Expand Down Expand Up @@ -516,7 +515,7 @@ namespace librealsense
std::vector<std::shared_ptr<device_info>> results;

auto valid_pid = filter_by_product(group.uvc_devices, ds::rs400_sku_pid);
auto group_devices = group_devices_and_hids_by_unique_id(group_devices_by_unique_id(valid_pid), group.hid_devices);
auto group_devices = group_devices_and_hids_by_unique_id(ctx, group_devices_by_unique_id(valid_pid), group.hid_devices);

for (auto& g : group_devices)
{
Expand Down
38 changes: 26 additions & 12 deletions src/ds5/ds5-motion.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,26 +43,40 @@ namespace librealsense

lazy<std::vector<uint8_t>> _fisheye_calibration_table_raw;

// Bandwidth parameters from BOSCH BMI 055 spec. used by D435i
#ifdef _WIN32
// Bandwidth parameters from BOSCH BMI 055 spec'
std::vector<std::pair<std::string, stream_profile>> sensor_name_and_hid_profiles =
{{"gyro_3d", {RS2_STREAM_GYRO, 0, 1, 1, 200, RS2_FORMAT_MOTION_RAW}},
{"gyro_3d", {RS2_STREAM_GYRO, 0, 1, 1, 400, RS2_FORMAT_MOTION_RAW}},
{"gyro_3d", {RS2_STREAM_GYRO, 0, 1, 1, 200, RS2_FORMAT_MOTION_XYZ32F}},
{"gyro_3d", {RS2_STREAM_GYRO, 0, 1, 1, 400, RS2_FORMAT_MOTION_XYZ32F}},
{"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 63, RS2_FORMAT_MOTION_RAW}},
{"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250, RS2_FORMAT_MOTION_RAW}},
{"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 63, RS2_FORMAT_MOTION_XYZ32F}},
{"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250, RS2_FORMAT_MOTION_XYZ32F}},
{"HID Sensor Class Device: Gyroscope", { RS2_STREAM_GYRO, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F}},
{"HID Sensor Class Device: Accelerometer", { RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F}},
{"HID Sensor Class Device: Custom", { RS2_STREAM_ACCEL, 0, 1, 1, 1000, RS2_FORMAT_MOTION_XYZ32F}}};
{{ "HID Sensor Class Device: Gyroscope", {RS2_STREAM_GYRO, 0, 1, 1, 200, RS2_FORMAT_MOTION_XYZ32F}},
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a way to support MOTION_RAW ? The asymmetry may introduce issues for CI to run tests on Linux vs Win

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

decided to remove RAW from linux too

{ "HID Sensor Class Device: Gyroscope", {RS2_STREAM_GYRO, 0, 1, 1, 400, RS2_FORMAT_MOTION_XYZ32F}},
{ "HID Sensor Class Device: Accelerometer", {RS2_STREAM_ACCEL, 0, 1, 1, 63, RS2_FORMAT_MOTION_XYZ32F}},
{ "HID Sensor Class Device: Accelerometer", {RS2_STREAM_ACCEL, 0, 1, 1, 250, RS2_FORMAT_MOTION_XYZ32F}}};

// Translate frequency to SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL
std::map<rs2_stream, std::map<unsigned, unsigned>> fps_and_sampling_frequency_per_rs2_stream =
{{RS2_STREAM_ACCEL,{{63, 1000},
{250, 400}}},
{RS2_STREAM_GYRO, {{200, 500},
{400, 250}}}};

#else
// Bandwidth parameters from BOSCH BMI 055 spec'
std::vector<std::pair<std::string, stream_profile>> sensor_name_and_hid_profiles =
{{"gyro_3d", {RS2_STREAM_GYRO, 0, 1, 1, 200, RS2_FORMAT_MOTION_RAW}},
{"gyro_3d", {RS2_STREAM_GYRO, 0, 1, 1, 400, RS2_FORMAT_MOTION_RAW}},
{"gyro_3d", {RS2_STREAM_GYRO, 0, 1, 1, 200, RS2_FORMAT_MOTION_XYZ32F}},
{"gyro_3d", {RS2_STREAM_GYRO, 0, 1, 1, 400, RS2_FORMAT_MOTION_XYZ32F}},
{"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 63, RS2_FORMAT_MOTION_RAW}},
{"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250, RS2_FORMAT_MOTION_RAW}},
{"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 63, RS2_FORMAT_MOTION_XYZ32F}},
{"accel_3d", {RS2_STREAM_ACCEL, 0, 1, 1, 250, RS2_FORMAT_MOTION_XYZ32F}}};

// The frequency selector is vendor and model-specific
std::map<rs2_stream, std::map<unsigned, unsigned>> fps_and_sampling_frequency_per_rs2_stream =
{{RS2_STREAM_ACCEL, {{63, 1},
{250, 3}}},
{RS2_STREAM_GYRO, {{200, 2},
{400, 4}}}};
#endif

protected:
std::shared_ptr<stream_interface> _fisheye_stream;
Expand Down
4 changes: 2 additions & 2 deletions src/ds5/ds5-private.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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" },
Expand Down
2 changes: 1 addition & 1 deletion src/ds5/ds5-timestamp.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -166,7 +166,7 @@ namespace librealsense
{
auto timestamp = *((uint64_t*)((const uint8_t*)fo.metadata));
// The FW timestamps for HID are converted to Nanosec in Linux kernel. This may produce conflicts with MS API.
return static_cast<rs2_time_t>(timestamp) * TIMESTAMP_NSEC_TO_MSEC;
return static_cast<rs2_time_t>(timestamp) * HID_TIMESTAMP_MULTIPLIER;
}

if (!started)
Expand Down
11 changes: 10 additions & 1 deletion src/types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 //
///////////////////////////////////
Expand Down Expand Up @@ -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
Expand Down
36 changes: 24 additions & 12 deletions src/win/win-backend.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down Expand Up @@ -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 };

Expand All @@ -94,22 +94,27 @@ namespace librealsense
return result;
}

std::shared_ptr<hid_device> wmf_backend::create_hid_device(hid_device_info info) const
wmf_hid_device::wmf_hid_device(const hid_device_info& info)
{
std::shared_ptr<hid_device> result = nullptr;
bool found = false;

auto action = [&result, &info](const hid_device_info& i, CComPtr<ISensor> ptr)
{
if (info.device_path == i.device_path)
wmf_hid_device::foreach_hid_device([&](const hid_device_info& hid_dev_info, CComPtr<ISensor> sensor) {
if (hid_dev_info.unique_id == info.unique_id)
{
result = std::make_shared<wmf_hid_device>(ptr);
_connected_sensors.push_back(std::make_shared<wmf_hid_sensor>(hid_dev_info, sensor));
found = true;
}
};
});

wmf_hid_device::foreach_hid_device(action);
if (!found)
{
LOG_ERROR("hid device is no longer connected!");
}
}

if (result.get()) return result;
throw std::runtime_error("Device no longer found!");
std::shared_ptr<hid_device> wmf_backend::create_hid_device(hid_device_info info) const
{
return std::make_shared<wmf_hid_device>(info);
}

std::vector<hid_device_info> wmf_backend::query_hid_devices() const
Expand Down Expand Up @@ -335,6 +340,13 @@ namespace librealsense
{
return std::make_shared<win_event_device_watcher>(this);
}

std::string wmf_backend::get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid) const
{
std::string device_serial = "";
platform::get_device_serial(device_vid, device_pid, device_uid, device_serial);
return device_serial;
}
}
}

Expand Down
2 changes: 2 additions & 0 deletions src/win/win-backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,8 @@ namespace librealsense
std::vector<hid_device_info> query_hid_devices() const override;
virtual std::shared_ptr<time_service> create_time_service() const override;
std::shared_ptr<device_watcher> create_device_watcher() const override;
std::string get_device_serial(uint16_t device_vid, uint16_t device_pid, const std::string& device_uid) const override;

private:
std::chrono::high_resolution_clock::time_point _start_time;
};
Expand Down
Loading