Skip to content

Commit

Permalink
Merge pull request #3763 from aangerma/development
Browse files Browse the repository at this point in the history 
 Added IMU support for l500
Tracked on: RS5-3964
  • Loading branch information
@ev-mp
ev-mp authored Apr 18, 2019
2 parents cb27100 + 525449e commit bd4ea5a
Show file tree
Hide file tree
Showing 14 changed files with 308 additions and 121 deletions.
2 changes: 1 addition & 1 deletion src/context.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -333,7 +333,7 @@ namespace librealsense
std::copy(begin(ds5_devices), end(ds5_devices), std::back_inserter(list));
}

auto l500_devices = l500_info::pick_l500_devices(ctx, devices.uvc_devices, devices.usb_devices);
auto l500_devices = l500_info::pick_l500_devices(ctx, devices);
std::copy(begin(l500_devices), end(l500_devices), std::back_inserter(list));

if (mask & RS2_PRODUCT_LINE_SR300)
Expand Down
2 changes: 1 addition & 1 deletion src/ds5/ds5-motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,7 +165,7 @@ namespace librealsense
static const char* custom_sensor_fw_ver = "5.6.0.0";

auto hid_ep = std::make_shared<ds5_hid_sensor>(this, ctx->get_backend().create_hid_device(all_hid_infos.front()),
std::unique_ptr<frame_timestamp_reader>(new ds5_iio_hid_timestamp_reader()),
std::unique_ptr<frame_timestamp_reader>(new iio_hid_timestamp_reader()),
std::unique_ptr<frame_timestamp_reader>(new ds5_custom_hid_timestamp_reader()),
fps_and_sampling_frequency_per_rs2_stream,
sensor_name_and_hid_profiles);
Expand Down
74 changes: 0 additions & 74 deletions src/ds5/ds5-timestamp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@

namespace librealsense
{
static const double TIMESTAMP_USEC_TO_MSEC = 0.001;

ds5_timestamp_reader_from_metadata::ds5_timestamp_reader_from_metadata(std::unique_ptr<frame_timestamp_reader> backup_timestamp_reader)
:_backup_timestamp_reader(std::move(backup_timestamp_reader)), _has_metadata(pins), one_time_note(false)
{
Expand Down Expand Up @@ -144,78 +142,6 @@ namespace librealsense
return RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME;
}

ds5_iio_hid_timestamp_reader::ds5_iio_hid_timestamp_reader()
{
counter.resize(sensors);
reset();
}

void ds5_iio_hid_timestamp_reader::reset()
{
std::lock_guard<std::recursive_mutex> lock(_mtx);
started = false;
for (auto i = 0; i < sensors; ++i)
{
counter[i] = 0;
}
}

rs2_time_t ds5_iio_hid_timestamp_reader::get_frame_timestamp(const request_mapping& mode, const platform::frame_object& fo)
{
std::lock_guard<std::recursive_mutex> lock(_mtx);

if(has_metadata(mode, fo.metadata, fo.metadata_size))
{
// The timestamps conversions path comprise of:
// FW TS (32bit) -> USB Phy Layer (no changes) -> Host Driver TS (Extend to 64bit) -> LRS read as 64 bit
// The flow introduces discrepancy with UVC stream which timestamps aer not extended to 64 bit by host driver both for Win and v4l backends.
// In order to allow for hw timestamp-based synchronization of Depth and IMU streams the latter will be trimmed to 32 bit.
// To revert to the extended 64 bit TS uncomment the next line instead
//auto timestamp = *((uint64_t*)((const uint8_t*)fo.metadata));
auto timestamp = *((uint32_t*)((const uint8_t*)fo.metadata));

// HID timestamps are aligned to FW Default - usec units
return static_cast<rs2_time_t>(timestamp * TIMESTAMP_USEC_TO_MSEC);
}

if (!started)
{
LOG_WARNING("HID timestamp not found! please apply HID patch.");
started = true;
}

return std::chrono::duration<rs2_time_t, std::milli>(std::chrono::system_clock::now().time_since_epoch()).count();
}

bool ds5_iio_hid_timestamp_reader::has_metadata(const request_mapping& mode, const void * metadata, size_t metadata_size) const
{
if(metadata != nullptr && metadata_size > 0)
{
return true;
}
return false;
}

unsigned long long ds5_iio_hid_timestamp_reader::get_frame_counter(const request_mapping & mode, const platform::frame_object& fo) const
{
std::lock_guard<std::recursive_mutex> lock(_mtx);
if (nullptr == mode.pf) return 0; // Windows support is limited
int index = 0;
if (mode.pf->fourcc == rs_fourcc('G','Y','R','O'))
index = 1;

return ++counter[index];
}

rs2_timestamp_domain ds5_iio_hid_timestamp_reader::get_frame_timestamp_domain(const request_mapping & mode, const platform::frame_object& fo) const
{
if(has_metadata(mode ,fo.metadata, fo.metadata_size))
{
return RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK;
}
return RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME;
}

ds5_custom_hid_timestamp_reader::ds5_custom_hid_timestamp_reader()
{
counter.resize(sensors);
Expand Down
20 changes: 0 additions & 20 deletions src/ds5/ds5-timestamp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,26 +50,6 @@ namespace librealsense
rs2_timestamp_domain get_frame_timestamp_domain(const request_mapping & mode, const platform::frame_object& fo) const override;
};

class ds5_iio_hid_timestamp_reader : public frame_timestamp_reader
{
static const int sensors = 2;
bool started;
mutable std::vector<int64_t> counter;
mutable std::recursive_mutex _mtx;
public:
ds5_iio_hid_timestamp_reader();

void reset() override;

rs2_time_t get_frame_timestamp(const request_mapping& mode, const platform::frame_object& fo) override;

bool has_metadata(const request_mapping& mode, const void * metadata, size_t metadata_size) const;

unsigned long long get_frame_counter(const request_mapping & mode, const platform::frame_object& fo) const override;

rs2_timestamp_domain get_frame_timestamp_domain(const request_mapping & mode, const platform::frame_object& fo) const override;
};

class ds5_custom_hid_timestamp_reader : public frame_timestamp_reader
{
static const int sensors = 4; // TODO: implement frame-counter for each GPIO or
Expand Down
2 changes: 2 additions & 0 deletions src/l500/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,11 @@ target_sources(${LRS_TARGET}
"${CMAKE_CURRENT_LIST_DIR}/l500-depth.cpp"
"${CMAKE_CURRENT_LIST_DIR}/l500-private.cpp"
"${CMAKE_CURRENT_LIST_DIR}/l500-color.cpp"
"${CMAKE_CURRENT_LIST_DIR}/l500-motion.cpp"
"${CMAKE_CURRENT_LIST_DIR}/l500-factory.cpp"
"${CMAKE_CURRENT_LIST_DIR}/l500-depth.h"
"${CMAKE_CURRENT_LIST_DIR}/l500-private.h"
"${CMAKE_CURRENT_LIST_DIR}/l500-color.h"
"${CMAKE_CURRENT_LIST_DIR}/l500-motion.h"
"${CMAKE_CURRENT_LIST_DIR}/l500-factory.h"
)
43 changes: 28 additions & 15 deletions src/l500/l500-factory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,24 +13,27 @@
#include "metadata-parser.h"

#include "l500-factory.h"
#include "l500-private.h"
#include "l500-depth.h"
#include "l500-motion.h"
#include "l500-color.h"

namespace librealsense
{
using namespace ivcam2;

// l515
class rs515_device : public l500_depth, public l500_color
class rs515_device : public l500_depth,
public l500_color,
public l500_motion
{
public:
rs515_device(std::shared_ptr<context> ctx,
const platform::backend_device_group& group,
bool register_device_notifications)
: device(ctx, group, register_device_notifications),
l500_depth(ctx, group),
l500_color(ctx, group)
l500_color(ctx, group),
l500_motion(ctx, group)
{}

std::shared_ptr<matcher> create_matcher(const frame_holder& frame) const override;
Expand All @@ -43,7 +46,8 @@ namespace librealsense
tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 360, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, 640, 360, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_CONFIDENCE, -1, 640, 360, RS2_FORMAT_RAW8, 30, profile_tag::PROFILE_TAG_SUPERSET });

tags.push_back({ RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
return tags;
};
};
Expand Down Expand Up @@ -85,32 +89,41 @@ namespace librealsense
return std::make_shared<rs500_device>(ctx, group, register_device_notifications);
case L515_PID:
return std::make_shared<rs515_device>(ctx, group, register_device_notifications);
default:
default:
throw std::runtime_error(to_string() << "Unsupported L500 model! 0x"
<< std::hex << std::setw(4) << std::setfill('0') << (int)pid);
}
}

std::vector<std::shared_ptr<device_info>> l500_info::pick_l500_devices(
std::shared_ptr<context> ctx,
std::vector<platform::uvc_device_info>& uvc,
std::vector<platform::usb_device_info>& usb)
platform::backend_device_group& group)
{
std::vector<platform::uvc_device_info> chosen;
std::vector<std::shared_ptr<device_info>> results;

auto correct_pid = filter_by_product(uvc, { L500_PID, L515_PID });
auto group_devices = group_devices_by_unique_id(correct_pid);
for (auto& group : group_devices)
auto correct_pid = filter_by_product(group.uvc_devices, { L500_PID, L515_PID });
auto group_devices = group_devices_and_hids_by_unique_id(group_devices_by_unique_id(correct_pid), group.hid_devices);
for (auto& g : group_devices)
{
if (!group.empty() && mi_present(group, 0))
if (!g.first.empty() && mi_present(g.first, 0))
{
auto depth = get_mi(group, 0);
auto depth = get_mi(g.first, 0);
platform::usb_device_info hwm;
if (!ivcam2::try_fetch_usb_device(usb, depth, hwm))

if (!ivcam2::try_fetch_usb_device(group.usb_devices, depth, hwm))
LOG_WARNING("try_fetch_usb_device(...) failed.");

auto info = std::make_shared<l500_info>(ctx, group, hwm);

#if defined( RS2_USE_WMF_BACKEND) || defined(RS2_USE_V4L2_BACKEND) //currently hid backend implemented only for windows and linux.
if (g.second.size() < 2)
{
LOG_WARNING("Skipping L500. " << g.second.size() << " HID devices were recognized, at least 2 required");
continue;
}

#endif
auto info = std::make_shared<l500_info>(ctx, g.first, hwm, g.second);
chosen.push_back(depth);
results.push_back(info);
}
Expand All @@ -120,7 +133,7 @@ namespace librealsense
}
}

trim_device_list(uvc, chosen);
trim_device_list(group.uvc_devices, chosen);

return results;
}
Expand Down
14 changes: 7 additions & 7 deletions src/l500/l500-factory.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
#pragma once

#include "l500-private.h"
#include "l500-depth.h"

namespace librealsense
{
Expand All @@ -15,28 +14,29 @@ namespace librealsense

l500_info(std::shared_ptr<context> ctx,
std::vector<platform::uvc_device_info> depth,
platform::usb_device_info hwm)
platform::usb_device_info hwm,
std::vector<platform::hid_device_info> hid)
: device_info(ctx),
_depth(std::move(depth)),
_hwm(std::move(hwm))
_hwm(std::move(hwm)),
_hid(std::move(hid))
{}

static std::vector<std::shared_ptr<device_info>> pick_l500_devices(
std::shared_ptr<context> ctx,
std::vector<platform::uvc_device_info>& platform,
std::vector<platform::usb_device_info>& usb);
platform::backend_device_group& group);

platform::backend_device_group get_device_data() const override
{
std::vector<platform::usb_device_info> usb_devices;
if (_hwm.id != "")
usb_devices = { _hwm };
return platform::backend_device_group({ _depth }, usb_devices);
return platform::backend_device_group({ _depth }, usb_devices, { _hid });
}

private:
std::vector<platform::uvc_device_info> _depth;

platform::usb_device_info _hwm;
std::vector<platform::hid_device_info> _hid;
};
}
Loading

0 comments on commit bd4ea5a

Please sign in to comment.