26 #include "../firmware_logger_device.h" 39 bool register_device_notifications)
40 :
device(ctx, group, register_device_notifications),
52 std::vector<tagged_profile>
tags;
78 bool register_device_notifications)
79 :
device(ctx, group, register_device_notifications),
90 std::vector<tagged_profile>
tags;
109 if (
auto vid_a = dynamic_cast<const video_stream_profile_interface*>(a))
111 for (
auto request : others)
130 bool register_device_notifications)
131 :
device(ctx, group, register_device_notifications),
144 std::vector<tagged_profile>
tags;
169 bool register_device_notifications)
170 :
device(ctx, group, register_device_notifications),
184 std::vector<tagged_profile>
tags;
210 bool register_device_notifications)
211 :
device(ctx, group, register_device_notifications),
224 std::vector<tagged_profile>
tags;
241 if (
auto vid_a = dynamic_cast<const video_stream_profile_interface*>(a))
243 for (
auto request : others)
264 bool register_device_notifications)
265 :
device(ctx, group, register_device_notifications),
279 std::vector<tagged_profile>
tags;
298 if (
auto vid_a = dynamic_cast<const video_stream_profile_interface*>(a))
300 for (
auto request : others)
318 bool register_device_notifications)
319 :
device(ctx, group, register_device_notifications),
331 std::vector<tagged_profile>
tags;
366 bool register_device_notifications)
367 :
device(ctx, group, register_device_notifications),
378 std::vector<tagged_profile>
tags;
404 bool register_device_notifications)
405 :
device(ctx, group, register_device_notifications),
417 std::vector<tagged_profile>
tags;
443 bool register_device_notifications)
444 :
device(ctx, group, register_device_notifications),
456 std::vector<tagged_profile>
tags;
488 bool register_device_notifications)
489 :
device(ctx, group, register_device_notifications),
502 std::vector<tagged_profile>
tags;
539 bool register_device_notifications)
540 :
device(ctx, group, register_device_notifications),
553 std::vector<tagged_profile>
tags;
581 bool register_device_notifications)
582 :
device(ctx, group, register_device_notifications),
596 std::vector<tagged_profile>
tags;
624 bool register_device_notifications)
625 :
device(ctx, group, register_device_notifications),
635 check_and_restore_rgb_stream_extrinsic();
642 std::vector<tagged_profile>
tags;
650 int fps = usb3mode ? 30 : 15;
665 for(
auto iter = 0,
rec =0; iter < 2; iter++,
rec++)
667 std::vector<byte> cal;
677 if (!is_rgb_extrinsic_valid(cal) && !
rec)
679 restore_rgb_extrinsic();
691 auto table = ds::check_calib<ds::rgb_calibration_table>(raw_data);
698 for (
auto i = 0;
i < 3;
i++)
701 if (!std::isfinite(trans_vector[
i]))
703 LOG_WARNING(
"RGB extrinsic - translation is corrupted: " << trans_vector);
707 if (std::fabs(trans_vector[i]) > std::numeric_limits<float>::epsilon())
713 LOG_WARNING(
"RGB extrinsic - invalid (zero) translation: " << trans_vector);
720 for (
auto i = 0;
i < 3;
i++)
722 for (
auto j = 0;
j < 3;
j++)
725 if (!std::isfinite(rect_rot_mat(
i,
j)))
727 LOG_DEBUG(
"RGB extrinsic - rotation matrix corrupted:\n" << rect_rot_mat);
731 if (std::fabs(rect_rot_mat(
i,
j)) > std::numeric_limits<float>::epsilon())
736 bool res = (num_found >= 3);
738 LOG_DEBUG(
"RGB extrinsic - rotation matrix invalid:\n" << rect_rot_mat);
745 <<
"Version: " <<std::setfill(
'0') << std::setw(4) << std::hex <<
table->header.version
746 <<
", type " << std::dec <<
table->header.table_type <<
", size " <<
table->header.table_size);
767 throw std::runtime_error(
to_string() <<
"Device memory read failed. max size: " 769 <<
", requested: " <<
int(size));
789 LOG_WARNING(
"invalid RGB extrinsic was identified, recovery routine was invoked");
792 if ((res = is_rgb_extrinsic_valid(read_rgb_gold())))
794 restore_calib_factory_settings();
800 const uint32_t gold_address = 0x17c49c;
801 const uint16_t bytes_to_read = 0x100;
802 auto alt_calib = read_sector(gold_address, bytes_to_read);
803 if ((res = is_rgb_extrinsic_valid(alt_calib)))
804 assign_rgb_stream_extrinsic(alt_calib);
815 LOG_WARNING(
"RGB stream extrinsic successfully recovered");
822 LOG_ERROR(
"RGB Extrinsic recovery routine failed");
828 LOG_ERROR(
"RGB Extrinsic recovery routine failed");
843 bool register_device_notifications)
844 :
device(ctx, group, register_device_notifications),
859 std::vector<tagged_profile>
tags;
863 int width = usb3mode ? 1280 : 640;
864 int height = usb3mode ? 720 : 480;
865 int fps = usb3mode ? 30 : 15;
884 bool register_device_notifications)
885 :
device(ctx, group, register_device_notifications),
897 std::vector<tagged_profile>
tags;
914 bool register_device_notifications)
915 :
device(ctx, group, register_device_notifications),
928 std::vector<tagged_profile>
tags;
936 int fps = usb3mode ? 30 : 15;
959 bool register_device_notifications)
960 :
device(ctx, group, register_device_notifications),
977 std::vector<tagged_profile>
tags;
985 int fps = usb3mode ? 30 : 15;
1002 bool register_device_notifications)
const 1006 if (_depth.size() == 0)
throw std::runtime_error(
"Depth Camera not found!");
1007 auto pid = _depth.front().pid;
1013 return std::make_shared<rs400_device>(
ctx,
group, register_device_notifications);
1015 return std::make_shared<rs405u_device>(
ctx,
group, register_device_notifications);
1018 return std::make_shared<rs410_device>(
ctx,
group, register_device_notifications);
1020 return std::make_shared<rs415_device>(
ctx,
group, register_device_notifications);
1022 return std::make_shared<rs416_device>(
ctx,
group, register_device_notifications);
1024 return std::make_shared<rs416_rgb_device>(
ctx,
group, register_device_notifications);
1026 return std::make_shared<rs420_device>(
ctx,
group, register_device_notifications);
1028 return std::make_shared<rs420_mm_device>(
ctx,
group, register_device_notifications);
1030 return std::make_shared<rs430_device>(
ctx,
group, register_device_notifications);
1032 return std::make_shared<rs430i_device>(
ctx,
group, register_device_notifications);
1034 return std::make_shared<rs430_mm_device>(
ctx,
group, register_device_notifications);
1036 return std::make_shared<rs430_rgb_mm_device>(
ctx,
group, register_device_notifications);
1038 return std::make_shared<rs435_device>(
ctx,
group, register_device_notifications);
1040 return std::make_shared<rs435i_device>(
ctx,
group, register_device_notifications);
1042 return std::make_shared<rs465_device>(
ctx,
group, register_device_notifications);
1044 return std::make_shared<rs410_device>(
ctx,
group, register_device_notifications);
1046 return std::make_shared<rs400_imu_device>(
ctx,
group, register_device_notifications);
1048 return std::make_shared<rs405_device>(
ctx,
group, register_device_notifications);
1050 return std::make_shared<rs455_device>(
ctx,
group, register_device_notifications);
1052 throw std::runtime_error(
to_string() <<
"Unsupported RS400 model! 0x" 1053 << std::hex << std::setw(4) << std::setfill(
'0') <<(
int)pid);
1058 std::shared_ptr<context>
ctx,
1061 std::vector<platform::uvc_device_info> chosen;
1062 std::vector<std::shared_ptr<device_info>> results;
1067 for (
auto&
g : group_devices)
1070 auto& hids =
g.second;
1076 bool is_device_multisensor =
false;
1079 if (is_pid_of_multisensor_device(uvc.pid))
1080 is_device_multisensor =
true;
1083 if(is_device_multisensor)
1085 all_sensors_present = all_sensors_present &&
mi_present(devices, 3);
1089 #if !defined(__APPLE__) // Not supported by macos 1091 bool is_device_hid_sensor =
false;
1092 for (
auto&& uvc : devices)
1094 if (is_pid_of_hid_sensor_device(uvc.pid))
1095 is_device_hid_sensor =
true;
1100 if (is_device_hid_sensor)
1102 all_sensors_present &= (hids.size() >= 2);
1106 if (!devices.empty() && all_sensors_present)
1110 std::vector<platform::usb_device_info> hwm_devices;
1113 hwm_devices.push_back(hwm);
1117 LOG_DEBUG(
"try_fetch_usb_device(...) failed.");
1120 auto info = std::make_shared<ds5_info>(
ctx,
devices, hwm_devices, hids);
1121 chosen.insert(chosen.end(), devices.begin(), devices.end());
1122 results.push_back(
info);
1139 return std::make_shared<timestamp_composite_matcher>(matchers);
1170 std::vector<stream_interface*> mm_streams = { _accel_stream.get(), _gyro_stream.get() };
1171 streams.insert(streams.end(), mm_streams.begin(), mm_streams.end());
1191 std::vector<stream_interface*> mm_streams = {
1192 _fisheye_stream.get(),
1193 _accel_stream.get(),
1196 streams.insert(streams.end(), mm_streams.begin(), mm_streams.end());
1216 std::vector<stream_interface*> mm_streams = {
1217 _fisheye_stream.get(),
1218 _accel_stream.get(),
1221 streams.insert(streams.end(), mm_streams.begin(), mm_streams.end());
1235 std::vector<stream_interface*> mm_streams = {
1236 _fisheye_stream.get(),
1237 _accel_stream.get(),
1240 streams.insert(streams.end(), mm_streams.begin(), mm_streams.end());
1248 std::vector<stream_interface*> mm_streams = { _accel_stream.get(), _gyro_stream.get() };
1249 streams.insert(streams.end(), mm_streams.begin(), mm_streams.end());
1257 std::vector<stream_interface*> mm_streams = { _accel_stream.get(), _gyro_stream.get()};
1258 streams.insert(streams.end(), mm_streams.begin(), mm_streams.end());
1265 std::vector<stream_interface*> mm_streams = { _accel_stream.get(), _gyro_stream.get()};
1278 std::vector<stream_interface*> mm_streams = { _accel_stream.get(), _gyro_stream.get()};
1279 streams.insert(streams.end(), mm_streams.begin(), mm_streams.end());
rs435_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
std::vector< tagged_profile > get_profiles_tags() const override
rs435i_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
firmware_version _fw_version
std::shared_ptr< hw_monitor > _hw_monitor
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< tagged_profile > get_profiles_tags() const override
std::vector< tagged_profile > get_profiles_tags() const override
std::vector< tagged_profile > get_profiles_tags() const override
bool compress_while_record() const override
platform::usb_spec get_usb_spec() const
void trim_device_list(std::vector< platform::usb_device_info > &devices, const std::vector< platform::usb_device_info > &chosen)
bool contradicts(const stream_profile_interface *a, const std::vector< stream_profile > &others) const override
std::shared_ptr< stream_interface > _color_stream
std::vector< tagged_profile > get_profiles_tags() const override
std::vector< tagged_profile > get_profiles_tags() const override
rs455_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
std::vector< tagged_profile > get_profiles_tags() const override
std::vector< tagged_profile > get_profiles_tags() const override
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
std::vector< tagged_profile > get_profiles_tags() const override
std::shared_ptr< hw_monitor > _hw_monitor
std::vector< tagged_profile > get_profiles_tags() const override
static const std::set< std::uint16_t > hid_sensors_pid
const uint16_t RS435_RGB_PID
synthetic_sensor & get_depth_sensor()
rs430_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
rs416_device(std::shared_ptr< context > ctx, const platform::backend_device_group &group, bool register_device_notifications)
std::vector< byte > read_rgb_gold() const
bool contradicts(const stream_profile_interface *a, const std::vector< stream_profile > &others) const override
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
rs405_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
std::shared_ptr< lazy< rs2_extrinsics > > _color_extrinsic
std::vector< tagged_profile > get_profiles_tags() const override
std::vector< tagged_profile > get_profiles_tags() const override
GLboolean GLboolean GLboolean GLboolean a
rs415_device(std::shared_ptr< context > ctx, const platform::backend_device_group &group, bool register_device_notifications)
const uint16_t RS400_IMU_PID
def info(name, value, persistent=False)
const uint16_t RS416_RGB_PID
std::shared_ptr< ds5_thermal_monitor > _thermal_monitor
bool contradicts(const stream_profile_interface *a, const std::vector< stream_profile > &others) const override
std::vector< byte > read_sector(const uint32_t address, const uint16_t size) const
const uint16_t RS405U_PID
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
bool compress_while_record() const override
void register_extrinsics(const stream_interface &from, const stream_interface &to, std::weak_ptr< lazy< rs2_extrinsics >> extr)
static const uint16_t HW_MONITOR_COMMAND_SIZE
std::vector< std::pair< std::vector< platform::uvc_device_info >, std::vector< platform::hid_device_info > > > group_devices_and_hids_by_unique_id(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)
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
rs416_rgb_device(std::shared_ptr< context > ctx, const platform::backend_device_group &group, bool register_device_notifications)
bool try_fetch_usb_device(std::vector< platform::usb_device_info > &devices, const platform::uvc_device_info &info, platform::usb_device_info &result)
static const std::set< std::uint16_t > rs400_sku_pid
rs420_mm_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
std::vector< byte > restore_calib_factory_settings() const
void check_and_restore_rgb_stream_extrinsic()
std::vector< tagged_profile > get_profiles_tags() const override
bool is_rgb_extrinsic_valid(const std::vector< uint8_t > &raw_data) const
rs410_device(std::shared_ptr< context > ctx, const platform::backend_device_group &group, bool register_device_notifications)
GLint GLsizei GLsizei height
rs430i_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
std::vector< uint8_t > data
std::shared_ptr< stream_interface > _depth_stream
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
rs430_mm_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
command get_flash_logs_command() const
const uint16_t RS_USB2_PID
lazy< std::vector< uint8_t > > _color_calib_table_raw
rs405u_device(std::shared_ptr< context > ctx, const platform::backend_device_group &group, bool register_device_notifications)
std::shared_ptr< stream_interface > _left_ir_stream
std::vector< tagged_profile > get_profiles_tags() const override
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
command get_firmware_logs_command() const
rs465_device(std::shared_ptr< context > ctx, const platform::backend_device_group &group, bool register_device_notifications)
std::vector< tagged_profile > get_profiles_tags() const override
static environment & get_instance()
extrinsics_graph & get_extrinsics_graph()
std::shared_ptr< device_interface > create(std::shared_ptr< context > ctx, bool register_device_notifications) const override
rs420_device(std::shared_ptr< context > ctx, const platform::backend_device_group &group, bool register_device_notifications)
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
GLenum GLenum GLsizei void * table
GLuint GLfloat GLfloat GLfloat x1
rs430_rgb_mm_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
const uint16_t RS420_MM_PID
const uint16_t RS435I_PID
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
rs400_device(std::shared_ptr< context > ctx, const platform::backend_device_group &group, bool register_device_notifications)
static std::vector< std::shared_ptr< device_info > > pick_ds5_devices(std::shared_ptr< context > ctx, platform::backend_device_group &gproup)
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
bool compress_while_record() const override
GLuint GLuint64EXT address
rs400_imu_device(std::shared_ptr< context > ctx, const platform::backend_device_group group, bool register_device_notifications)
const uint16_t RS430_MM_RGB_PID
void restore_rgb_extrinsic(void)
std::shared_ptr< matcher > create_composite_matcher(std::vector< std::shared_ptr< matcher >> matchers)
virtual uint32_t get_framerate() const =0
std::vector< tagged_profile > get_profiles_tags() const override
void assign_rgb_stream_extrinsic(const std::vector< byte > &calib)
static const std::set< std::uint16_t > multi_sensors_pid
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
std::shared_ptr< matcher > create_matcher(const frame_holder &frame) const override
std::vector< tagged_profile > get_profiles_tags() const override
std::vector< tagged_profile > get_profiles_tags() const override
std::shared_ptr< stream_interface > _right_ir_stream
const uint16_t RS430I_PID
const uint16_t RS430_MM_PID
bool mi_present(const std::vector< platform::uvc_device_info > &devices, uint32_t mi)
std::string to_string(T value)
static std::shared_ptr< matcher > create(rs2_matchers matcher, std::vector< stream_interface * > profiles)