From 134278ae8694b089d1d3df030cd7355cebc44e87 Mon Sep 17 00:00:00 2001 From: guoping wen Date: Mon, 6 Jan 2025 18:32:19 -0800 Subject: [PATCH] support d430 GMSL --- src/ds/advanced_mode/advanced_mode.cpp | 1 + src/ds/d400/d400-device.cpp | 6 +- src/ds/d400/d400-factory.cpp | 100 +++++++++++++++++++++++++ src/ds/d400/d400-private.h | 14 +++- src/linux/backend-v4l2.cpp | 85 +++++++++++++++++++-- src/linux/backend-v4l2.h | 2 +- 6 files changed, 194 insertions(+), 14 deletions(-) diff --git a/src/ds/advanced_mode/advanced_mode.cpp b/src/ds/advanced_mode/advanced_mode.cpp index d91d234b55..1d2b12ccb4 100644 --- a/src/ds/advanced_mode/advanced_mode.cpp +++ b/src/ds/advanced_mode/advanced_mode.cpp @@ -112,6 +112,7 @@ namespace librealsense case ds::RS421_PID: case ds::RS430_PID: case ds::RS430I_PID: + case ds::RS430_GMSL_PID: case ds::RS435_RGB_PID: case ds::RS435I_PID: default_430(p); diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 2a5cd76174..5a210d76df 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -501,7 +501,7 @@ namespace librealsense std::unique_ptr< frame_timestamp_reader > timestamp_reader_backup( new ds_timestamp_reader() ); frame_timestamp_reader* timestamp_reader_from_metadata; - if (all_device_infos.front().pid != RS457_PID) + if (all_device_infos.front().pid != RS457_PID && all_device_infos.front().pid != RS430_GMSL_PID) timestamp_reader_from_metadata = new ds_timestamp_reader_from_metadata(std::move(timestamp_reader_backup)); else timestamp_reader_from_metadata = new ds_timestamp_reader_from_metadata_mipi(std::move(timestamp_reader_backup)); @@ -550,7 +550,7 @@ namespace librealsense auto raw_sensor = get_raw_depth_sensor(); _pid = group.uvc_devices.front().pid; // to be changed for D457 - bool mipi_sensor = (RS457_PID == _pid); + bool mipi_sensor = (RS457_PID == _pid || RS430_GMSL_PID == _pid); _color_calib_table_raw = [this]() { @@ -688,7 +688,7 @@ namespace librealsense //if hw_monitor was created by usb replace it with xu // D400_IMU will remain using USB interface due to HW limitations { - if (_pid == ds::RS457_PID) + if (_pid == ds::RS457_PID || _pid == ds::RS430_GMSL_PID) { depth_sensor.register_option(RS2_OPTION_ASIC_TEMPERATURE, std::make_shared(_hw_monitor, diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index 44867a2e97..ccf64082ab 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -499,6 +499,7 @@ namespace librealsense , firmware_logger_device( dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) { + std::cout << "rs430i_device ..." << std::endl; } std::vector get_profiles_tags() const override @@ -686,6 +687,69 @@ namespace librealsense }; + + class rs430_gmsl_device : public d400_active, + public d400_motion_uvc, + public ds_advanced_mode_base, + public firmware_logger_device + { + public: + rs430_gmsl_device( std::shared_ptr< const d400_info > const & dev_info, bool register_device_notifications ) + : device( dev_info, register_device_notifications ) + , backend_device( dev_info, register_device_notifications ) + , d400_device( dev_info ) + , d400_active( dev_info ) + , d400_motion_uvc( dev_info ) + , ds_advanced_mode_base( d400_device::_hw_monitor, get_depth_sensor() ) + , firmware_logger_device( + dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command() ) + { + } + + std::shared_ptr create_matcher(const frame_holder& frame) const override; + + std::vector get_profiles_tags() const override + { + std::vector tags; + + tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + + return tags; + }; + + void hardware_reset() override + { + d400_device::hardware_reset(); + //limitation: the user must hold the context from which the device was created + //creating fake notification to trigger invoke_devices_changed_callbacks, causing disconnection and connection + auto dev_info = this->get_device_info(); + auto non_const_device_info = std::const_pointer_cast< librealsense::device_info >( dev_info ); + std::vector< std::shared_ptr< device_info > > devices{ non_const_device_info }; + auto ctx = std::weak_ptr< context >( get_context() ); + std::thread fake_notification( + [ ctx, devs = std::move( devices ) ]() + { + try + { + if( auto strong = ctx.lock() ) + { + strong->invoke_devices_changed_callbacks( devs, {} ); + // MIPI devices do not re-enumerate so we need to give them some time to restart + std::this_thread::sleep_for( std::chrono::milliseconds( 3000 ) ); + } + if( auto strong = ctx.lock() ) + strong->invoke_devices_changed_callbacks( {}, devs ); + } + catch( const std::exception & e ) + { + LOG_ERROR( e.what() ); + return; + } + } ); + fake_notification.detach(); + } + }; + // AWGCT class rs430_rgb_mm_device : public d400_active, public d400_color, @@ -1128,6 +1192,26 @@ namespace librealsense bool const register_device_notifications = true; auto pid = _group.uvc_devices.front().pid; + auto mi = _group.uvc_devices.front().mi; + std::string unique_id = _group.uvc_devices.front().unique_id; + std::string device_path = _group.uvc_devices.front().device_path; + std::string serial = _group.uvc_devices.front().serial; + + uint32_t uvc_capabilities = _group.uvc_devices.front().uvc_capabilities; + bool has_metadata_node = _group.uvc_devices.front().has_metadata_node; + std::string metadata_node_id = _group.uvc_devices.front().metadata_node_id;; + +/* + std::cout << "d400_info::create_device: " << std::endl; + std::cout << std::hex << pid << std::endl; + std::cout << std::dec << mi << std::endl; + std::cout << unique_id << std::endl; + std::cout << device_path << std::endl; + std::cout << serial << std::endl; + std::cout << std::hex << uvc_capabilities << std::endl; + std::cout << metadata_node_id << std::endl; +*/ + switch(pid) { case RS400_PID: @@ -1171,6 +1255,8 @@ namespace librealsense return std::make_shared< rs455_device >( dev_info, register_device_notifications ); case RS457_PID: return std::make_shared< rs457_device >( dev_info, register_device_notifications ); + case RS430_GMSL_PID: + return std::make_shared< rs430_gmsl_device >( dev_info, register_device_notifications ); default: throw std::runtime_error( rsutils::string::from() << "Unsupported RS400 model! 0x" << std::hex << std::setw( 4 ) << std::setfill( '0' ) << (int)pid ); @@ -1369,6 +1455,20 @@ namespace librealsense return matcher_factory::create(RS2_MATCHER_DEFAULT, streams); } + + std::shared_ptr rs430_gmsl_device::create_matcher(const frame_holder& frame) const + { + std::vector streams = { _depth_stream.get() , _left_ir_stream.get() , _right_ir_stream.get() }; + std::vector mm_streams = { _accel_stream.get(), _gyro_stream.get()}; + streams.insert(streams.end(), mm_streams.begin(), mm_streams.end()); +// if( frame.frame->find_metadata( RS2_FRAME_METADATA_FRAME_COUNTER, nullptr ) ) +// { +// return matcher_factory::create(RS2_MATCHER_DLR_C, streams); +// } + return matcher_factory::create(RS2_MATCHER_DEFAULT, streams); + } + + std::shared_ptr rs400_imu_device::create_matcher(const frame_holder& frame) const { // TODO - A proper matcher for High-FPS sensor is required diff --git a/src/ds/d400/d400-private.h b/src/ds/d400/d400-private.h index c7ea54c5f6..bc4d31403c 100644 --- a/src/ds/d400/d400-private.h +++ b/src/ds/d400/d400-private.h @@ -39,6 +39,7 @@ namespace librealsense const uint16_t RS455_PID = 0x0B5C; // D455 const uint16_t RS457_PID = 0xabcd; // D457 const uint16_t RS457_RECOVERY_PID = 0xbbcd; // D457 DFU Recovery + const uint16_t RS430_GMSL_PID = 0xabce; // D450 GMSL // d400 Devices supported by the current version static const std::set rs400_sku_pid = { @@ -64,7 +65,8 @@ namespace librealsense ds::RS416_PID, ds::RS405_PID, ds::RS455_PID, - ds::RS457_PID + ds::RS457_PID, + ds::RS430_GMSL_PID }; static const std::set d400_multi_sensors_pid = { @@ -77,11 +79,13 @@ namespace librealsense ds::RS435_RGB_PID, ds::RS435I_PID, ds::RS455_PID, - ds::RS457_PID + ds::RS457_PID, + ds::RS430_GMSL_PID }; static const std::set d400_mipi_device_pid = { - ds::RS457_PID + ds::RS457_PID, + ds::RS430_GMSL_PID }; static const std::set d400_hid_sensors_pid = { @@ -133,6 +137,7 @@ namespace librealsense { RS455_PID, "Intel RealSense D455" }, { RS457_PID, "Intel RealSense D457" }, { RS457_RECOVERY_PID, "Intel RealSense D457 Recovery"}, + { RS430_GMSL_PID, "Intel RealSense D430" }, }; static std::map d400_device_to_fw_min_version = { @@ -161,7 +166,8 @@ namespace librealsense {RS405_PID, "5.12.11.8" }, {RS455_PID, "5.13.0.50" }, {RS457_PID, "5.13.1.1" }, - {RS457_RECOVERY_PID, "5.13.1.1" } + {RS457_RECOVERY_PID, "5.13.1.1" }, + {RS430_GMSL_PID, "5.16.1.111" }, }; std::vector filter_d400_device_by_capability( diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 49c2686248..774d241fa2 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -547,6 +547,34 @@ namespace librealsense throw linux_backend_exception(rsutils::string::from() <<__FUNCTION__ << " xioctl(VIDIOC_QUERYCAP) failed"); } + +/* + uint8_t gvd[276]; + struct v4l2_ext_control ctrl; + + memset(gvd,0,276); + + ctrl.id = RS_CAMERA_CID_GVD; + ctrl.size = sizeof(gvd); + ctrl.p_u8 = gvd; + + struct v4l2_ext_controls ext; + + ext.ctrl_class = V4L2_CTRL_CLASS_CAMERA; + ext.controls = &ctrl; + ext.count = 1; + + if (ioctl(*fd, VIDIOC_G_EXT_CTRLS, &ext) == 0) + { +// memcpy(buffer, gvd + DS5_CMD_OPCODE_SIZE, 0x110); + + for (int i = 0; i < 16; i++) + { + std::cout << std::hex << (int) gvd[i] << std::endl; + } + } +*/ + return cap; } @@ -801,7 +829,7 @@ namespace librealsense } void v4l_uvc_device::get_mipi_device_info(const std::string& dev_name, - std::string& bus_info, std::string& card) + std::string& bus_info, std::string& card, uint16_t& device_id) { struct v4l2_capability vcap; int fd = open(dev_name.c_str(), O_RDWR); @@ -831,6 +859,48 @@ namespace librealsense bus_info = reinterpret_cast(vcap.bus_info); card = reinterpret_cast(vcap.card); } + + + uint8_t gvd[276]; + struct v4l2_ext_control ctrl; + + memset(gvd,0,276); + + ctrl.id = RS_CAMERA_CID_GVD; + ctrl.size = sizeof(gvd); + ctrl.p_u8 = gvd; + + struct v4l2_ext_controls ext; + + ext.ctrl_class = V4L2_CTRL_CLASS_CAMERA; + ext.controls = &ctrl; + ext.count = 1; + + if (ioctl(fd, VIDIOC_G_EXT_CTRLS, &ext) == 0) + { +// memcpy(buffer, gvd + DS5_CMD_OPCODE_SIZE, 0x110); + +// for (int i = 0; i < 16; i++) +// { +// std::cout << std::hex << (int) gvd[i] << std::endl; +// } + + uint8_t product_id = 0; + + product_id = gvd[4 + 4]; +// std::cout << "product_id:" << (int) product_id << std::endl; + + switch(product_id) + { + case(0x0F): + device_id = 0xABCE; + break; + default: + device_id = 0xABCD; + break; + } + } + ::close(fd); } @@ -839,15 +909,18 @@ namespace librealsense uint16_t vid{}, pid{}, mi{}; usb_spec usb_specification(usb_undefined); std::string bus_info, card; + uint16_t device_pid = 0; auto dev_name = "/dev/" + name; - get_mipi_device_info(dev_name, bus_info, card); + get_mipi_device_info(dev_name, bus_info, card, device_pid); // the following 2 lines need to be changed in order to enable multiple mipi devices support // or maybe another field in the info structure - TBD vid = 0x8086; - pid = 0xABCD; // D457 dev + pid = device_pid; + +// std::cout << "video_path:" << video_path << ", name:" << dev_name << ", pid=" << std::hex << (int) pid << std::endl; static std::regex video_dev_index("\\d+$"); std::smatch match; @@ -871,9 +944,9 @@ namespace librealsense // further development is needed to permit use of several mipi devices static int first_video_index = ind; // Use camera_video_nodes as number of /dev/video[%d] for each camera sensor subset - const int camera_video_nodes = 6; + const int camera_video_nodes = 5; int cam_id = ind / camera_video_nodes; - ind = (ind - first_video_index) % camera_video_nodes; // offset from first mipi video node and assume 6 nodes per mipi camera + ind = (ind - first_video_index) % camera_video_nodes; // offset from first mipi video node and assume 5 nodes per mipi camera if (ind == 0 || ind == 2 || ind == 4) mi = 0; // video node indicator else if (ind == 1 | ind == 3) @@ -2835,7 +2908,7 @@ namespace librealsense std::shared_ptr v4l_backend::create_uvc_device(uvc_device_info info) const { - bool mipi_device = 0xABCD == info.pid; // D457 development. Not for upstream + bool mipi_device = (0xABCD == info.pid || 0xABCE == info.pid); // D457 development. Not for upstream auto v4l_uvc_dev = mipi_device ? std::make_shared(info) : ((!info.has_metadata_node) ? std::make_shared(info) : std::make_shared(info)); diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index bc24df144e..52836b0499 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -330,7 +330,7 @@ namespace librealsense static uvc_device_info get_info_from_mipi_device_path(const std::string& video_path, const std::string& name); static void get_mipi_device_info(const std::string& dev_name, - std::string& bus_info, std::string& card); + std::string& bus_info, std::string& card, uint16_t& product_id); v4l_uvc_device(const uvc_device_info& info, bool use_memory_map = false);