From cfd2b0682ec82ba65256571ce7f1103fa8cd98fb Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Wed, 26 Jul 2023 11:51:39 +0300 Subject: [PATCH 1/7] adding d500 folder - no device using it for now --- include/librealsense2/h/rs_option.h | 1 + include/librealsense2/h/rs_sensor.h | 2 + src/ds/CMakeLists.txt | 1 + src/ds/d400/d400-options.cpp | 2 +- src/ds/d500/CMakeLists.txt | 21 + src/ds/d500/d500-active.cpp | 33 + src/ds/d500/d500-active.h | 19 + src/ds/d500/d500-color.cpp | 176 + src/ds/d500/d500-color.h | 79 + src/ds/d500/d500-device.cpp | 838 + src/ds/d500/d500-device.h | 114 + src/ds/d500/d500-factory.cpp | 132 + src/ds/d500/d500-factory.h | 35 + src/ds/d500/d500-motion.cpp | 103 + src/ds/d500/d500-motion.h | 41 + src/ds/d500/d500-options.cpp | 50 + src/ds/d500/d500-options.h | 74 + src/ds/d500/d500-private.cpp | 251 + src/ds/d500/d500-private.h | 185 + src/ds/d500/hw_monitor_extended_buffers.cpp | 133 + src/ds/d500/hw_monitor_extended_buffers.h | 54 + src/ds/ds-private.h | 2 +- src/image.h | 13 +- src/proc/CMakeLists.txt | 2 + src/proc/color-formats-converter.cpp | 424 + src/proc/color-formats-converter.h | 12 + src/proc/y16i-to-y10msby10msb.cpp | 48 + src/proc/y16i-to-y10msby10msb.h | 22 + src/to-string.cpp | 3 + third-party/json.hpp | 24619 ++++++++++++++++ .../realsense/librealsense/StreamFormat.java | 4 +- wrappers/tensorflow/.idea/.name | 1 + 32 files changed, 27486 insertions(+), 8 deletions(-) create mode 100644 src/ds/d500/CMakeLists.txt create mode 100644 src/ds/d500/d500-active.cpp create mode 100644 src/ds/d500/d500-active.h create mode 100644 src/ds/d500/d500-color.cpp create mode 100644 src/ds/d500/d500-color.h create mode 100644 src/ds/d500/d500-device.cpp create mode 100644 src/ds/d500/d500-device.h create mode 100644 src/ds/d500/d500-factory.cpp create mode 100644 src/ds/d500/d500-factory.h create mode 100644 src/ds/d500/d500-motion.cpp create mode 100644 src/ds/d500/d500-motion.h create mode 100644 src/ds/d500/d500-options.cpp create mode 100644 src/ds/d500/d500-options.h create mode 100644 src/ds/d500/d500-private.cpp create mode 100644 src/ds/d500/d500-private.h create mode 100644 src/ds/d500/hw_monitor_extended_buffers.cpp create mode 100644 src/ds/d500/hw_monitor_extended_buffers.h create mode 100644 src/proc/y16i-to-y10msby10msb.cpp create mode 100644 src/proc/y16i-to-y10msby10msb.h create mode 100644 third-party/json.hpp create mode 100644 wrappers/tensorflow/.idea/.name diff --git a/include/librealsense2/h/rs_option.h b/include/librealsense2/h/rs_option.h index 2855b332d2..ba1e39873b 100644 --- a/include/librealsense2/h/rs_option.h +++ b/include/librealsense2/h/rs_option.h @@ -117,6 +117,7 @@ extern "C" { RS2_OPTION_AUTO_GAIN_LIMIT_TOGGLE, /**< Enable / disable color image auto-gain*/ RS2_OPTION_EMITTER_FREQUENCY, /**< Select emitter (laser projector) frequency, see rs2_emitter_frequency for values */ RS2_OPTION_DEPTH_AUTO_EXPOSURE_MODE, /**< Select depth sensor auto exposure mode see rs2_depth_auto_exposure_mode for values */ + RS2_OPTION_LEFT_IR_TEMPERATURE, /**< Temperature of the Left IR Sensor */ RS2_OPTION_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */ } rs2_option; diff --git a/include/librealsense2/h/rs_sensor.h b/include/librealsense2/h/rs_sensor.h index ff8b74ce22..78b15a7679 100644 --- a/include/librealsense2/h/rs_sensor.h +++ b/include/librealsense2/h/rs_sensor.h @@ -90,6 +90,8 @@ typedef enum rs2_format RS2_FORMAT_Z16H , /**< DEPRECATED! - Variable-length Huffman-compressed 16-bit depth values. */ RS2_FORMAT_FG , /**< 16-bit per-pixel frame grabber format. */ RS2_FORMAT_Y411 , /**< 12-bit per-pixel. */ + RS2_FORMAT_Y16I , /**< 12-bit per pixel interleaved. 12-bit left, 12-bit right. */ + RS2_FORMAT_M420 , /**< 24-bit for every pixel: y for each pixel, and u,v data for every four pixels - packed as 2 lines of y, 1 line of u,v */ RS2_FORMAT_COMBINED_MOTION , /**< Combined motion data, as in the combined_motion structure */ RS2_FORMAT_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */ } rs2_format; diff --git a/src/ds/CMakeLists.txt b/src/ds/CMakeLists.txt index 54f0a58ce8..f9a0d7e22f 100644 --- a/src/ds/CMakeLists.txt +++ b/src/ds/CMakeLists.txt @@ -2,6 +2,7 @@ # Copyright(c) 2022 Intel Corporation. All Rights Reserved. include(${_rel_path}/ds/d400/CMakeLists.txt) +include(${_rel_path}/ds/d500/CMakeLists.txt) target_sources(${LRS_TARGET} PRIVATE diff --git a/src/ds/d400/d400-options.cpp b/src/ds/d400/d400-options.cpp index b83a6079e5..e89ecc51aa 100644 --- a/src/ds/d400/d400-options.cpp +++ b/src/ds/d400/d400-options.cpp @@ -50,7 +50,7 @@ namespace librealsense float temperature; try { - command cmd(ds::PROJ_TEMP_MIPI); + command cmd(ds::GTEMP); auto res = _hw_monitor->send(cmd); temperature = static_cast(res[0]); } diff --git a/src/ds/d500/CMakeLists.txt b/src/ds/d500/CMakeLists.txt new file mode 100644 index 0000000000..b88ce698a4 --- /dev/null +++ b/src/ds/d500/CMakeLists.txt @@ -0,0 +1,21 @@ +# License: Apache 2.0. See LICENSE file in root directory. +# Copyright(c) 2022 Intel Corporation. All Rights Reserved. +target_sources(${LRS_TARGET} + PRIVATE + "${CMAKE_CURRENT_LIST_DIR}/d500-device.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-color.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-motion.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-active.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-private.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-factory.cpp" + "${CMAKE_CURRENT_LIST_DIR}/hw_monitor_extended_buffers.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-options.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-device.h" + "${CMAKE_CURRENT_LIST_DIR}/d500-color.h" + "${CMAKE_CURRENT_LIST_DIR}/d500-motion.h" + "${CMAKE_CURRENT_LIST_DIR}/d500-active.h" + "${CMAKE_CURRENT_LIST_DIR}/d500-private.h" + "${CMAKE_CURRENT_LIST_DIR}/d500-factory.h" + "${CMAKE_CURRENT_LIST_DIR}/hw_monitor_extended_buffers.h" + "${CMAKE_CURRENT_LIST_DIR}/d500-options.h" +) diff --git a/src/ds/d500/d500-active.cpp b/src/ds/d500/d500-active.cpp new file mode 100644 index 0000000000..1b856637a0 --- /dev/null +++ b/src/ds/d500/d500-active.cpp @@ -0,0 +1,33 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2022 Intel Corporation. All Rights Reserved. + +#include +#include +#include +#include +#include + +#include "device.h" +#include "context.h" +#include "image.h" +#include "metadata-parser.h" + +#include "d500-active.h" +#include "d500-private.h" +#include "ds/ds-options.h" +#include "ds/ds-timestamp.h" + +namespace librealsense +{ + d500_active::d500_active(std::shared_ptr ctx, + const platform::backend_device_group& group) + : device(ctx, group), d500_device(ctx, group) + { + using namespace ds; + + _ds_active_common = std::make_shared(get_raw_depth_sensor(), get_depth_sensor(), this, + _device_capabilities, _hw_monitor, _fw_version); + + _ds_active_common->register_options(); + } +} diff --git a/src/ds/d500/d500-active.h b/src/ds/d500/d500-active.h new file mode 100644 index 0000000000..d8ed998c16 --- /dev/null +++ b/src/ds/d500/d500-active.h @@ -0,0 +1,19 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2022 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "d500-device.h" +#include "ds/ds-active-common.h" + +namespace librealsense +{ + class d500_active : public virtual d500_device + { + public: + d500_active(std::shared_ptr ctx, + const platform::backend_device_group& group); + private: + std::shared_ptr _ds_active_common; + }; +} diff --git a/src/ds/d500/d500-color.cpp b/src/ds/d500/d500-color.cpp new file mode 100644 index 0000000000..cccdfefe2d --- /dev/null +++ b/src/ds/d500/d500-color.cpp @@ -0,0 +1,176 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2022 Intel Corporation. All Rights Reserved. + +#include +#include "metadata.h" + +#include "ds/ds-timestamp.h" +#include "proc/color-formats-converter.h" +#include "d500-options.h" +#include "d500-color.h" + +namespace librealsense +{ + std::map d500_color_fourcc_to_rs2_format = { + {rs_fourcc('Y','U','Y','2'), RS2_FORMAT_YUYV}, + {rs_fourcc('Y','U','Y','V'), RS2_FORMAT_YUYV}, + {rs_fourcc('U','Y','V','Y'), RS2_FORMAT_UYVY}, + {rs_fourcc('M','J','P','G'), RS2_FORMAT_MJPEG}, + {rs_fourcc('B','Y','R','2'), RS2_FORMAT_RAW16}, + {rs_fourcc('M','4','2','0'), RS2_FORMAT_M420} + }; + std::map d500_color_fourcc_to_rs2_stream = { + {rs_fourcc('Y','U','Y','2'), RS2_STREAM_COLOR}, + {rs_fourcc('Y','U','Y','V'), RS2_STREAM_COLOR}, + {rs_fourcc('U','Y','V','Y'), RS2_STREAM_COLOR}, + {rs_fourcc('B','Y','R','2'), RS2_STREAM_COLOR}, + {rs_fourcc('M','J','P','G'), RS2_STREAM_COLOR}, + {rs_fourcc('M','4','2','0'), RS2_STREAM_COLOR} + }; + + d500_color::d500_color(std::shared_ptr ctx, + const platform::backend_device_group& group) + : d500_device(ctx, group), device(ctx, group), + _color_stream(new stream(RS2_STREAM_COLOR)), + _separate_color(true) + { + create_color_device(ctx, group); + init(); + } + + void d500_color::create_color_device(std::shared_ptr ctx, const platform::backend_device_group& group) + { + using namespace ds; + auto&& backend = ctx->get_backend(); + + _color_calib_table_raw = [this]() + { + return get_d500_raw_calibration_table(d500_calibration_table_id::rgb_calibration_id); + }; + + _color_extrinsic = std::make_shared>([this]() { return from_pose(get_d500_color_stream_extrinsic(*_color_calib_table_raw)); }); + environment::get_instance().get_extrinsics_graph().register_extrinsics(*_color_stream, *_depth_stream, _color_extrinsic); + register_stream_to_extrinsic_group(*_color_stream, 0); + + std::vector color_devs_info = filter_by_mi(group.uvc_devices, 3); + + std::unique_ptr ds_timestamp_reader_backup(new ds_timestamp_reader(backend.create_time_service())); + std::unique_ptr ds_timestamp_reader_metadata(new ds_timestamp_reader_from_metadata(std::move(ds_timestamp_reader_backup))); + + auto enable_global_time_option = std::shared_ptr(new global_time_option()); + auto raw_color_ep = std::make_shared("Raw RGB Camera", + backend.create_uvc_device(color_devs_info.front()), + std::unique_ptr(new global_timestamp_reader(std::move(ds_timestamp_reader_metadata), _tf_keeper, enable_global_time_option)), + this); + + auto color_ep = std::make_shared(this, + raw_color_ep, + d500_color_fourcc_to_rs2_format, + d500_color_fourcc_to_rs2_stream); + + color_ep->register_option(RS2_OPTION_GLOBAL_TIME_ENABLED, enable_global_time_option); + + color_ep->register_info(RS2_CAMERA_INFO_PHYSICAL_PORT, color_devs_info.front().device_path); + + _color_device_idx = add_sensor(color_ep); + } + + void d500_color::init() + { + auto& color_ep = get_color_sensor(); + auto& raw_color_ep = get_raw_color_sensor(); + + _ds_color_common = std::make_shared(raw_color_ep, color_ep, + _fw_version, _hw_monitor, this); + + register_options(); + register_metadata(); + register_processing_blocks(); + } + + void d500_color::register_options() + { + auto& color_ep = get_color_sensor(); + auto& raw_color_ep = get_raw_color_sensor(); + + _ds_color_common->register_color_options(); + + color_ep.register_pu(RS2_OPTION_AUTO_EXPOSURE_PRIORITY); + + _ds_color_common->register_standard_options(); + + color_ep.register_pu(RS2_OPTION_HUE); + } + + void d500_color::register_metadata() + { + auto& color_ep = get_color_sensor(); + + auto md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_rgb_mode, rgb_mode) + + offsetof(md_rgb_normal_mode, intel_rgb_control); + + color_ep.register_metadata(RS2_FRAME_METADATA_AUTO_EXPOSURE, make_attribute_parser(&md_rgb_control::ae_mode, md_rgb_control_attributes::ae_mode_attribute, md_prop_offset, + [](rs2_metadata_type param) { return (param != 1); })); // OFF value via UVC is 1 (ON is 8) + + _ds_color_common->register_metadata(); + } + + void d500_color::register_processing_blocks() + { + auto& color_ep = get_color_sensor(); + + color_ep.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_RAW16, RS2_STREAM_COLOR)); + + color_ep.register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_M420, map_supported_color_formats(RS2_FORMAT_M420), RS2_STREAM_COLOR)); + } + + rs2_intrinsics d500_color_sensor::get_intrinsics(const stream_profile& profile) const + { + return get_d500_intrinsic_by_resolution( + *_owner->_color_calib_table_raw, + ds::d500_calibration_table_id::rgb_calibration_id, + profile.width, profile.height); + } + + stream_profiles d500_color_sensor::init_stream_profiles() + { + auto lock = environment::get_instance().get_extrinsics_graph().lock(); + auto&& results = synthetic_sensor::init_stream_profiles(); + + for (auto&& p : results) + { + // Register stream types + if (p->get_stream_type() == RS2_STREAM_COLOR) + { + assign_stream(_owner->_color_stream, p); + } + + auto&& video = dynamic_cast(p.get()); + const auto&& profile = to_profile(p.get()); + + std::weak_ptr wp = + std::dynamic_pointer_cast(this->shared_from_this()); + video->set_intrinsics([profile, wp]() + { + auto sp = wp.lock(); + if (sp) + return sp->get_intrinsics(profile); + else + return rs2_intrinsics{}; + }); + } + + return results; + } + + processing_blocks d500_color_sensor::get_recommended_processing_blocks() const + { + return get_color_recommended_proccesing_blocks(); + } + + void d500_color::register_stream_to_extrinsic_group(const stream_interface& stream, uint32_t group_index) + { + device::register_stream_to_extrinsic_group(stream, group_index); + } +} diff --git a/src/ds/d500/d500-color.h b/src/ds/d500/d500-color.h new file mode 100644 index 0000000000..dd9b17a362 --- /dev/null +++ b/src/ds/d500/d500-color.h @@ -0,0 +1,79 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2015 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "d500-device.h" +#include "ds/ds-color-common.h" + +#include + +#include "stream.h" + +namespace librealsense +{ + class d500_color : public virtual d500_device + { + public: + d500_color(std::shared_ptr ctx, + const platform::backend_device_group& group); + + synthetic_sensor& get_color_sensor() + { + return dynamic_cast(get_sensor(_color_device_idx)); + } + + uvc_sensor& get_raw_color_sensor() + { + synthetic_sensor& color_sensor = get_color_sensor(); + return dynamic_cast(*color_sensor.get_raw_sensor()); + } + + protected: + std::shared_ptr _color_stream; + std::shared_ptr _ds_color_common; + + private: + void register_options(); + void register_metadata(); + void register_processing_blocks(); + + void register_stream_to_extrinsic_group(const stream_interface& stream, uint32_t group_index); + + void create_color_device(std::shared_ptr ctx, + const platform::backend_device_group& group); + void init(); + + friend class d500_color_sensor; + friend class rs435i_device; + friend class ds_color_common; + + uint8_t _color_device_idx = -1; + bool _separate_color; + lazy> _color_calib_table_raw; + std::shared_ptr> _color_extrinsic; + }; + + class d500_color_sensor : public synthetic_sensor, + public video_sensor_interface, + public roi_sensor_base, + public color_sensor + { + public: + explicit d500_color_sensor(d500_color* owner, + std::shared_ptr uvc_sensor, + std::map d500_color_fourcc_to_rs2_format, + std::map d500_color_fourcc_to_rs2_stream) + : synthetic_sensor("RGB Camera", uvc_sensor, owner, d500_color_fourcc_to_rs2_format, d500_color_fourcc_to_rs2_stream), + _owner(owner) + {} + + rs2_intrinsics get_intrinsics(const stream_profile& profile) const override; + stream_profiles init_stream_profiles() override; + processing_blocks get_recommended_processing_blocks() const override; + + protected: + const d500_color* _owner; + }; + +} diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp new file mode 100644 index 0000000000..da20af3bc8 --- /dev/null +++ b/src/ds/d500/d500-device.cpp @@ -0,0 +1,838 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2022 Intel Corporation. All Rights Reserved. + +#include +#include + +#include "device.h" +#include "context.h" +#include "image.h" +#include "metadata-parser.h" + +#include "d500-device.h" +#include "d500-private.h" +#include "d500-options.h" +#include "ds/ds-options.h" +#include "ds/ds-timestamp.h" +#include "stream.h" +#include "environment.h" +#include "d500-color.h" +#include "ds/d400/d400-nonmonochrome.h" + +#include "proc/depth-formats-converter.h" +#include "proc/y8i-to-y8y8.h" +#include "proc/y12i-to-y16y16.h" +#include "proc/y16i-to-y10msby10msb.h" +#include "proc/color-formats-converter.h" + +#include "hdr-config.h" +#include "../common/fw/firmware-version.h" +#include "fw-update/fw-update-unsigned.h" +#include "../third-party/json.hpp" + +#ifdef HWM_OVER_XU +constexpr bool hw_mon_over_xu = true; +#else +constexpr bool hw_mon_over_xu = false; +#endif + +namespace librealsense +{ + std::map d500_depth_fourcc_to_rs2_format = { + {rs_fourcc('Y','U','Y','2'), RS2_FORMAT_YUYV}, + {rs_fourcc('Y','U','Y','V'), RS2_FORMAT_YUYV}, + {rs_fourcc('U','Y','V','Y'), RS2_FORMAT_UYVY}, + {rs_fourcc('G','R','E','Y'), RS2_FORMAT_Y8}, + {rs_fourcc('Y','8','I',' '), RS2_FORMAT_Y8I}, + {rs_fourcc('W','1','0',' '), RS2_FORMAT_W10}, + {rs_fourcc('Y','1','6',' '), RS2_FORMAT_Y16}, + {rs_fourcc('Y','1','2','I'), RS2_FORMAT_Y12I}, + {rs_fourcc('Y','1','6','I'), RS2_FORMAT_Y16I}, + {rs_fourcc('Z','1','6',' '), RS2_FORMAT_Z16}, + {rs_fourcc('Z','1','6','H'), RS2_FORMAT_Z16H}, + {rs_fourcc('R','G','B','2'), RS2_FORMAT_BGR8}, + {rs_fourcc('M','J','P','G'), RS2_FORMAT_MJPEG}, + {rs_fourcc('B','Y','R','2'), RS2_FORMAT_RAW16} + + }; + std::map d500_depth_fourcc_to_rs2_stream = { + {rs_fourcc('Y','U','Y','2'), RS2_STREAM_COLOR}, + {rs_fourcc('Y','U','Y','V'), RS2_STREAM_COLOR}, + {rs_fourcc('U','Y','V','Y'), RS2_STREAM_INFRARED}, + {rs_fourcc('G','R','E','Y'), RS2_STREAM_INFRARED}, + {rs_fourcc('Y','8','I',' '), RS2_STREAM_INFRARED}, + {rs_fourcc('W','1','0',' '), RS2_STREAM_INFRARED}, + {rs_fourcc('Y','1','6',' '), RS2_STREAM_INFRARED}, + {rs_fourcc('Y','1','2','I'), RS2_STREAM_INFRARED}, + {rs_fourcc('Y','1','6','I'), RS2_STREAM_INFRARED}, + {rs_fourcc('R','G','B','2'), RS2_STREAM_INFRARED}, + {rs_fourcc('Z','1','6',' '), RS2_STREAM_DEPTH}, + {rs_fourcc('Z','1','6','H'), RS2_STREAM_DEPTH}, + {rs_fourcc('B','Y','R','2'), RS2_STREAM_COLOR}, + {rs_fourcc('M','J','P','G'), RS2_STREAM_COLOR} + }; + + std::vector d500_device::send_receive_raw_data(const std::vector& input) + { + return _hw_monitor->send(input); + } + + std::vector d500_device::build_command(uint32_t opcode, + uint32_t param1, + uint32_t param2, + uint32_t param3, + uint32_t param4, + uint8_t const * data, + size_t dataLength) const + { + return _hw_monitor->build_command(opcode, param1, param2, param3, param4, data, dataLength); + } + + void d500_device::hardware_reset() + { + command cmd(ds::HWRST); + cmd.require_response = false; + _hw_monitor->send(cmd); + } + + void d500_device::enter_update_state() const + { + _ds_device_common->enter_update_state(); + } + + std::vector d500_device::backup_flash(update_progress_callback_ptr callback) + { + return _ds_device_common->backup_flash(callback); + } + + void d500_device::update_flash(const std::vector& image, update_progress_callback_ptr callback, int update_mode) + { + _ds_device_common->update_flash(image, callback, update_mode); + } + + class d500_depth_sensor : public synthetic_sensor, public video_sensor_interface, public depth_stereo_sensor, public roi_sensor_base + { + public: + explicit d500_depth_sensor(d500_device* owner, + std::shared_ptr uvc_sensor) + : synthetic_sensor(ds::DEPTH_STEREO, uvc_sensor, owner, d500_depth_fourcc_to_rs2_format, + d500_depth_fourcc_to_rs2_stream), + _owner(owner), + _depth_units(-1), + _hdr_cfg(nullptr) + { } + + processing_blocks get_recommended_processing_blocks() const override + { + return get_ds_depth_recommended_proccesing_blocks(); + }; + + rs2_intrinsics get_intrinsics(const stream_profile& profile) const override + { + return get_d500_intrinsic_by_resolution( + *_owner->_coefficients_table_raw, + ds::d500_calibration_table_id::depth_calibration_id, + profile.width, profile.height, _owner->_is_symmetrization_enabled); + } + + void set_frame_metadata_modifier(on_frame_md callback) override + { + _metadata_modifier = callback; + auto s = get_raw_sensor().get(); + auto uvc = As< librealsense::uvc_sensor >(s); + if(uvc) + uvc->set_frame_metadata_modifier(callback); + } + + void open(const stream_profiles& requests) override + { + group_multiple_fw_calls(*this, [&]() { + _depth_units = get_option(RS2_OPTION_DEPTH_UNITS).query(); + set_frame_metadata_modifier([&](frame_additional_data& data) {data.depth_units = _depth_units.load(); }); + + synthetic_sensor::open(requests); + + // needed in order to restore the HDR sub-preset when streaming is turned off and on + if (_hdr_cfg && _hdr_cfg->is_enabled()) + get_option(RS2_OPTION_HDR_ENABLED).set(1.f); + }); //group_multiple_fw_calls + } + + void close() override + { + synthetic_sensor::close(); + } + + rs2_intrinsics get_color_intrinsics(const stream_profile& profile) const + { + return get_d500_intrinsic_by_resolution( + *_owner->_color_calib_table_raw, + ds::d500_calibration_table_id::rgb_calibration_id, + profile.width, profile.height); + } + + /* + Infrared profiles are initialized with the following logic: + - If device has color sensor (D415 / D435), infrared profile is chosen with Y8 format + - If device does not have color sensor: + * if it is a rolling shutter device (D400 / D410 / D415 / D405), infrared profile is chosen with RGB8 format + * for other devices (D420 / D430), infrared profile is chosen with Y8 format + */ + stream_profiles init_stream_profiles() override + { + auto lock = environment::get_instance().get_extrinsics_graph().lock(); + + auto&& results = synthetic_sensor::init_stream_profiles(); + + for (auto&& p : results) + { + // Register stream types + if (p->get_stream_type() == RS2_STREAM_DEPTH) + { + assign_stream(_owner->_depth_stream, p); + } + else if (p->get_stream_type() == RS2_STREAM_INFRARED && p->get_stream_index() < 2) + { + assign_stream(_owner->_left_ir_stream, p); + } + else if (p->get_stream_type() == RS2_STREAM_INFRARED && p->get_stream_index() == 2) + { + assign_stream(_owner->_right_ir_stream, p); + } + else if (p->get_stream_type() == RS2_STREAM_COLOR) + { + assign_stream(_owner->_color_stream, p); + } + auto&& vid_profile = dynamic_cast(p.get()); + + // used when color stream comes from depth sensor (as in D405) + if (p->get_stream_type() == RS2_STREAM_COLOR) + { + const auto&& profile = to_profile(p.get()); + std::weak_ptr wp = + std::dynamic_pointer_cast(this->shared_from_this()); + vid_profile->set_intrinsics([profile, wp]() + { + auto sp = wp.lock(); + if (sp) + return sp->get_color_intrinsics(profile); + else + return rs2_intrinsics{}; + }); + } + // Register intrinsics + else if (p->get_format() != RS2_FORMAT_Y16) // Y16 format indicate unrectified images, no intrinsics are available for these + { + const auto&& profile = to_profile(p.get()); + std::weak_ptr wp = + std::dynamic_pointer_cast(this->shared_from_this()); + vid_profile->set_intrinsics([profile, wp]() + { + auto sp = wp.lock(); + if (sp) + return sp->get_intrinsics(profile); + else + return rs2_intrinsics{}; + }); + } + } + + return results; + } + + float get_depth_scale() const override + { + if (_depth_units < 0) + _depth_units = get_option(RS2_OPTION_DEPTH_UNITS).query(); + return _depth_units; + } + + void set_depth_scale(float val) + { + _depth_units = val; + set_frame_metadata_modifier([&](frame_additional_data& data) {data.depth_units = _depth_units.load(); }); + } + + void init_hdr_config(const option_range& exposure_range, const option_range& gain_range) + { + _hdr_cfg = std::make_shared(*(_owner->_hw_monitor), get_raw_sensor(), + exposure_range, gain_range); + } + + std::shared_ptr get_hdr_config() { return _hdr_cfg; } + + float get_stereo_baseline_mm() const override { return _owner->get_stereo_baseline_mm(); } + + void create_snapshot(std::shared_ptr& snapshot) const override + { + snapshot = std::make_shared(get_depth_scale()); + } + + void create_snapshot(std::shared_ptr& snapshot) const override + { + snapshot = std::make_shared(get_depth_scale(), get_stereo_baseline_mm()); + } + + void enable_recording(std::function recording_function) override + { + //does not change over time + } + + void enable_recording(std::function recording_function) override + { + //does not change over time + } + + float get_preset_max_value() const override + { + return static_cast(RS2_RS400_VISUAL_PRESET_MEDIUM_DENSITY); + } + + protected: + const d500_device* _owner; + mutable std::atomic _depth_units; + float _stereo_baseline_mm; + std::shared_ptr _hdr_cfg; + }; + + bool d500_device::is_camera_in_advanced_mode() const + { + return _ds_device_common->is_camera_in_advanced_mode(); + } + + float d500_device::get_stereo_baseline_mm() const // to be d500 adapted + { + using namespace ds; + auto table = check_calib(*_coefficients_table_raw); + return fabs(table->baseline); + } + + std::vector d500_device::get_d500_raw_calibration_table(ds::d500_calibration_table_id table_id) const // to be d500 adapted + { + using namespace ds; + command cmd(GET_HKR_CONFIG_TABLE, + static_cast(d500_calib_location::d500_calib_flash_memory), + static_cast(table_id), + static_cast(d500_calib_type::d500_calib_dynamic)); + return _hw_monitor->send(cmd); + } + + std::vector d500_device::get_new_calibration_table() const // to be d500 adapted + { + command cmd(ds::RECPARAMSGET); + return _hw_monitor->send(cmd); + } + + // The GVD structure is currently only partialy parsed + // Once all the fields are enabled in FW, other required fields will be parsed, as required + ds::ds_caps d500_device::parse_device_capabilities( const std::vector &gvd_buf ) const + { + using namespace ds; + + // Opaque retrieval + ds_caps val{ds_caps::CAP_UNDEFINED}; + if (gvd_buf[active_projector]) // DepthActiveMode + val |= ds_caps::CAP_ACTIVE_PROJECTOR; + if (gvd_buf[rgb_sensor]) // WithRGB + val |= ds_caps::CAP_RGB_SENSOR; + if (gvd_buf[imu_sensor]) + { + val |= ds_caps::CAP_IMU_SENSOR; + if (gvd_buf[imu_acc_chip_id] == I2C_IMU_BMI055_ID_ACC) + val |= ds_caps::CAP_BMI_055; + else if (gvd_buf[imu_acc_chip_id] == I2C_IMU_BMI085_ID_ACC) + val |= ds_caps::CAP_BMI_085; + else if (d500_hid_bmi_085_pid.end() != d500_hid_bmi_085_pid.find(_pid)) + val |= ds_caps::CAP_BMI_085; + else + LOG_WARNING("The IMU sensor is undefined for PID " << std::hex << _pid << " and imu_chip_id: " << gvd_buf[imu_acc_chip_id] << std::dec); + } + if (0xFF != (gvd_buf[fisheye_sensor_lb] & gvd_buf[fisheye_sensor_hb])) + val |= ds_caps::CAP_FISHEYE_SENSOR; + if (0x1 == gvd_buf[depth_sensor_type]) + val |= ds_caps::CAP_ROLLING_SHUTTER; // e.g. ASRC + if (0x2 == gvd_buf[depth_sensor_type]) + val |= ds_caps::CAP_GLOBAL_SHUTTER; // e.g. AWGC + val |= ds_caps::CAP_INTERCAM_HW_SYNC; + + return val; + } + + std::shared_ptr d500_device::create_depth_device(std::shared_ptr ctx, + const std::vector& all_device_infos) + { + using namespace ds; + + auto&& backend = ctx->get_backend(); + + std::vector> depth_devices; + for (auto&& info : filter_by_mi(all_device_infos, 0)) // Filter just mi=0, DEPTH + depth_devices.push_back(backend.create_uvc_device(info)); + + std::unique_ptr timestamp_reader_backup(new ds_timestamp_reader(backend.create_time_service())); + std::unique_ptr timestamp_reader_metadata(new ds_timestamp_reader_from_metadata(std::move(timestamp_reader_backup))); + auto enable_global_time_option = std::shared_ptr(new global_time_option()); + auto raw_depth_ep = std::make_shared("Raw Depth Sensor", std::make_shared(depth_devices), + std::unique_ptr(new global_timestamp_reader(std::move(timestamp_reader_metadata), _tf_keeper, enable_global_time_option)), this); + + raw_depth_ep->register_xu(depth_xu); // make sure the XU is initialized every time we power the camera + + auto depth_ep = std::make_shared(this, raw_depth_ep); + + depth_ep->register_info(RS2_CAMERA_INFO_PHYSICAL_PORT, filter_by_mi(all_device_infos, 0).front().device_path); + + depth_ep->register_option(RS2_OPTION_GLOBAL_TIME_ENABLED, enable_global_time_option); + + depth_ep->register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 1)); + depth_ep->register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_Z16, RS2_STREAM_DEPTH)); + + depth_ep->register_processing_block({ {RS2_FORMAT_W10} }, { {RS2_FORMAT_RAW10, RS2_STREAM_INFRARED, 1} }, []() { return std::make_shared(RS2_FORMAT_RAW10); }); + depth_ep->register_processing_block({ {RS2_FORMAT_W10} }, { {RS2_FORMAT_Y10BPACK, RS2_STREAM_INFRARED, 1} }, []() { return std::make_shared(RS2_FORMAT_Y10BPACK); }); + + return depth_ep; + } + + d500_device::d500_device(std::shared_ptr ctx, + const platform::backend_device_group& group) + : device(ctx, group), global_time_interface(), + _device_capabilities(ds::ds_caps::CAP_UNDEFINED), + _depth_stream(new stream(RS2_STREAM_DEPTH)), + _left_ir_stream(new stream(RS2_STREAM_INFRARED, 1)), + _right_ir_stream(new stream(RS2_STREAM_INFRARED, 2)), + _color_stream(nullptr) + { + _depth_device_idx = add_sensor(create_depth_device(ctx, group.uvc_devices)); + init(ctx, group); + } + + void d500_device::init(std::shared_ptr ctx, + const platform::backend_device_group& group) + { + using namespace ds; + + auto&& backend = ctx->get_backend(); + auto& raw_sensor = get_raw_depth_sensor(); + _pid = group.uvc_devices.front().pid; + + _color_calib_table_raw = [this]() + { + return get_d500_raw_calibration_table(d500_calibration_table_id::rgb_calibration_id); + }; + + if (((hw_mon_over_xu) && (RS400_IMU_PID != _pid)) || (!group.usb_devices.size())) + { + _hw_monitor = std::make_shared( + std::make_shared( + std::make_shared( + raw_sensor, depth_xu, DS5_HWMONITOR), + raw_sensor)); + } + else + { + _hw_monitor = std::make_shared( + std::make_shared( + backend.create_usb_device(group.usb_devices.front()), raw_sensor)); + } + + _ds_device_common = std::make_shared(this, _hw_monitor); + + // Define Left-to-Right extrinsics calculation (lazy) + // Reference CS - Right-handed; positive [X,Y,Z] point to [Left,Up,Forward] accordingly. + _left_right_extrinsics = std::make_shared>([this]() + { + rs2_extrinsics ext = identity_matrix(); + auto table = check_calib(*_coefficients_table_raw); + ext.translation[0] = 0.001f * table->baseline; // mm to meters + return ext; + }); + + environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_depth_stream, *_left_ir_stream); + environment::get_instance().get_extrinsics_graph().register_extrinsics(*_depth_stream, *_right_ir_stream, _left_right_extrinsics); + + register_stream_to_extrinsic_group(*_depth_stream, 0); + register_stream_to_extrinsic_group(*_left_ir_stream, 0); + register_stream_to_extrinsic_group(*_right_ir_stream, 0); + + _coefficients_table_raw = [this]() { return get_d500_raw_calibration_table(d500_calibration_table_id::depth_calibration_id); }; + _new_calib_table_raw = [this]() { return get_new_calibration_table(); }; + + std::string device_name = (rs500_sku_names.end() != rs500_sku_names.find(_pid)) ? rs500_sku_names.at(_pid) : "RS5xx"; + + std::vector gvd_buff(HW_MONITOR_BUFFER_SIZE); + + auto& depth_sensor = get_depth_sensor(); + auto& raw_depth_sensor = get_raw_depth_sensor(); + + using namespace platform; + + std::string asic_serial, pid_hex_str, usb_type_str; + d500_gvd_parsed_fields gvd_parsed_fields; + bool advanced_mode = false; + bool usb_modality = true; + group_multiple_fw_calls(depth_sensor, [&]() { + + _hw_monitor->get_gvd(gvd_buff.size(), gvd_buff.data(), ds::fw_cmd::GVD); + + constexpr auto gvd_header_size = 8; + get_gvd_details(gvd_buff, &gvd_parsed_fields); + auto gvd_payload_data = gvd_buff.data() + gvd_header_size; + auto computed_crc = calc_crc32(gvd_payload_data, gvd_parsed_fields.payload_size); + LOG_INFO("gvd version = " << gvd_parsed_fields.gvd_version); + LOG_INFO("gvd payload size = " << gvd_parsed_fields.payload_size); + LOG_INFO("gvd crc = " << gvd_parsed_fields.crc32); + LOG_INFO("gvd optical module sn = " << gvd_parsed_fields.optical_module_sn); + if (computed_crc != gvd_parsed_fields.crc32) + LOG_ERROR("CRC mismatch in D500 GVD - received CRC = " << gvd_parsed_fields.crc32 << ", computed CRC = " << computed_crc); + + _device_capabilities = ds_caps::CAP_ACTIVE_PROJECTOR | ds_caps::CAP_RGB_SENSOR | ds_caps::CAP_IMU_SENSOR | + ds_caps::CAP_BMI_085 | ds_caps::CAP_GLOBAL_SHUTTER | ds_caps::CAP_INTERCAM_HW_SYNC; + + _fw_version = firmware_version(gvd_parsed_fields.fw_version); + + auto _usb_mode = usb3_type; + usb_type_str = usb_spec_names.at(_usb_mode); + _usb_mode = raw_depth_sensor.get_usb_specification(); + if (usb_spec_names.count(_usb_mode) && (usb_undefined != _usb_mode)) + usb_type_str = usb_spec_names.at(_usb_mode); + else // Backend fails to provide USB descriptor - occurs with RS3 build. Requires further work + usb_modality = false; + + _is_symmetrization_enabled = check_symmetrization_enabled(); + + depth_sensor.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_Z16H, RS2_STREAM_DEPTH)); + + depth_sensor.register_processing_block( + { {RS2_FORMAT_Y8I} }, + { {RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 1} , {RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 2} }, + []() { return std::make_shared(); } + ); // L+R + + depth_sensor.register_processing_block( + { RS2_FORMAT_Y16I }, + { {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1}, {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2} }, + []() {return std::make_shared(); } + ); + + pid_hex_str = hexify(_pid); + + _is_locked = _ds_device_common->is_locked(GVD, is_camera_locked_offset); + + depth_sensor.register_option(RS2_OPTION_OUTPUT_TRIGGER_ENABLED, + std::make_shared>(raw_depth_sensor, depth_xu, DS5_EXT_TRIGGER, + "Generate trigger from the camera to external device once per frame")); + + std::shared_ptr