l500-motion.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2019 Intel Corporation. All Rights Reserved.
3 
4 #include "l500-color.h"
5 #include "l500-private.h"
6 #include "l500-motion.h"
7 #include "../backend.h"
9 
10 namespace librealsense
11 {
12 
13 #ifdef _WIN32
14  std::vector<std::pair<std::string, stream_profile>> l500_sensor_name_and_hid_profiles =
15  {{ "HID Sensor Class Device: Gyroscope", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, 0, 1, 1, 100 }},
16  { "HID Sensor Class Device: Gyroscope", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, 0, 1, 1, 200 }},
17  { "HID Sensor Class Device: Gyroscope", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, 0, 1, 1, 400 }},
18  { "HID Sensor Class Device: Accelerometer", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, 0, 1, 1, 100 }},
19  { "HID Sensor Class Device: Accelerometer", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, 0, 1, 1, 200 }},
20  { "HID Sensor Class Device: Accelerometer", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, 0, 1, 1, 400 }}};
21 
22  // Translate frequency to SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL
23  std::map<rs2_stream, std::map<unsigned, unsigned>> l500_fps_and_sampling_frequency_per_rs2_stream =
24  { {RS2_STREAM_ACCEL,{{100, 1000},
25  {200, 500},
26  {400, 250}}},
27  {RS2_STREAM_GYRO, {{100, 1000},
28  {200, 500},
29  {400, 250}}}};
30 
31 #else
32  std::vector<std::pair<std::string, stream_profile>> l500_sensor_name_and_hid_profiles =
33  {{ "gyro_3d", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, 0, 1, 1, 100 }},
34  { "gyro_3d", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, 0, 1, 1, 200 }},
35  { "gyro_3d", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, 0, 1, 1, 400 }},
36  { "accel_3d", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, 0, 1, 1, 100 }},
37  { "accel_3d", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, 0, 1, 1, 200 }},
38  { "accel_3d", { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, 0, 1, 1, 400 }}};
39 
40  // The frequency selector is vendor and model-specific
41  std::map<rs2_stream, std::map<unsigned, unsigned>> l500_fps_and_sampling_frequency_per_rs2_stream =
42  { {RS2_STREAM_ACCEL,{{100, 1},
43  {200, 2},
44  {400, 4}}},
45  {RS2_STREAM_GYRO, {{100, 1},
46  {200, 2},
47  {400, 4}}} };
48 #endif
49 
51  {
52  public:
54  std::shared_ptr<sensor_base> sensor,
55  device* device,
56  l500_motion* owner)
57  : synthetic_sensor(name, sensor, device), _owner(owner)
58  {
59  }
60 
62  {
63  return _owner->get_motion_intrinsics(stream);
64  }
65 
67  {
70 
71  for (auto p : results)
72  {
73  // Register stream types
74  if (p->get_stream_type() == RS2_STREAM_ACCEL)
76  if (p->get_stream_type() == RS2_STREAM_GYRO)
78 
79  //set motion intrinsics
80  if (p->get_stream_type() == RS2_STREAM_ACCEL || p->get_stream_type() == RS2_STREAM_GYRO)
81  {
82  auto motion = dynamic_cast<motion_stream_profile_interface*>(p.get());
83  assert(motion); //Expecting to succeed for motion stream (since we are under the "if")
84  auto st = p->get_stream_type();
85  motion->set_intrinsics([this, st]() { return get_motion_intrinsics(st); });
86  }
87  }
88 
89  return results;
90  }
91 
92  private:
94  };
95 
96  std::shared_ptr<synthetic_sensor> l500_motion::create_hid_device(std::shared_ptr<context> ctx, const std::vector<platform::hid_device_info>& all_hid_infos)
97  {
98  if (all_hid_infos.empty())
99  {
100  LOG_WARNING("No HID info provided, IMU is disabled");
101  return nullptr;
102  }
103 
104  std::unique_ptr<frame_timestamp_reader> iio_hid_ts_reader(new iio_hid_timestamp_reader());
105  std::unique_ptr<frame_timestamp_reader> custom_hid_ts_reader(new iio_hid_timestamp_reader());
106  auto enable_global_time_option = std::shared_ptr<global_time_option>(new global_time_option());
107  auto raw_hid_ep = std::make_shared<hid_sensor>(ctx->get_backend().create_hid_device(all_hid_infos.front()),
108  std::unique_ptr<frame_timestamp_reader>(new global_timestamp_reader(std::move(iio_hid_ts_reader), _tf_keeper, enable_global_time_option)),
109  std::unique_ptr<frame_timestamp_reader>(new global_timestamp_reader(std::move(custom_hid_ts_reader), _tf_keeper, enable_global_time_option)),
110  l500_fps_and_sampling_frequency_per_rs2_stream,
111  l500_sensor_name_and_hid_profiles,
112  this);
113 
114  auto hid_ep = std::make_shared<l500_hid_sensor>("Motion Module", raw_hid_ep, this, this);
115 
116  hid_ep->register_option(RS2_OPTION_GLOBAL_TIME_ENABLED, enable_global_time_option);
117  hid_ep->get_option(RS2_OPTION_GLOBAL_TIME_ENABLED).set(0);
118  hid_ep->register_option(RS2_OPTION_GLOBAL_TIME_ENABLED, enable_global_time_option);
119 
120  // register pre-processing
121  std::shared_ptr<enable_motion_correction> mm_correct_opt = nullptr;
122 
123  // Motion intrinsic calibration presents is a prerequisite for motion correction.
124  try
125  {
126  // L515 motion correction with IMU supported from FW version 01.04.01.00
127  if (_fw_version >= firmware_version("1.4.1.0") && _mm_calib)
128  {
129  mm_correct_opt = std::make_shared<enable_motion_correction>(hid_ep.get(), option_range{ 0, 1, 1, 1 });
130  hid_ep->register_option(RS2_OPTION_ENABLE_MOTION_CORRECTION, mm_correct_opt);
131  }
132  }
133  catch (...) {}
134 
135  hid_ep->register_processing_block(
137  { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL} },
138  [&, mm_correct_opt]() { return std::make_shared<acceleration_transform>(_mm_calib, mm_correct_opt); }
139  );
140 
141  hid_ep->register_processing_block(
143  { {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} },
144  [&, mm_correct_opt]() { return std::make_shared<gyroscope_transform>(_mm_calib, mm_correct_opt); }
145  );
146  return hid_ep;
147  }
148 
150  :device(ctx, group), l500_device(ctx, group),
151  _accel_stream(new stream(RS2_STREAM_ACCEL)),
152  _gyro_stream(new stream(RS2_STREAM_GYRO))
153  {
154  std::vector<platform::hid_device_info> hid_infos = group.hid_devices;
155 
156  if (!hid_infos.empty())
157  {
158  // product id
159  _pid = static_cast<uint16_t>(strtoul(hid_infos.front().pid.data(), nullptr, 16));
160 
161  // motion correction
162  _mm_calib = std::make_shared<mm_calib_handler>(_hw_monitor, _pid);
163  _accel_intrinsic = std::make_shared<lazy<ds::imu_intrinsic>>([this]() { return _mm_calib->get_intrinsic(RS2_STREAM_ACCEL); });
164  _gyro_intrinsic = std::make_shared<lazy<ds::imu_intrinsic>>([this]() { return _mm_calib->get_intrinsic(RS2_STREAM_GYRO); });
165 
166  // use predefined extrinsics
167  _depth_to_imu = std::make_shared<lazy<rs2_extrinsics>>([this]() { return _mm_calib->get_extrinsic(RS2_STREAM_ACCEL); });
168  }
169 
170  // Make sure all MM streams are positioned with the same extrinsics
175 
176  auto hid_ep = create_hid_device(ctx, group.hid_devices);
177  if (hid_ep)
178  {
180  // HID metadata attributes
181  hid_ep->get_raw_sensor()->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_hid_header_parser(&platform::hid_header::timestamp));
182  }
183  }
184 
185  std::vector<tagged_profile> l500_motion::get_profiles_tags() const
186  {
187  std::vector<tagged_profile> tags;
188 
191  return tags;
192  }
193 
195  {
196  if (stream == RS2_STREAM_ACCEL)
198 
199  if (stream == RS2_STREAM_GYRO)
201 
202  throw std::runtime_error(to_string() << "Motion Intrinsics unknown for stream " << rs2_stream_to_string(stream) << "!");
203 
204  }
205 }
std::shared_ptr< hw_monitor > _hw_monitor
Definition: l500-device.h:91
static const textual_icon lock
Definition: model-views.h:218
std::shared_ptr< lazy< rs2_extrinsics > > _depth_to_imu
Definition: l500-motion.h:37
GLuint const GLchar * name
stream_profiles init_stream_profiles() override
Definition: l500-motion.cpp:66
std::vector< tagged_profile > get_profiles_tags() const override
GLfloat GLfloat p
Definition: glext.h:12687
#define LOG_WARNING(...)
Definition: src/types.h:241
virtual stream_profiles init_stream_profiles() override
Definition: sensor.cpp:1225
std::shared_ptr< lazy< ds::imu_intrinsic > > _accel_intrinsic
Definition: l500-motion.h:35
void register_same_extrinsics(const stream_interface &from, const stream_interface &to)
Definition: environment.cpp:23
unsigned short uint16_t
Definition: stdint.h:79
GLsizei const GLchar *const * string
std::shared_ptr< stream_interface > _depth_stream
Definition: l500-device.h:98
l500_hid_sensor(std::string name, std::shared_ptr< sensor_base > sensor, device *device, l500_motion *owner)
Definition: l500-motion.cpp:53
std::shared_ptr< stream_interface > _gyro_stream
Definition: l500-motion.h:43
std::vector< std::pair< std::string, stream_profile > > l500_sensor_name_and_hid_profiles
Definition: l500-motion.cpp:32
void register_extrinsics(const stream_interface &from, const stream_interface &to, std::weak_ptr< lazy< rs2_extrinsics >> extr)
Definition: environment.cpp:28
std::vector< hid_device_info > hid_devices
Definition: backend.h:527
std::map< rs2_stream, std::map< unsigned, unsigned > > l500_fps_and_sampling_frequency_per_rs2_stream
Definition: l500-motion.cpp:41
std::shared_ptr< lazy< ds::imu_intrinsic > > _gyro_intrinsic
Definition: l500-motion.h:36
GLboolean GLuint group
Definition: glext.h:5688
std::shared_ptr< mm_calib_handler > _mm_calib
Definition: l500-motion.h:34
std::shared_ptr< synthetic_sensor > create_hid_device(std::shared_ptr< context > ctx, const std::vector< platform::hid_device_info > &all_hid_infos)
Definition: l500-motion.cpp:96
const l500_motion * _owner
Definition: l500-motion.cpp:93
std::vector< std::shared_ptr< stream_profile_interface >> stream_profiles
Definition: streaming.h:165
rs2_stream
Streams are different types of data provided by RealSense devices.
Definition: rs_sensor.h:42
static environment & get_instance()
int add_sensor(const std::shared_ptr< sensor_interface > &sensor_base)
Definition: device.cpp:163
extrinsics_graph & get_extrinsics_graph()
const char * rs2_stream_to_string(rs2_stream stream)
Definition: rs.cpp:1262
void assign_stream(const std::shared_ptr< stream_interface > &stream, std::shared_ptr< stream_profile_interface > target) const
Definition: sensor.cpp:184
l500_motion(std::shared_ptr< context > ctx, const platform::backend_device_group &group)
typename::boost::move_detail::remove_reference< T >::type && move(T &&t) BOOST_NOEXCEPT
void register_stream_to_extrinsic_group(const stream_interface &stream, uint32_t groupd_index)
Definition: device.cpp:246
rs2_motion_device_intrinsic get_motion_intrinsics(rs2_stream stream) const
Motion device intrinsics: scale, bias, and variances.
Definition: rs_types.h:103
std::shared_ptr< stream_interface > _accel_stream
Definition: l500-motion.h:42
std::shared_ptr< md_attribute_parser_base > make_hid_header_parser(Attribute St::*attribute, attrib_modifyer mod=nullptr)
A utility function to create HID metadata header parser.
rs2_motion_device_intrinsic get_motion_intrinsics(rs2_stream stream) const
Definition: l500-motion.cpp:61
optional_value< uint8_t > _motion_module_device_idx
Definition: l500-motion.h:32
std::string to_string(T value)
rs2_motion_device_intrinsic create_motion_intrinsics(imu_intrinsic data)
Definition: ds5-private.h:604


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:21