PR #12041 from Remi: D500 base classes

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;
 

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;

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

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<float>(res[0]);
         }

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"
+)

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 <mutex>
+#include <chrono>
+#include <vector>
+#include <iterator>
+#include <cstddef>
+
+#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<context> ctx,
+                           const platform::backend_device_group& group)
+        : device(ctx, group), d500_device(ctx, group)
+    {
+        using namespace ds;
+
+        _ds_active_common = std::make_shared<ds_active_common>(get_raw_depth_sensor(), get_depth_sensor(), this,
+            _device_capabilities, _hw_monitor, _fw_version);
+
+        _ds_active_common->register_options();
+    }
+}

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<context> ctx,
+                   const platform::backend_device_group& group);
+    private:
+        std::shared_ptr<ds_active_common> _ds_active_common;
+    };
+}

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 <cstddef>
+#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<uint32_t, rs2_format> 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<uint32_t, rs2_stream> 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<context> 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<context> 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<lazy<rs2_extrinsics>>([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<platform::uvc_device_info> color_devs_info = filter_by_mi(group.uvc_devices, 3);
+
+        std::unique_ptr<frame_timestamp_reader> ds_timestamp_reader_backup(new ds_timestamp_reader(backend.create_time_service()));
+        std::unique_ptr<frame_timestamp_reader> ds_timestamp_reader_metadata(new ds_timestamp_reader_from_metadata(std::move(ds_timestamp_reader_backup)));
+
+        auto enable_global_time_option = std::shared_ptr<global_time_option>(new global_time_option());
+        auto raw_color_ep = std::make_shared<uvc_sensor>("Raw RGB Camera",
+            backend.create_uvc_device(color_devs_info.front()),
+            std::unique_ptr<frame_timestamp_reader>(new global_timestamp_reader(std::move(ds_timestamp_reader_metadata), _tf_keeper, enable_global_time_option)),
+            this);
+
+        auto color_ep = std::make_shared<d500_color_sensor>(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<ds_color_common>(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<m420_converter>(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<video_stream_profile_interface*>(p.get());
+            const auto&& profile = to_profile(p.get());
+
+            std::weak_ptr<d500_color_sensor> wp =
+                std::dynamic_pointer_cast<d500_color_sensor>(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);
+    }
+}

src/ds/d500/d500-color.h

@@ -0,0 +1,77 @@
+// 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 <map>
+
+#include "stream.h"
+
+namespace librealsense
+{
+    class d500_color : public virtual d500_device
+    {
+    public:
+        d500_color(std::shared_ptr<context> ctx,
+                  const platform::backend_device_group& group);
+
+        synthetic_sensor& get_color_sensor()
+        {
+            return dynamic_cast<synthetic_sensor&>(get_sensor(_color_device_idx));
+        }
+
+        uvc_sensor& get_raw_color_sensor()
+        {
+            synthetic_sensor& color_sensor = get_color_sensor();
+            return dynamic_cast<uvc_sensor&>(*color_sensor.get_raw_sensor());
+        }
+
+    protected:
+        std::shared_ptr<stream_interface> _color_stream;
+        std::shared_ptr<ds_color_common> _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<context> ctx,
+            const platform::backend_device_group& group);
+        void init();
+
+        friend class d500_color_sensor;
+
+        uint8_t _color_device_idx = -1;
+        bool _separate_color;
+        lazy<std::vector<uint8_t>> _color_calib_table_raw;
+        std::shared_ptr<lazy<rs2_extrinsics>> _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> uvc_sensor,
+            std::map<uint32_t, rs2_format> d500_color_fourcc_to_rs2_format,
+            std::map<uint32_t, rs2_stream> 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;
+    };
+
+}