Skip to content

Commit

Permalink
support d430 GMSL
Browse files Browse the repository at this point in the history
  • Loading branch information
@gwen2018
gwen2018 committed Jan 7, 2025
1 parent 1258860 commit 134278a
Show file tree
Hide file tree
Showing 6 changed files with 194 additions and 14 deletions.
1 change: 1 addition & 0 deletions src/ds/advanced_mode/advanced_mode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
6 changes: 3 additions & 3 deletions src/ds/d400/d400-device.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down Expand Up @@ -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]()
{
Expand Down Expand Up @@ -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<asic_temperature_option_mipi>(_hw_monitor,
Expand Down
100 changes: 100 additions & 0 deletions src/ds/d400/d400-factory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<tagged_profile> get_profiles_tags() const override
Expand Down Expand Up @@ -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<matcher> create_matcher(const frame_holder& frame) const override;

std::vector<tagged_profile> get_profiles_tags() const override
{
std::vector<tagged_profile> 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,
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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 );
Expand Down Expand Up @@ -1369,6 +1455,20 @@ namespace librealsense
return matcher_factory::create(RS2_MATCHER_DEFAULT, streams);
}


std::shared_ptr<matcher> rs430_gmsl_device::create_matcher(const frame_holder& frame) const
{
std::vector<stream_interface*> streams = { _depth_stream.get() , _left_ir_stream.get() , _right_ir_stream.get() };
std::vector<stream_interface*> 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<matcher> rs400_imu_device::create_matcher(const frame_holder& frame) const
{
// TODO - A proper matcher for High-FPS sensor is required
Expand Down
14 changes: 10 additions & 4 deletions src/ds/d400/d400-private.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::uint16_t> rs400_sku_pid = {
Expand All @@ -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<std::uint16_t> d400_multi_sensors_pid = {
Expand All @@ -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<std::uint16_t> d400_mipi_device_pid = {
ds::RS457_PID
ds::RS457_PID,
ds::RS430_GMSL_PID
};

static const std::set<std::uint16_t> d400_hid_sensors_pid = {
Expand Down Expand Up @@ -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<uint16_t, std::string> d400_device_to_fw_min_version = {
Expand Down Expand Up @@ -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<platform::uvc_device_info> filter_d400_device_by_capability(
Expand Down
85 changes: 79 additions & 6 deletions src/linux/backend-v4l2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -831,6 +859,48 @@ namespace librealsense
bus_info = reinterpret_cast<const char *>(vcap.bus_info);
card = reinterpret_cast<const char *>(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);
}

Expand All @@ -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;
Expand All @@ -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)
Expand Down Expand Up @@ -2835,7 +2908,7 @@ namespace librealsense

std::shared_ptr<uvc_device> 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<v4l_mipi_device>(info) :
((!info.has_metadata_node) ? std::make_shared<v4l_uvc_device>(info) :
std::make_shared<v4l_uvc_meta_device>(info));
Expand Down
2 changes: 1 addition & 1 deletion src/linux/backend-v4l2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down

0 comments on commit 134278a

Please sign in to comment.