From 4103a60709d3ba1005ce3d9569916e5e0f62d994 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Thu, 21 Nov 2019 16:29:52 +0000 Subject: [PATCH 001/361] mipi wip --- src/ds5/ds5-factory.cpp | 37 ++++++++++++ src/linux/backend-v4l2.cpp | 120 ++++++++++++++++++++++++------------- 2 files changed, 117 insertions(+), 40 deletions(-) diff --git a/src/ds5/ds5-factory.cpp b/src/ds5/ds5-factory.cpp index 9e72cc1e82..5c6aaeab77 100644 --- a/src/ds5/ds5-factory.cpp +++ b/src/ds5/ds5-factory.cpp @@ -568,6 +568,43 @@ namespace librealsense }; }; + // AWGC + class ds431_device : public ds5_active, + public ds5_color, + public ds5_advanced_mode_base + { + public: + ds431_device(std::shared_ptr ctx, + const platform::backend_device_group group, + bool register_device_notifications) + : device(ctx, group, register_device_notifications), + ds5_device(ctx, group), + ds5_active(ctx, group), + ds5_color(ctx, group), + ds5_advanced_mode_base(ds5_device::_hw_monitor, get_depth_sensor()) {} + + std::shared_ptr create_matcher(const frame_holder& frame) const override; + + std::vector get_profiles_tags() const override + { + std::vector tags; + auto usb_spec = get_usb_spec(); + if (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined) + { + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_DEPTH, -1, 1280, 720, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_INFRARED, -1, 1280, 720, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET }); + } + else + { + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_INFRARED, -1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET }); + } + return tags; + }; + }; + // AWGCT class rs430_rgb_mm_device : public ds5_active, public ds5_color, diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index b1caa103db..7682c2a382 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -517,7 +517,9 @@ namespace librealsense std::string name = entry->d_name; if(name == "." || name == "..") continue; - // Resolve a pathname to ignore virtual video devices + // Resolve a pathname to ignore virtual video devices and sub-devices + static const std::regex uvc_pattern("(\\/usb\\d+\\/)\\w+"); // Locate UVC device path pattern ../usbX/... + static const std::regex video_dev_pattern("(\\/video\\d+)$"); std::string path = "/sys/class/video4linux/" + name; std::string real_path{}; char buff[PATH_MAX] = {0}; @@ -526,12 +528,18 @@ namespace librealsense real_path = std::string(buff); if (real_path.find("virtual") != std::string::npos) continue; + if (!std::regex_search(real_path, video_dev_pattern)) + { + LOG_INFO("Skipping Video4Linux entry " << real_path << " - not a device"); + continue; + } } try { - uint16_t vid, pid, mi; + uint16_t vid{}, pid{}, mi{}; std::string busnum, devnum, devpath; + usb_spec usb_specification(usb_undefined); auto dev_name = "/dev/" + name; @@ -543,56 +551,88 @@ namespace librealsense if(!S_ISCHR(st.st_mode)) throw linux_backend_exception(dev_name + " is no device"); - // Search directory and up to three parent directories to find busnum/devnum - std::ostringstream ss; ss << "/sys/dev/char/" << major(st.st_rdev) << ":" << minor(st.st_rdev) << "/device/"; - auto path = ss.str(); - auto valid_path = false; - for(auto i=0U; i < MAX_DEV_PARENT_DIR; ++i) + + if (std::regex_search(real_path, uvc_pattern)) { - if(std::ifstream(path + "busnum") >> busnum) + LOG_INFO("Enumerating UVC device " << path << " realpath=" << real_path); + // Search directory and up to three parent directories to find busnum/devnum + auto valid_path = false; + std::ostringstream ss; ss << "/sys/dev/char/" << major(st.st_rdev) << ":" << minor(st.st_rdev) << "/device/"; + auto char_dev_path = ss.str(); + + for(auto i=0U; i < MAX_DEV_PARENT_DIR; ++i) { - if(std::ifstream(path + "devnum") >> devnum) + if(std::ifstream(char_dev_path + "busnum") >> busnum) { - if(std::ifstream(path + "devpath") >> devpath) + if(std::ifstream(char_dev_path + "devnum") >> devnum) { - valid_path = true; - break; + if(std::ifstream(char_dev_path + "devpath") >> devpath) + { + valid_path = true; + break; + } } } + char_dev_path += "../"; } - path += "../"; + if(!valid_path) + { + #ifndef RS2_USE_CUDA + /* On the Jetson TX, the camera module is CSI & I2C and does not report as this code expects + Patch suggested by JetsonHacks: https://github.com/jetsonhacks/buildLibrealsense2TX */ + LOG_INFO("Failed to read busnum/devnum. Device Path: " << path); + #endif + continue; + } + + std::string modalias; + if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/modalias") >> modalias)) + throw linux_backend_exception("Failed to read modalias"); + if(modalias.size() < 14 || modalias.substr(0,5) != "usb:v" || modalias[9] != 'p') + throw linux_backend_exception("Not a usb format modalias"); + if(!(std::istringstream(modalias.substr(5,4)) >> std::hex >> vid)) + throw linux_backend_exception("Failed to read vendor ID"); + if(!(std::istringstream(modalias.substr(10,4)) >> std::hex >> pid)) + throw linux_backend_exception("Failed to read product ID"); + if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/bInterfaceNumber") >> std::hex >> mi)) + throw linux_backend_exception("Failed to read interface number"); + + // Find the USB specification (USB2/3) type from the underlying device + // Use device mapping obtained in previous step to traverse node tree + // and extract the required descriptors + // Traverse from + // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/3-6:1.0/video4linux/video0 + // to + // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/version + usb_specification = get_usb_connection_type(real_path + "/../../../"); } - if(!valid_path) + else // Video4Linux Devices that are not UVC { -#ifndef RS2_USE_CUDA - /* On the Jetson TX, the camera module is CSI & I2C and does not report as this code expects - Patch suggested by JetsonHacks: https://github.com/jetsonhacks/buildLibrealsense2TX */ - LOG_INFO("Failed to read busnum/devnum. Device Path: " << path); -#endif + LOG_INFO("Enumerating Video4Linux device " << path << " realpath=" << real_path); + +// std::string modalias; +// if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/modalias") >> modalias)) +// throw linux_backend_exception("Failed to read modalias"); +// if(modalias.size() < 14 || modalias.substr(0,5) != "usb:v" || modalias[9] != 'p') +// throw linux_backend_exception("Not a usb format modalias"); +// if(!(std::istringstream(modalias.substr(5,4)) >> std::hex >> vid)) +// throw linux_backend_exception("Failed to read vendor ID"); +// if(!(std::istringstream(modalias.substr(10,4)) >> std::hex >> pid)) +// throw linux_backend_exception("Failed to read product ID"); +// if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/bInterfaceNumber") >> std::hex >> mi)) +// throw linux_backend_exception("Failed to read interface number"); + + // Find the USB specification (USB2/3) type from the underlying device + // Use device mapping obtained in previous step to traverse node tree + // and extract the required descriptors + // Traverse from + // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/3-6:1.0/video4linux/video0 + // to + // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/version +// usb_spec usb_specification = get_usb_connection_type(real_path + "/../../../"); continue; } - std::string modalias; - if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/modalias") >> modalias)) - throw linux_backend_exception("Failed to read modalias"); - if(modalias.size() < 14 || modalias.substr(0,5) != "usb:v" || modalias[9] != 'p') - throw linux_backend_exception("Not a usb format modalias"); - if(!(std::istringstream(modalias.substr(5,4)) >> std::hex >> vid)) - throw linux_backend_exception("Failed to read vendor ID"); - if(!(std::istringstream(modalias.substr(10,4)) >> std::hex >> pid)) - throw linux_backend_exception("Failed to read product ID"); - if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/bInterfaceNumber") >> std::hex >> mi)) - throw linux_backend_exception("Failed to read interface number"); - - // Find the USB specification (USB2/3) type from the underlying device - // Use device mapping obtained in previous step to traverse node tree - // and extract the required descriptors - // Traverse from - // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/3-6:1.0/video4linux/video0 - // to - // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/version - usb_spec usb_specification = get_usb_connection_type(real_path + "/../../../"); - uvc_device_info info{}; info.pid = pid; info.vid = vid; From 4f4400ec42436475d3d76e7d0b3002a6ddf4d1a0 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 25 Nov 2019 08:44:05 +0000 Subject: [PATCH 002/361] D431 enablement --- src/backend.h | 7 ++-- src/ds5/ds5-factory.cpp | 23 ++++++++++-- src/ds5/ds5-private.h | 5 +++ src/linux/backend-v4l2.cpp | 72 +++++++++++++++++++++++++------------- 4 files changed, 77 insertions(+), 30 deletions(-) diff --git a/src/backend.h b/src/backend.h index 9f7831b0de..836ddc104f 100644 --- a/src/backend.h +++ b/src/backend.h @@ -196,11 +196,12 @@ namespace librealsense s << "id- " << id << "\nvid- " << std::hex << vid << "\npid- " << std::hex << pid << - "\nmi- " << mi << + "\nmi- " << std::dec << mi << "\nunique_id- " << unique_id << "\npath- " << device_path << - "\nsusb specification- " << std::hex << (uint16_t)conn_spec << std::dec << - (has_metadata_node ? ( "\nmetadata node-" + metadata_node_id) : ""); + "\nUVC capabilities- " << std::hex << uvc_capabilities << + "\nUVC specification- " << std::hex << (uint16_t)conn_spec << std::dec << + (has_metadata_node ? ( "\nmetadata node-" + metadata_node_id) : "") << std::endl; return s.str(); } diff --git a/src/ds5/ds5-factory.cpp b/src/ds5/ds5-factory.cpp index 5c6aaeab77..48d13223be 100644 --- a/src/ds5/ds5-factory.cpp +++ b/src/ds5/ds5-factory.cpp @@ -569,12 +569,12 @@ namespace librealsense }; // AWGC - class ds431_device : public ds5_active, + class rs431_device : public ds5_active, public ds5_color, public ds5_advanced_mode_base { public: - ds431_device(std::shared_ptr ctx, + rs431_device(std::shared_ptr ctx, const platform::backend_device_group group, bool register_device_notifications) : device(ctx, group, register_device_notifications), @@ -1110,6 +1110,8 @@ namespace librealsense return std::make_shared(ctx, group, register_device_notifications); case ds::RS455_PID: return std::make_shared(ctx, group, register_device_notifications); + case RS431_PID: + return std::make_shared(ctx, group, register_device_notifications); default: throw std::runtime_error(to_string() << "Unsupported RS400 model! 0x" << std::hex << std::setw(4) << std::setfill('0') <<(int)pid); @@ -1139,7 +1141,10 @@ namespace librealsense for (auto&& uvc : devices) { if (is_pid_of_multisensor_device(uvc.pid)) + { is_device_multisensor = true; + break; + } } if(is_device_multisensor) @@ -1154,7 +1159,10 @@ namespace librealsense for (auto&& uvc : devices) { if (is_pid_of_hid_sensor_device(uvc.pid)) + { is_device_hid_sensor = true; + break; + } } // Device with hids can be enabled only if both hids (gyro and accelerometer) are present @@ -1321,6 +1329,17 @@ namespace librealsense return matcher_factory::create(RS2_MATCHER_DEFAULT, streams); } + + std::shared_ptr rs431_device::create_matcher(const frame_holder& frame) const + { + std::vector streams = { _depth_stream.get() , _left_ir_stream.get() , _right_ir_stream.get(), _color_stream.get() }; + if (frame.frame->supports_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER)) + { + 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/ds5/ds5-private.h b/src/ds5/ds5-private.h index 51a0b095dd..e90d5b0cb7 100644 --- a/src/ds5/ds5-private.h +++ b/src/ds5/ds5-private.h @@ -22,6 +22,7 @@ namespace librealsense { namespace ds { + const uint16_t RS431_PID = 0xabcd; // TODO 431 - Dev . Do not upstream! const uint16_t RS400_PID = 0x0ad1; // PSR const uint16_t RS410_PID = 0x0ad2; // ASR const uint16_t RS415_PID = 0x0ad3; // ASRC @@ -85,6 +86,7 @@ namespace librealsense ds::RS416_PID, ds::RS405_PID, ds::RS455_PID, + ds::RS431_PID, }; static const std::set multi_sensors_pid = { @@ -98,6 +100,7 @@ namespace librealsense ds::RS435I_PID, ds::RS465_PID, ds::RS455_PID, + ds::RS431_PID, }; static const std::set hid_sensors_pid = { @@ -139,6 +142,8 @@ namespace librealsense { RS420_MM_PID, "Intel RealSense D420 with Tracking Module"}, { RS410_MM_PID, "Intel RealSense D410 with Tracking Module"}, { RS400_MM_PID, "Intel RealSense D400 with Tracking Module"}, + { RS430_PID, "Intel RealSense D430"}, + { RS430I_PID, "Intel RealSense D430I"}, { RS430_MM_RGB_PID, "Intel RealSense D430 with Tracking and RGB Modules"}, { RS460_PID, "Intel RealSense D460" }, { RS435_RGB_PID, "Intel RealSense D435"}, diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 7682c2a382..7ca06da595 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -520,6 +520,8 @@ namespace librealsense // Resolve a pathname to ignore virtual video devices and sub-devices static const std::regex uvc_pattern("(\\/usb\\d+\\/)\\w+"); // Locate UVC device path pattern ../usbX/... static const std::regex video_dev_pattern("(\\/video\\d+)$"); + static std::regex video_dev_index("\\d+$"); + std::string path = "/sys/class/video4linux/" + name; std::string real_path{}; char buff[PATH_MAX] = {0}; @@ -540,6 +542,7 @@ namespace librealsense uint16_t vid{}, pid{}, mi{}; std::string busnum, devnum, devpath; usb_spec usb_specification(usb_undefined); + bool v4l_node=false; auto dev_name = "/dev/" + name; @@ -554,7 +557,7 @@ namespace librealsense if (std::regex_search(real_path, uvc_pattern)) { - LOG_INFO("Enumerating UVC device " << path << " realpath=" << real_path); + LOG_INFO("Enumerating UVC " << name << " realpath=" << real_path); // Search directory and up to three parent directories to find busnum/devnum auto valid_path = false; std::ostringstream ss; ss << "/sys/dev/char/" << major(st.st_rdev) << ":" << minor(st.st_rdev) << "/device/"; @@ -606,30 +609,47 @@ namespace librealsense // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/version usb_specification = get_usb_connection_type(real_path + "/../../../"); } - else // Video4Linux Devices that are not UVC + else // Video4Linux Devices that are not listed as UVC { - LOG_INFO("Enumerating Video4Linux device " << path << " realpath=" << real_path); - -// std::string modalias; -// if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/modalias") >> modalias)) -// throw linux_backend_exception("Failed to read modalias"); -// if(modalias.size() < 14 || modalias.substr(0,5) != "usb:v" || modalias[9] != 'p') -// throw linux_backend_exception("Not a usb format modalias"); -// if(!(std::istringstream(modalias.substr(5,4)) >> std::hex >> vid)) -// throw linux_backend_exception("Failed to read vendor ID"); -// if(!(std::istringstream(modalias.substr(10,4)) >> std::hex >> pid)) -// throw linux_backend_exception("Failed to read product ID"); -// if(!(std::ifstream("/sys/class/video4linux/" + name + "/device/bInterfaceNumber") >> std::hex >> mi)) -// throw linux_backend_exception("Failed to read interface number"); + LOG_INFO("Enumerating v4l " << name << " realpath=" << real_path); + v4l_node = true; - // Find the USB specification (USB2/3) type from the underlying device - // Use device mapping obtained in previous step to traverse node tree - // and extract the required descriptors - // Traverse from - // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/3-6:1.0/video4linux/video0 - // to - // /sys/devices/pci0000:00/0000:00:xx.0/ABC/M-N/version -// usb_spec usb_specification = get_usb_connection_type(real_path + "/../../../"); + // D431-specific + vid = 0x8086; + pid = 0xABCD; // TBD Evgeni + + std::smatch match; + uint8_t ind{}; + if (std::regex_search(name, match, video_dev_index)) + { + ind = static_cast(std::stoi(match[0])); + } + else + { + LOG_WARNING("Unresolved Video4Linux device pattern: " << name << ", device is skipped"); + continue; + } + + switch(ind) + { + case 0: + case 1: + mi = 0; + break; + case 3: + mi = 3; + break; + default: + mi = 0xffff; + break; + } + } + + // D431 Dev - skip unsupported devices. Do no upstream! + // manually rectify descriptor inconsistencies + if (0xffff == mi) + { + LOG_DEBUG("D431 - Uninitialized device " << name << ", skipped during enumeration"); continue; } @@ -643,6 +663,8 @@ namespace librealsense info.conn_spec = usb_specification; info.uvc_capabilities = get_dev_capabilities(dev_name); + std::cout << "Device " << name << ":\n" << std::string(info); + uvc_nodes.emplace_back(info, dev_name); } catch(const std::exception & e) @@ -675,7 +697,7 @@ namespace librealsense { if (uvc_devices.empty()) { - LOG_ERROR("uvc meta-node with no video streaming node encountered: " << std::string(cur_node.first)); + LOG_ERROR("UVC meta-node with no video streaming node encountered: " << std::string(cur_node.first)); continue; } @@ -684,7 +706,7 @@ namespace librealsense if (uvc_node.first.uvc_capabilities & V4L2_CAP_META_CAPTURE) { - LOG_ERROR("Consequtive uvc meta-nodes encountered: " << std::string(uvc_node.first) << " and " << std::string(cur_node.first) ); + LOG_ERROR("Consequtive UVC meta-nodes encountered: " << std::string(uvc_node.first) << " and " << std::string(cur_node.first) ); continue; } From 4d016133dbf1c58c9c0f3b634dbf95e894e378aa Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Mon, 25 Nov 2019 17:45:44 +0000 Subject: [PATCH 003/361] Enable RGB streaming, with no controls enabled. Depth WIP --- common/model-views.cpp | 1 + examples/C/depth/rs-depth.c | 2 +- examples/capture/rs-capture.cpp | 7 +- src/ds5/ds5-active.cpp | 2 +- src/ds5/ds5-color.cpp | 289 +++++++++++++++++--------------- src/ds5/ds5-device.cpp | 225 ++++++++++++++----------- src/ds5/ds5-factory.cpp | 2 +- src/linux/backend-v4l2.cpp | 9 +- src/linux/backend-v4l2.h | 17 ++ 9 files changed, 315 insertions(+), 239 deletions(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index bd6745ddd4..94976c100a 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -6603,6 +6603,7 @@ namespace rs2 is_advanced_device = true; try { + //is_advanced_mode_enabled = true;// D431 Development.. advanced_dev.is_enabled(); // Prevent intermittent errors in polling mode to keep imgui in sync is_advanced_mode_enabled = advanced_dev.is_enabled(); } diff --git a/examples/C/depth/rs-depth.c b/examples/C/depth/rs-depth.c index 337b71cfe2..f21e4ebac5 100644 --- a/examples/C/depth/rs-depth.c +++ b/examples/C/depth/rs-depth.c @@ -114,7 +114,7 @@ int main() check_error(e); // Start the pipeline streaming - // The retunred object should be released with rs2_delete_pipeline_profile(...) + // The returned object should be released with rs2_delete_pipeline_profile(...) rs2_pipeline_profile* pipeline_profile = rs2_pipeline_start_with_config(pipeline, config, &e); if (e) { diff --git a/examples/capture/rs-capture.cpp b/examples/capture/rs-capture.cpp index f0073cf348..36b828e22d 100644 --- a/examples/capture/rs-capture.cpp +++ b/examples/capture/rs-capture.cpp @@ -17,13 +17,16 @@ int main(int argc, char * argv[]) try // Declare rates printer for showing streaming rates of the enabled streams. rs2::rates_printer printer; + rs2::config cfg; + //cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30); + cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGB8, 30); // Declare RealSense pipeline, encapsulating the actual device and sensors rs2::pipeline pipe; // Start streaming with default recommended configuration // The default video configuration contains Depth and Color streams // If a device is capable to stream IMU data, both Gyro and Accelerometer are enabled by default - pipe.start(); + pipe.start(cfg); while (app) // Application still alive? { @@ -47,4 +50,4 @@ catch (const std::exception& e) { std::cerr << e.what() << std::endl; return EXIT_FAILURE; -} \ No newline at end of file +} diff --git a/src/ds5/ds5-active.cpp b/src/ds5/ds5-active.cpp index 583c151d33..e22803ebf8 100644 --- a/src/ds5/ds5-active.cpp +++ b/src/ds5/ds5-active.cpp @@ -27,7 +27,7 @@ namespace librealsense //Projector's capacity is established based on actual HW capabilities auto pid = group.uvc_devices.front().pid; - if ((pid != RS_USB2_PID) && ((_device_capabilities & d400_caps::CAP_ACTIVE_PROJECTOR) == d400_caps::CAP_ACTIVE_PROJECTOR)) + if (pid != RS_USB2_PID) { auto&& depth_ep = get_depth_sensor(); auto&& raw_depth_ep = get_raw_depth_sensor(); diff --git a/src/ds5/ds5-color.cpp b/src/ds5/ds5-color.cpp index d2dc6a004a..b5e4f08fa6 100644 --- a/src/ds5/ds5-color.cpp +++ b/src/ds5/ds5-color.cpp @@ -72,8 +72,10 @@ namespace librealsense raw_color_ep, ds5_color_fourcc_to_rs2_format, ds5_color_fourcc_to_rs2_stream); - - color_ep->register_option(RS2_OPTION_GLOBAL_TIME_ENABLED, enable_global_time_option); + if (ds::RS431_PID != color_devices_info.front().pid) + { + 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); @@ -101,164 +103,183 @@ namespace librealsense { auto& color_ep = get_color_sensor(); auto& raw_color_ep = get_raw_color_sensor(); - - color_ep.register_pu(RS2_OPTION_BRIGHTNESS); - color_ep.register_pu(RS2_OPTION_CONTRAST); - color_ep.register_pu(RS2_OPTION_SATURATION); - color_ep.register_pu(RS2_OPTION_GAMMA); - color_ep.register_pu(RS2_OPTION_SHARPNESS); - color_ep.register_pu(RS2_OPTION_BACKLIGHT_COMPENSATION); - - auto white_balance_option = std::make_shared(raw_color_ep, RS2_OPTION_WHITE_BALANCE); - color_ep.register_option(RS2_OPTION_WHITE_BALANCE, white_balance_option); - auto auto_white_balance_option = std::make_shared(raw_color_ep, RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE); - color_ep.register_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, auto_white_balance_option); - color_ep.register_option(RS2_OPTION_WHITE_BALANCE, - std::make_shared( - white_balance_option, - auto_white_balance_option)); - - // Currently disabled for certain sensors - if (!val_in_range(_pid, { ds::RS465_PID })) + + if (ds::RS431_PID != color_devices_info.front().pid) { - color_ep.register_pu(RS2_OPTION_HUE); - } - - color_ep.register_option(RS2_OPTION_POWER_LINE_FREQUENCY, - std::make_shared(raw_color_ep, RS2_OPTION_POWER_LINE_FREQUENCY, - std::map{ { 0.f, "Disabled"}, - { 1.f, "50Hz" }, - { 2.f, "60Hz" }, - { 3.f, "Auto" }, })); + color_ep.register_pu(RS2_OPTION_BRIGHTNESS); + color_ep.register_pu(RS2_OPTION_CONTRAST); + color_ep.register_pu(RS2_OPTION_SATURATION); + color_ep.register_pu(RS2_OPTION_GAMMA); + color_ep.register_pu(RS2_OPTION_SHARPNESS); + color_ep.register_pu(RS2_OPTION_BACKLIGHT_COMPENSATION); + + auto white_balance_option = std::make_shared(raw_color_ep, RS2_OPTION_WHITE_BALANCE); + color_ep.register_option(RS2_OPTION_WHITE_BALANCE, white_balance_option); + auto auto_white_balance_option = std::make_shared(raw_color_ep, RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE); + color_ep.register_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, auto_white_balance_option); + color_ep.register_option(RS2_OPTION_WHITE_BALANCE, + std::make_shared( + white_balance_option, + auto_white_balance_option)); - if (_separate_color) - { // Currently disabled for certain sensors if (!val_in_range(_pid, { ds::RS465_PID })) { - color_ep.register_pu(RS2_OPTION_AUTO_EXPOSURE_PRIORITY); - } - // From 5.11.15 auto-exposure priority is supported on the D465 - else if (_fw_version >= firmware_version("5.11.15.0")) - { - color_ep.register_pu(RS2_OPTION_AUTO_EXPOSURE_PRIORITY); + color_ep.register_pu(RS2_OPTION_HUE); } - auto gain_option = std::make_shared(raw_color_ep, RS2_OPTION_GAIN); - auto exposure_option = std::make_shared(raw_color_ep, RS2_OPTION_EXPOSURE); - auto auto_exposure_option = std::make_shared(raw_color_ep, RS2_OPTION_ENABLE_AUTO_EXPOSURE); - color_ep.register_option(RS2_OPTION_GAIN, gain_option); - color_ep.register_option(RS2_OPTION_EXPOSURE, exposure_option); - color_ep.register_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, auto_exposure_option); - color_ep.register_option(RS2_OPTION_EXPOSURE, - std::make_shared( - exposure_option, - auto_exposure_option)); - color_ep.register_option(RS2_OPTION_GAIN, - std::make_shared( - gain_option, - auto_exposure_option)); + color_ep.register_option(RS2_OPTION_POWER_LINE_FREQUENCY, + std::make_shared(raw_color_ep, RS2_OPTION_POWER_LINE_FREQUENCY, + std::map{ { 0.f, "Disabled"}, + { 1.f, "50Hz" }, + { 2.f, "60Hz" }, + { 3.f, "Auto" }, })); - // Starting with firmware 5.10.9, auto-exposure ROI is available for color sensor - if (_fw_version >= firmware_version("5.10.9.0")) + if (_separate_color) { - roi_sensor_interface* roi_sensor; - if ((roi_sensor = dynamic_cast(&color_ep))) - roi_sensor->set_roi_method(std::make_shared(*_hw_monitor, ds::fw_cmd::SETRGBAEROI)); + // Currently disabled for certain sensors + if (!val_in_range(_pid, { ds::RS465_PID })) + { + color_ep.register_pu(RS2_OPTION_AUTO_EXPOSURE_PRIORITY); + } + // From 5.11.15 auto-exposure priority is supported on the D465 + else if (_fw_version >= firmware_version("5.11.15.0")) + { + color_ep.register_pu(RS2_OPTION_AUTO_EXPOSURE_PRIORITY); + } + + auto gain_option = std::make_shared(raw_color_ep, RS2_OPTION_GAIN); + auto exposure_option = std::make_shared(raw_color_ep, RS2_OPTION_EXPOSURE); + auto auto_exposure_option = std::make_shared(raw_color_ep, RS2_OPTION_ENABLE_AUTO_EXPOSURE); + color_ep.register_option(RS2_OPTION_GAIN, gain_option); + color_ep.register_option(RS2_OPTION_EXPOSURE, exposure_option); + color_ep.register_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, auto_exposure_option); + color_ep.register_option(RS2_OPTION_EXPOSURE, + std::make_shared( + exposure_option, + auto_exposure_option)); + color_ep.register_option(RS2_OPTION_GAIN, + std::make_shared( + gain_option, + auto_exposure_option)); + + // Starting with firmware 5.10.9, auto-exposure ROI is available for color sensor + if (_fw_version >= firmware_version("5.10.9.0")) + { + roi_sensor_interface* roi_sensor; + if ((roi_sensor = dynamic_cast(&color_ep))) + roi_sensor->set_roi_method(std::make_shared(*_hw_monitor, ds::fw_cmd::SETRGBAEROI)); + } + + // Register for tracking of thermal compensation changes + if (val_in_range(_pid, { ds::RS455_PID })) + { + if (_thermal_monitor) + _thermal_monitor->add_observer([&](float){ + _color_calib_table_raw.reset(); } ); + } + + 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) } - - // Register for tracking of thermal compensation changes - if (val_in_range(_pid, { ds::RS455_PID })) + else { - if (_thermal_monitor) - _thermal_monitor->add_observer([&](float){ - _color_calib_table_raw.reset(); } ); + // attributes of md_rgb_control + 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)); } - 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_FRAME_TIMESTAMP, make_uvc_header_parser(&platform::uvc_header::timestamp)); + color_ep.register_metadata(RS2_FRAME_METADATA_ACTUAL_FPS, std::make_shared(false, [](const rs2_metadata_type& param) + {return param*100;})); //the units of the exposure of the RGB sensor are 100 microseconds so the md_attribute_actual_fps need the lambda to convert it to microseconds + + // attributes of md_capture_timing + auto md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_rgb_mode, rgb_mode) + + offsetof(md_rgb_normal_mode, intel_capture_timing); + + color_ep.register_metadata(RS2_FRAME_METADATA_FRAME_COUNTER, make_attribute_parser(&md_capture_timing::frame_counter, md_capture_timing_attributes::frame_counter_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, make_rs400_sensor_ts_parser(make_uvc_header_parser(&platform::uvc_header::timestamp), + make_attribute_parser(&md_capture_timing::sensor_timestamp, md_capture_timing_attributes::sensor_timestamp_attribute, md_prop_offset))); - 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) - } - else - { // attributes of md_rgb_control - auto md_prop_offset = offsetof(metadata_raw, mode) + + 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)); - } - - color_ep.register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_uvc_header_parser(&platform::uvc_header::timestamp)); - color_ep.register_metadata(RS2_FRAME_METADATA_ACTUAL_FPS, std::make_shared(false, [](const rs2_metadata_type& param) - {return param * 100; })); //the units of the exposure of the RGB sensor are 100 microseconds so the md_attribute_actual_fps need the lambda to convert it to microseconds - - // attributes of md_capture_timing - auto md_prop_offset = offsetof(metadata_raw, mode) + - offsetof(md_rgb_mode, rgb_mode) + - offsetof(md_rgb_normal_mode, intel_capture_timing); - - color_ep.register_metadata(RS2_FRAME_METADATA_FRAME_COUNTER, make_attribute_parser(&md_capture_timing::frame_counter, md_capture_timing_attributes::frame_counter_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, make_rs400_sensor_ts_parser(make_uvc_header_parser(&platform::uvc_header::timestamp), - make_attribute_parser(&md_capture_timing::sensor_timestamp, md_capture_timing_attributes::sensor_timestamp_attribute, md_prop_offset))); - - // attributes of md_rgb_control - 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_GAIN_LEVEL, make_attribute_parser(&md_rgb_control::gain, md_rgb_control_attributes::gain_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE, make_attribute_parser(&md_rgb_control::manual_exp, md_rgb_control_attributes::manual_exp_attribute, md_prop_offset)); - - // attributes of md_capture_stats - md_prop_offset = offsetof(metadata_raw, mode) + - offsetof(md_rgb_mode, rgb_mode) + - offsetof(md_rgb_normal_mode, intel_capture_stats); - - color_ep.register_metadata(RS2_FRAME_METADATA_WHITE_BALANCE, make_attribute_parser(&md_capture_stats::white_balance, md_capture_stat_attributes::white_balance_attribute, md_prop_offset)); - - // attributes of md_rgb_control - 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_BRIGHTNESS, - make_attribute_parser(&md_rgb_control::brightness, md_rgb_control_attributes::brightness_attribute, md_prop_offset, + color_ep.register_metadata(RS2_FRAME_METADATA_GAIN_LEVEL, make_attribute_parser(&md_rgb_control::gain, md_rgb_control_attributes::gain_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE, make_attribute_parser(&md_rgb_control::manual_exp, md_rgb_control_attributes::manual_exp_attribute, md_prop_offset)); + + // attributes of md_capture_stats + md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_rgb_mode, rgb_mode) + + offsetof(md_rgb_normal_mode, intel_capture_stats); + + color_ep.register_metadata(RS2_FRAME_METADATA_WHITE_BALANCE, make_attribute_parser(&md_capture_stats::white_balance, md_capture_stat_attributes::white_balance_attribute, md_prop_offset)); + + // attributes of md_rgb_control + 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_BRIGHTNESS, + make_attribute_parser(&md_rgb_control::brightness, md_rgb_control_attributes::brightness_attribute, md_prop_offset, + [](const rs2_metadata_type& param) { + // cast to short in order to return negative values + return *(short*)&(param); + })); + color_ep.register_metadata(RS2_FRAME_METADATA_CONTRAST, make_attribute_parser(&md_rgb_control::contrast, md_rgb_control_attributes::contrast_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_SATURATION, make_attribute_parser(&md_rgb_control::saturation, md_rgb_control_attributes::saturation_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_SHARPNESS, make_attribute_parser(&md_rgb_control::sharpness, md_rgb_control_attributes::sharpness_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_AUTO_WHITE_BALANCE_TEMPERATURE, make_attribute_parser(&md_rgb_control::awb_temp, md_rgb_control_attributes::awb_temp_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_BACKLIGHT_COMPENSATION, make_attribute_parser(&md_rgb_control::backlight_comp, md_rgb_control_attributes::backlight_comp_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_GAMMA, make_attribute_parser(&md_rgb_control::gamma, md_rgb_control_attributes::gamma_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_HUE, make_attribute_parser(&md_rgb_control::hue, md_rgb_control_attributes::hue_attribute, md_prop_offset, [](const rs2_metadata_type& param) { // cast to short in order to return negative values return *(short*)&(param); })); - color_ep.register_metadata(RS2_FRAME_METADATA_CONTRAST, make_attribute_parser(&md_rgb_control::contrast, md_rgb_control_attributes::contrast_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_SATURATION, make_attribute_parser(&md_rgb_control::saturation, md_rgb_control_attributes::saturation_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_SHARPNESS, make_attribute_parser(&md_rgb_control::sharpness, md_rgb_control_attributes::sharpness_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_AUTO_WHITE_BALANCE_TEMPERATURE, make_attribute_parser(&md_rgb_control::awb_temp, md_rgb_control_attributes::awb_temp_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_BACKLIGHT_COMPENSATION, make_attribute_parser(&md_rgb_control::backlight_comp, md_rgb_control_attributes::backlight_comp_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_GAMMA, make_attribute_parser(&md_rgb_control::gamma, md_rgb_control_attributes::gamma_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_HUE, make_attribute_parser(&md_rgb_control::hue, md_rgb_control_attributes::hue_attribute, md_prop_offset, - [](const rs2_metadata_type& param) { - // cast to short in order to return negative values - return *(short*)&(param); - })); - color_ep.register_metadata(RS2_FRAME_METADATA_MANUAL_WHITE_BALANCE, make_attribute_parser(&md_rgb_control::manual_wb, md_rgb_control_attributes::manual_wb_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_POWER_LINE_FREQUENCY, make_attribute_parser(&md_rgb_control::power_line_frequency, md_rgb_control_attributes::power_line_frequency_attribute, md_prop_offset)); - color_ep.register_metadata(RS2_FRAME_METADATA_LOW_LIGHT_COMPENSATION, make_attribute_parser(&md_rgb_control::low_light_comp, md_rgb_control_attributes::low_light_comp_attribute, md_prop_offset)); - - - color_ep.register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_YUYV, map_supported_color_formats(RS2_FORMAT_YUYV), RS2_STREAM_COLOR)); - color_ep.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_RAW16, RS2_STREAM_COLOR)); - - if (_pid == ds::RS465_PID) - { - color_ep.register_processing_block({ {RS2_FORMAT_MJPEG} }, { {RS2_FORMAT_RGB8, RS2_STREAM_COLOR} }, []() { return std::make_shared(RS2_FORMAT_RGB8); }); - color_ep.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_MJPEG, RS2_STREAM_COLOR)); - } + color_ep.register_metadata(RS2_FRAME_METADATA_MANUAL_WHITE_BALANCE, make_attribute_parser(&md_rgb_control::manual_wb, md_rgb_control_attributes::manual_wb_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_POWER_LINE_FREQUENCY, make_attribute_parser(&md_rgb_control::power_line_frequency, md_rgb_control_attributes::power_line_frequency_attribute, md_prop_offset)); + color_ep.register_metadata(RS2_FRAME_METADATA_LOW_LIGHT_COMPENSATION, make_attribute_parser(&md_rgb_control::low_light_comp, md_rgb_control_attributes::low_light_comp_attribute, md_prop_offset)); + + // Starting with firmware 5.10.9, auto-exposure ROI is available for color sensor + } + } //D431 + + color_ep.register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_YUYV, map_supported_color_formats(RS2_FORMAT_YUYV), RS2_STREAM_COLOR)); + color_ep.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_RAW16, RS2_STREAM_COLOR)); + + if (_pid == ds::RS465_PID) + { + color_ep.register_processing_block({ {RS2_FORMAT_MJPEG} }, { {RS2_FORMAT_RGB8, RS2_STREAM_COLOR} }, []() { return std::make_shared(RS2_FORMAT_RGB8); }); + color_ep.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_MJPEG, RS2_STREAM_COLOR)); + } + } rs2_intrinsics ds5_color_sensor::get_intrinsics(const stream_profile& profile) const { + if ("ABCD" == get_info(RS2_CAMERA_INFO_PRODUCT_ID)) // RS431 Development. to be removed. TODO + { + rs2_intrinsics intr{}; + intr.width =640; + intr.height =640; + intr.fx =320.f; + intr.fy =240.f; + intr.ppx =630.f; + intr.ppy =630.f; + intr.model = RS2_DISTORTION_NONE; + return intr; + } + return get_intrinsic_by_resolution( *_owner->_color_calib_table_raw, ds::calibration_table_id::rgb_calibration_id, diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index be1c6c0010..37f23cf848 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -380,6 +380,19 @@ namespace librealsense } else { + if ("ABCD" == get_info(RS2_CAMERA_INFO_PRODUCT_ID)) // RS431 Development. to be removed. TODO + { + rs2_intrinsics intr{}; + intr.width =640; + intr.height =640; + intr.fx =320.f; + intr.fy =240.f; + intr.ppx =630.f; + intr.ppy =630.f; + intr.model = RS2_DISTORTION_NONE; + return intr; + } + return get_intrinsic_by_resolution( *_owner->_coefficients_table_raw, ds::calibration_table_id::coefficients_table_id, @@ -640,6 +653,9 @@ namespace librealsense float ds5_device::get_stereo_baseline_mm() const { using namespace ds; + if ("ABCD" == get_info(RS2_CAMERA_INFO_PRODUCT_ID)) // RS431 Development. to be removed. TODO + return 55; + auto table = check_calib(*_coefficients_table_raw); return fabs(table->baseline); } @@ -755,6 +771,8 @@ namespace librealsense auto&& backend = ctx->get_backend(); auto& raw_sensor = get_raw_depth_sensor(); auto pid = group.uvc_devices.front().pid; + auto pid_hex_str = hexify(pid); + bool mipi_sensor= (RS431_PID ==pid); _color_calib_table_raw = [this]() { @@ -771,7 +789,8 @@ namespace librealsense } else { - _hw_monitor = std::make_shared( + if (!mipi_sensor) + _hw_monitor = std::make_shared( std::make_shared( backend.create_usb_device(group.usb_devices.front()), raw_sensor)); } @@ -804,27 +823,37 @@ namespace librealsense auto& depth_sensor = get_depth_sensor(); auto& raw_depth_sensor = get_raw_depth_sensor(); - using namespace platform; - - // minimal firmware version in which hdr feature is supported - firmware_version hdr_firmware_version("5.12.8.100"); - + std::string optic_serial; + std::string asic_serial; + std::string fwv; + if (!mipi_sensor) + { std::string optic_serial, asic_serial, pid_hex_str, usb_type_str; bool advanced_mode, usb_modality; group_multiple_fw_calls(depth_sensor, [&]() { + _hw_monitor->get_gvd(gvd_buff.size(), gvd_buff.data(), GVD); + // fooling tests recordings - don't remove + _hw_monitor->get_gvd(gvd_buff.size(), gvd_buff.data(), GVD); + + optic_serial = _hw_monitor->get_module_serial_string(gvd_buff, module_serial_offset); + asic_serial = _hw_monitor->get_module_serial_string(gvd_buff, module_asic_serial_offset); + fwv = _hw_monitor->get_firmware_version_string(gvd_buff, camera_fw_version_offset); + } + } + else + { + fwv = "1.1.1.1"; // D431 temporal + optic_serial = "11114444"; + asic_serial = "22223333"; + } - _hw_monitor->get_gvd(gvd_buff.size(), gvd_buff.data(), GVD); - - optic_serial = _hw_monitor->get_module_serial_string(gvd_buff, module_serial_offset); - asic_serial = _hw_monitor->get_module_serial_string(gvd_buff, module_asic_serial_offset); - auto fwv = _hw_monitor->get_firmware_version_string(gvd_buff, camera_fw_version_offset); _fw_version = firmware_version(fwv); _recommended_fw_version = firmware_version(D4XX_RECOMMENDED_FIRMWARE_VERSION); if (_fw_version >= firmware_version("5.10.4.0")) _device_capabilities = parse_device_capabilities(); - advanced_mode = is_camera_in_advanced_mode(); + auto advanced_mode = is_camera_in_advanced_mode(); auto _usb_mode = usb3_type; usb_type_str = usb_spec_names.at(_usb_mode); @@ -854,7 +883,6 @@ namespace librealsense { {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1}, {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2} }, []() {return std::make_shared(); } ); - pid_hex_str = hexify(_pid); if ((_pid == RS416_PID || _pid == RS416_RGB_PID) && _fw_version >= firmware_version("5.12.0.1")) @@ -1090,9 +1118,7 @@ namespace librealsense lazy([default_depth_units]() { return default_depth_units; }))); } - - // Metadata registration - depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_uvc_header_parser(&uvc_header::timestamp)); + // Metadata registration // Auto exposure and gain limit if (_fw_version >= firmware_version("5.12.10.11")) @@ -1128,90 +1154,97 @@ namespace librealsense } }); //group_multiple_fw_calls - // attributes of md_capture_timing - auto md_prop_offset = offsetof(metadata_raw, mode) + - offsetof(md_depth_mode, depth_y_mode) + - offsetof(md_depth_y_normal_mode, intel_capture_timing); - - depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_COUNTER, make_attribute_parser(&md_capture_timing::frame_counter, md_capture_timing_attributes::frame_counter_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, make_rs400_sensor_ts_parser(make_uvc_header_parser(&uvc_header::timestamp), - make_attribute_parser(&md_capture_timing::sensor_timestamp, md_capture_timing_attributes::sensor_timestamp_attribute, md_prop_offset))); - - // attributes of md_capture_stats - md_prop_offset = offsetof(metadata_raw, mode) + - offsetof(md_depth_mode, depth_y_mode) + - offsetof(md_depth_y_normal_mode, intel_capture_stats); - - depth_sensor.register_metadata(RS2_FRAME_METADATA_WHITE_BALANCE, make_attribute_parser(&md_capture_stats::white_balance, md_capture_stat_attributes::white_balance_attribute, md_prop_offset)); - - // attributes of md_depth_control - md_prop_offset = offsetof(metadata_raw, mode) + - offsetof(md_depth_mode, depth_y_mode) + - offsetof(md_depth_y_normal_mode, intel_depth_control); - - depth_sensor.register_metadata(RS2_FRAME_METADATA_GAIN_LEVEL, make_attribute_parser(&md_depth_control::manual_gain, md_depth_control_attributes::gain_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE, make_attribute_parser(&md_depth_control::manual_exposure, md_depth_control_attributes::exposure_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_AUTO_EXPOSURE, make_attribute_parser(&md_depth_control::auto_exposure_mode, md_depth_control_attributes::ae_mode_attribute, md_prop_offset)); - - depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_LASER_POWER, make_attribute_parser(&md_depth_control::laser_power, md_depth_control_attributes::laser_pwr_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_LASER_POWER_MODE, make_attribute_parser(&md_depth_control::emitterMode, md_depth_control_attributes::emitter_mode_attribute, md_prop_offset, - [](const rs2_metadata_type& param) { return param == 1 ? 1 : 0; })); // starting at version 2.30.1 this control is superceeded by RS2_FRAME_METADATA_FRAME_EMITTER_MODE - depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_PRIORITY, make_attribute_parser(&md_depth_control::exposure_priority, md_depth_control_attributes::exposure_priority_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_ROI_LEFT, make_attribute_parser(&md_depth_control::exposure_roi_left, md_depth_control_attributes::roi_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_ROI_RIGHT, make_attribute_parser(&md_depth_control::exposure_roi_right, md_depth_control_attributes::roi_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_ROI_TOP, make_attribute_parser(&md_depth_control::exposure_roi_top, md_depth_control_attributes::roi_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_ROI_BOTTOM, make_attribute_parser(&md_depth_control::exposure_roi_bottom, md_depth_control_attributes::roi_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_EMITTER_MODE, make_attribute_parser(&md_depth_control::emitterMode, md_depth_control_attributes::emitter_mode_attribute, md_prop_offset)); - depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_LED_POWER, make_attribute_parser(&md_depth_control::ledPower, md_depth_control_attributes::led_power_attribute, md_prop_offset)); - - // md_configuration - will be used for internal validation only - md_prop_offset = offsetof(metadata_raw, mode) + offsetof(md_depth_mode, depth_y_mode) + offsetof(md_depth_y_normal_mode, intel_configuration); - - depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_HW_TYPE, make_attribute_parser(&md_configuration::hw_type, md_configuration_attributes::hw_type_attribute, md_prop_offset)); - depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_SKU_ID, make_attribute_parser(&md_configuration::sku_id, md_configuration_attributes::sku_id_attribute, md_prop_offset)); - depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_FORMAT, make_attribute_parser(&md_configuration::format, md_configuration_attributes::format_attribute, md_prop_offset)); - depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_WIDTH, make_attribute_parser(&md_configuration::width, md_configuration_attributes::width_attribute, md_prop_offset)); - depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_HEIGHT, make_attribute_parser(&md_configuration::height, md_configuration_attributes::height_attribute, md_prop_offset)); - depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_ACTUAL_FPS, std::make_shared ()); - - if (_fw_version >= firmware_version("5.12.7.0")) + if (!mipi_sensor) { - depth_sensor.register_metadata(RS2_FRAME_METADATA_GPIO_INPUT_DATA, make_attribute_parser(&md_configuration::gpioInputData, md_configuration_attributes::gpio_input_data_attribute, md_prop_offset)); - } + // Metadata registration + depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_uvc_header_parser(&uvc_header::timestamp)); - if (_fw_version >= hdr_firmware_version) - { // attributes of md_capture_timing - auto md_prop_offset = offsetof(metadata_raw, mode) + offsetof(md_depth_mode, depth_y_mode) + offsetof(md_depth_y_normal_mode, intel_configuration); - - depth_sensor.register_metadata(RS2_FRAME_METADATA_SEQUENCE_SIZE, - make_attribute_parser(&md_configuration::sub_preset_info, - md_configuration_attributes::sub_preset_info_attribute, md_prop_offset , - [](const rs2_metadata_type& param) { - // bit mask and offset used to get data from bitfield - return (param & md_configuration::SUB_PRESET_BIT_MASK_SEQUENCE_SIZE) - >> md_configuration::SUB_PRESET_BIT_OFFSET_SEQUENCE_SIZE; - })); - - depth_sensor.register_metadata(RS2_FRAME_METADATA_SEQUENCE_ID, - make_attribute_parser(&md_configuration::sub_preset_info, - md_configuration_attributes::sub_preset_info_attribute, md_prop_offset , - [](const rs2_metadata_type& param) { - // bit mask and offset used to get data from bitfield - return (param & md_configuration::SUB_PRESET_BIT_MASK_SEQUENCE_ID) - >> md_configuration::SUB_PRESET_BIT_OFFSET_SEQUENCE_ID; - })); - - depth_sensor.register_metadata(RS2_FRAME_METADATA_SEQUENCE_NAME, - make_attribute_parser(&md_configuration::sub_preset_info, - md_configuration_attributes::sub_preset_info_attribute, md_prop_offset, - [](const rs2_metadata_type& param) { - // bit mask and offset used to get data from bitfield - return (param & md_configuration::SUB_PRESET_BIT_MASK_ID) - >> md_configuration::SUB_PRESET_BIT_OFFSET_ID; - })); - } + auto md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_depth_mode, depth_y_mode) + + offsetof(md_depth_y_normal_mode, intel_capture_timing); + + depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_COUNTER, make_attribute_parser(&md_capture_timing::frame_counter, md_capture_timing_attributes::frame_counter_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, make_rs400_sensor_ts_parser(make_uvc_header_parser(&uvc_header::timestamp), + make_attribute_parser(&md_capture_timing::sensor_timestamp, md_capture_timing_attributes::sensor_timestamp_attribute, md_prop_offset))); + + // attributes of md_capture_stats + md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_depth_mode, depth_y_mode) + + offsetof(md_depth_y_normal_mode, intel_capture_stats); + + depth_sensor.register_metadata(RS2_FRAME_METADATA_WHITE_BALANCE, make_attribute_parser(&md_capture_stats::white_balance, md_capture_stat_attributes::white_balance_attribute, md_prop_offset)); + + // attributes of md_depth_control + md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_depth_mode, depth_y_mode) + + offsetof(md_depth_y_normal_mode, intel_depth_control); + + + depth_sensor.register_metadata(RS2_FRAME_METADATA_GAIN_LEVEL, make_attribute_parser(&md_depth_control::manual_gain, md_depth_control_attributes::gain_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE, make_attribute_parser(&md_depth_control::manual_exposure, md_depth_control_attributes::exposure_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_AUTO_EXPOSURE, make_attribute_parser(&md_depth_control::auto_exposure_mode, md_depth_control_attributes::ae_mode_attribute, md_prop_offset)); + + depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_LASER_POWER, make_attribute_parser(&md_depth_control::laser_power, md_depth_control_attributes::laser_pwr_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_LASER_POWER_MODE, make_attribute_parser(&md_depth_control::emitterMode, md_depth_control_attributes::emitter_mode_attribute, md_prop_offset, + [](const rs2_metadata_type& param) { return param == 1 ? 1 : 0; })); // starting at version 2.30.1 this control is superceeded by RS2_FRAME_METADATA_FRAME_EMITTER_MODE + depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_PRIORITY, make_attribute_parser(&md_depth_control::exposure_priority, md_depth_control_attributes::exposure_priority_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_ROI_LEFT, make_attribute_parser(&md_depth_control::exposure_roi_left, md_depth_control_attributes::roi_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_ROI_RIGHT, make_attribute_parser(&md_depth_control::exposure_roi_right, md_depth_control_attributes::roi_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_ROI_TOP, make_attribute_parser(&md_depth_control::exposure_roi_top, md_depth_control_attributes::roi_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_EXPOSURE_ROI_BOTTOM, make_attribute_parser(&md_depth_control::exposure_roi_bottom, md_depth_control_attributes::roi_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_EMITTER_MODE, make_attribute_parser(&md_depth_control::emitterMode, md_depth_control_attributes::emitter_mode_attribute, md_prop_offset)); + depth_sensor.register_metadata(RS2_FRAME_METADATA_FRAME_LED_POWER, make_attribute_parser(&md_depth_control::ledPower, md_depth_control_attributes::led_power_attribute, md_prop_offset)); + + // md_configuration - will be used for internal validation only + md_prop_offset = offsetof(metadata_raw, mode) + offsetof(md_depth_mode, depth_y_mode) + offsetof(md_depth_y_normal_mode, intel_configuration); + + depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_HW_TYPE, make_attribute_parser(&md_configuration::hw_type, md_configuration_attributes::hw_type_attribute, md_prop_offset)); + depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_SKU_ID, make_attribute_parser(&md_configuration::sku_id, md_configuration_attributes::sku_id_attribute, md_prop_offset)); + depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_FORMAT, make_attribute_parser(&md_configuration::format, md_configuration_attributes::format_attribute, md_prop_offset)); + depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_WIDTH, make_attribute_parser(&md_configuration::width, md_configuration_attributes::width_attribute, md_prop_offset)); + depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_HEIGHT, make_attribute_parser(&md_configuration::height, md_configuration_attributes::height_attribute, md_prop_offset)); + depth_sensor.register_metadata((rs2_frame_metadata_value)RS2_FRAME_METADATA_ACTUAL_FPS, std::make_shared()); + + if (_fw_version >= firmware_version("5.12.7.0")) + { + depth_sensor.register_metadata(RS2_FRAME_METADATA_GPIO_INPUT_DATA, make_attribute_parser(&md_configuration::gpioInputData, md_configuration_attributes::gpio_input_data_attribute, md_prop_offset)); + } + + if (_fw_version >= hdr_firmware_version) + { + // attributes of md_capture_timing + auto md_prop_offset = offsetof(metadata_raw, mode) + offsetof(md_depth_mode, depth_y_mode) + offsetof(md_depth_y_normal_mode, intel_configuration); + + depth_sensor.register_metadata(RS2_FRAME_METADATA_SEQUENCE_SIZE, + make_attribute_parser(&md_configuration::sub_preset_info, + md_configuration_attributes::sub_preset_info_attribute, md_prop_offset, + [](const rs2_metadata_type& param) { + // bit mask and offset used to get data from bitfield + return (param & md_configuration::SUB_PRESET_BIT_MASK_SEQUENCE_SIZE) + >> md_configuration::SUB_PRESET_BIT_OFFSET_SEQUENCE_SIZE; + })); + + depth_sensor.register_metadata(RS2_FRAME_METADATA_SEQUENCE_ID, + make_attribute_parser(&md_configuration::sub_preset_info, + md_configuration_attributes::sub_preset_info_attribute, md_prop_offset, + [](const rs2_metadata_type& param) { + // bit mask and offset used to get data from bitfield + return (param & md_configuration::SUB_PRESET_BIT_MASK_SEQUENCE_ID) + >> md_configuration::SUB_PRESET_BIT_OFFSET_SEQUENCE_ID; + })); + + depth_sensor.register_metadata(RS2_FRAME_METADATA_SEQUENCE_NAME, + make_attribute_parser(&md_configuration::sub_preset_info, + md_configuration_attributes::sub_preset_info_attribute, md_prop_offset, + [](const rs2_metadata_type& param) { + // bit mask and offset used to get data from bitfield + return (param & md_configuration::SUB_PRESET_BIT_MASK_ID) + >> md_configuration::SUB_PRESET_BIT_OFFSET_ID; + })); + } + } //mipi register_info(RS2_CAMERA_INFO_NAME, device_name); register_info(RS2_CAMERA_INFO_SERIAL_NUMBER, optic_serial); diff --git a/src/ds5/ds5-factory.cpp b/src/ds5/ds5-factory.cpp index 48d13223be..c4c0335f48 100644 --- a/src/ds5/ds5-factory.cpp +++ b/src/ds5/ds5-factory.cpp @@ -568,7 +568,7 @@ namespace librealsense }; }; - // AWGC + // D431 Development class rs431_device : public ds5_active, public ds5_color, public ds5_advanced_mode_base diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 7ca06da595..ff1182b387 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -663,7 +663,7 @@ namespace librealsense info.conn_spec = usb_specification; info.uvc_capabilities = get_dev_capabilities(dev_name); - std::cout << "Device " << name << ":\n" << std::string(info); + //std::cout << "Device " << name << ":\n" << std::string(info); uvc_nodes.emplace_back(info, dev_name); } @@ -1655,7 +1655,7 @@ namespace librealsense fmt.fmt.pix.field = V4L2_FIELD_NONE; if(xioctl(_fd, VIDIOC_S_FMT, &fmt) < 0) { - throw linux_backend_exception("xioctl(VIDIOC_S_FMT) failed"); + throw linux_backend_exception(to_string() << "xioctl(VIDIOC_S_FMT) failed, errno=" << errno); } else LOG_INFO("Video node was successfully configured to " << fourcc_to_string(fmt.fmt.pix.pixelformat) << " format" <<", fd " << std::dec << _fd); @@ -1925,8 +1925,9 @@ namespace librealsense std::shared_ptr v4l_backend::create_uvc_device(uvc_device_info info) const { - auto v4l_uvc_dev = (!info.has_metadata_node) ? std::make_shared(info) : - std::make_shared(info); + bool use_memory_map = 0xABCD == info.pid; // D431 development. Not for upstream + auto v4l_uvc_dev = (!info.has_metadata_node) ? std::make_shared(info,use_memory_map) : + std::make_shared(info,use_memory_map); return std::make_shared(v4l_uvc_dev); } diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index 328ed1fbea..69723d4e01 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -375,6 +375,23 @@ namespace librealsense stream_profile _md_profile; }; + // D431 Development. To be merged into underlying class + class v4l_mipi_device : public v4l_uvc_device + { + public: + v4l_mipi_device(const uvc_device_info& info, bool use_memory_map = t); + + ~v4l_mipi_device(); + + void init_xu(const extension_unit&) override; + bool get_pu(rs2_option opt, int32_t& value) const override; + bool set_pu(rs2_option opt, int32_t value) override; + bool set_xu(const extension_unit& xu, uint8_t control, const uint8_t* data, int size) override; + bool get_xu(const extension_unit& xu, uint8_t control, uint8_t* data, int size) const override; + control_range get_xu_range(const extension_unit& xu, uint8_t control, int len) const override; + control_range get_pu_range(rs2_option option) const override; + }; + class v4l_backend : public backend { public: From 145c4669dfe27ae1bffcd4223c3acd1987c4b512 Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Tue, 26 Nov 2019 09:28:47 +0000 Subject: [PATCH 004/361] Enabling controls: Exposuer and Auto-Exposure for RGB sensor Work-around for improper enumeration exposing RGB's YUYV as UYVY --- src/ds5/ds5-color.cpp | 76 ++++++++++++++----------- src/linux/backend-v4l2.cpp | 112 ++++++++++++++++++++++++++++++++++++- src/linux/backend-v4l2.h | 3 +- 3 files changed, 153 insertions(+), 38 deletions(-) diff --git a/src/ds5/ds5-color.cpp b/src/ds5/ds5-color.cpp index b5e4f08fa6..c7d49c3da7 100644 --- a/src/ds5/ds5-color.cpp +++ b/src/ds5/ds5-color.cpp @@ -104,7 +104,7 @@ namespace librealsense auto& color_ep = get_color_sensor(); auto& raw_color_ep = get_raw_color_sensor(); - if (ds::RS431_PID != color_devices_info.front().pid) + if (ds::RS431_PID != _pid) { color_ep.register_pu(RS2_OPTION_BRIGHTNESS); color_ep.register_pu(RS2_OPTION_CONTRAST); @@ -158,33 +158,37 @@ namespace librealsense std::make_shared( exposure_option, auto_exposure_option)); - color_ep.register_option(RS2_OPTION_GAIN, - std::make_shared( - gain_option, - auto_exposure_option)); - - // Starting with firmware 5.10.9, auto-exposure ROI is available for color sensor - if (_fw_version >= firmware_version("5.10.9.0")) - { - roi_sensor_interface* roi_sensor; - if ((roi_sensor = dynamic_cast(&color_ep))) - roi_sensor->set_roi_method(std::make_shared(*_hw_monitor, ds::fw_cmd::SETRGBAEROI)); - } - // Register for tracking of thermal compensation changes - if (val_in_range(_pid, { ds::RS455_PID })) + if (ds::RS431_PID != _pid) { - if (_thermal_monitor) - _thermal_monitor->add_observer([&](float){ - _color_calib_table_raw.reset(); } ); + color_ep.register_option(RS2_OPTION_GAIN, + std::make_shared( + gain_option, + auto_exposure_option)); + + // Starting with firmware 5.10.9, auto-exposure ROI is available for color sensor + if (_fw_version >= firmware_version("5.10.9.0")) + { + roi_sensor_interface* roi_sensor; + if ((roi_sensor = dynamic_cast(&color_ep))) + roi_sensor->set_roi_method(std::make_shared(*_hw_monitor, ds::fw_cmd::SETRGBAEROI)); + } + + // Register for tracking of thermal compensation changes + if (val_in_range(_pid, { ds::RS455_PID })) + { + if (_thermal_monitor) + _thermal_monitor->add_observer([&](float) { + _color_calib_table_raw.reset(); }); + } + + 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) } - - 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) } else { @@ -251,17 +255,23 @@ namespace librealsense color_ep.register_metadata(RS2_FRAME_METADATA_LOW_LIGHT_COMPENSATION, make_attribute_parser(&md_rgb_control::low_light_comp, md_rgb_control_attributes::low_light_comp_attribute, md_prop_offset)); // Starting with firmware 5.10.9, auto-exposure ROI is available for color sensor - } + + color_ep.register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_UYVY, map_supported_color_formats(RS2_FORMAT_UYVY), RS2_STREAM_COLOR)); + } //D431 + else + { + // Work-around for improper enumeration given to RGB output as UYUV instead of YUYV + color_ep.register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_UYVY, map_supported_color_formats(RS2_FORMAT_UYVY), RS2_STREAM_COLOR)); + } - color_ep.register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_YUYV, map_supported_color_formats(RS2_FORMAT_YUYV), RS2_STREAM_COLOR)); - 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_id_pbf(RS2_FORMAT_RAW16, RS2_STREAM_COLOR)); - if (_pid == ds::RS465_PID) - { - color_ep.register_processing_block({ {RS2_FORMAT_MJPEG} }, { {RS2_FORMAT_RGB8, RS2_STREAM_COLOR} }, []() { return std::make_shared(RS2_FORMAT_RGB8); }); - color_ep.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_MJPEG, RS2_STREAM_COLOR)); - } + if (_pid == ds::RS465_PID) + { + color_ep.register_processing_block({ {RS2_FORMAT_MJPEG} }, { {RS2_FORMAT_RGB8, RS2_STREAM_COLOR} }, []() { return std::make_shared(RS2_FORMAT_RGB8); }); + color_ep.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_MJPEG, RS2_STREAM_COLOR)); + } } diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index ff1182b387..1ac3a0555c 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -54,6 +54,24 @@ const size_t MAX_DEV_PARENT_DIR = 10; #include "../tm2/tm-boot.h" +//D431 Dev. TODO -shall be refactored into the kernel headers. +const uint32_t RS_STREAM_CONFIG_0 = 0x4000; +const uint32_t RS_CAMERA_CID_BASE = (V4L2_CTRL_CLASS_CAMERA | RS_STREAM_CONFIG_0); +const uint32_t RS_CAMERA_CID_LASER_POWER = (RS_CAMERA_CID_BASE+1); +const uint32_t RS_CAMERA_CID_MANUAL_LASER_POWER = (RS_CAMERA_CID_BASE+2); +const uint32_t RS_CAMERA_DEPTH_CALIBRATION_TABLE_GET = (RS_CAMERA_CID_BASE+3); +const uint32_t RS_CAMERA_DEPTH_CALIBRATION_TABLE_SET = (RS_CAMERA_CID_BASE+4); +const uint32_t RS_CAMERA_COEFF_CALIBRATION_TABLE_GET = (RS_CAMERA_CID_BASE+5); +const uint32_t RS_CAMERA_COEFF_CALIBRATION_TABLE_SET = (RS_CAMERA_CID_BASE+6); +const uint32_t RS_CAMERA_CID_FW_VERSION = (RS_CAMERA_CID_BASE+7); +const uint32_t RS_CAMERA_CID_GVD = (RS_CAMERA_CID_BASE+8); +const uint32_t RS_CAMERA_CID_AE_ROI_GET = (RS_CAMERA_CID_BASE+9); +const uint32_t RS_CAMERA_CID_AE_ROI_SET = (RS_CAMERA_CID_BASE+10); +const uint32_t RS_CAMERA_CID_AE_SETPOINT_GET = (RS_CAMERA_CID_BASE+11); +const uint32_t RS_CAMERA_CID_AE_SETPOINT_SET = (RS_CAMERA_CID_BASE+12); +const uint32_t RS_CAMERA_CID_ERB = (RS_CAMERA_CID_BASE+13); +const uint32_t RS_CAMERA_CID_EWB = (RS_CAMERA_CID_BASE+14); +const uint32_t RS_CAMERA_CID_HWMC = (RS_CAMERA_CID_BASE+15); #ifdef ANDROID @@ -1923,11 +1941,99 @@ namespace librealsense } } + v4l_mipi_device::v4l_mipi_device(const uvc_device_info& info, bool use_memory_map): + v4l_uvc_device(info,use_memory_map) + {} + + v4l_mipi_device::~v4l_mipi_device() + {} + + bool v4l_mipi_device::get_pu(rs2_option opt, int32_t& value) const + { + v4l2_ext_control control{get_cid(opt), 0, 0, 0}; + v4l2_ext_controls ctrls_block { V4L2_CTRL_CLASS_CAMERA, 1, 0, {0 ,0}, &control}; + + if (xioctl(_fd, VIDIOC_G_EXT_CTRLS, &ctrls_block) < 0) + { + if (errno == EIO || errno == EAGAIN) // TODO: Log? + return false; + + throw linux_backend_exception("xioctl(VIDIOC_G_EXT_CTRLS) failed"); + } + + //if (RS2_OPTION_ENABLE_AUTO_EXPOSURE==opt) { control.value = (V4L2_EXPOSURE_MANUAL==control.value) ? 0 : 1; } + value = control.value; + + return true; + } + bool v4l_mipi_device::set_pu(rs2_option opt, int32_t value) + { + v4l2_ext_control control{get_cid(opt), 0, 0, value}; + v4l2_ext_controls ctrls_block { V4L2_CTRL_CLASS_CAMERA, 1, 0, {0 ,0}, &control}; + //if (RS2_OPTION_ENABLE_AUTO_EXPOSURE==opt) { control.value = value ? V4L2_EXPOSURE_APERTURE_PRIORITY : V4L2_EXPOSURE_MANUAL; } + if (xioctl(_fd, VIDIOC_S_EXT_CTRLS, &ctrls_block) < 0) + { + if (errno == EIO || errno == EAGAIN) // TODO: Log? + return false; + + throw linux_backend_exception("xioctl(VIDIOC_S_EXT_CTRLS) failed"); + } + + return true; + } + bool v4l_mipi_device::set_xu(const extension_unit& xu, uint8_t control, const uint8_t* data, int size) + { + LOG_INFO("Function not implemented - " << __FUNCTION__); + return true; + } + bool v4l_mipi_device::get_xu(const extension_unit& xu, uint8_t control, uint8_t* data, int size) const + { + LOG_INFO("Function not implemented - " << __FUNCTION__); + return true; + } + control_range v4l_mipi_device::get_xu_range(const extension_unit& xu, uint8_t control, int len) const + { + LOG_INFO("Function not implemented - " << __FUNCTION__); + return v4l_uvc_device::get_xu_range(xu, control, len); + } + control_range v4l_mipi_device::get_pu_range(rs2_option option) const + { + return v4l_uvc_device::get_pu_range(option); + } + + // D431 controls map + /* + + */ +// uint32_t v4l_mipi_device::get_cid(rs2_option option) const +// { +// switch(option) +// { +// case RS2_OPTION_BACKLIGHT_COMPENSATION: return V4L2_CID_BACKLIGHT_COMPENSATION; +// case RS2_OPTION_BRIGHTNESS: return V4L2_CID_BRIGHTNESS; +// case RS2_OPTION_CONTRAST: return V4L2_CID_CONTRAST; +// case RS2_OPTION_EXPOSURE: return V4L2_CID_EXPOSURE_ABSOLUTE; // Is this actually valid? I'm getting a lot of VIDIOC error 22s... +// case RS2_OPTION_GAIN: return V4L2_CID_GAIN; +// case RS2_OPTION_GAMMA: return V4L2_CID_GAMMA; +// case RS2_OPTION_HUE: return V4L2_CID_HUE; +// case RS2_OPTION_SATURATION: return V4L2_CID_SATURATION; +// case RS2_OPTION_SHARPNESS: return V4L2_CID_SHARPNESS; +// case RS2_OPTION_WHITE_BALANCE: return V4L2_CID_WHITE_BALANCE_TEMPERATURE; +// case RS2_OPTION_ENABLE_AUTO_EXPOSURE: return V4L2_CID_EXPOSURE_AUTO; // Automatic gain/exposure control +// case RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE: return V4L2_CID_AUTO_WHITE_BALANCE; +// case RS2_OPTION_POWER_LINE_FREQUENCY : return V4L2_CID_POWER_LINE_FREQUENCY; +// case RS2_OPTION_AUTO_EXPOSURE_PRIORITY: return V4L2_CID_EXPOSURE_AUTO_PRIORITY; +// default: throw linux_backend_exception(to_string() << "no v4l2 cid for mipi option " << option); +// } +// } + + std::shared_ptr v4l_backend::create_uvc_device(uvc_device_info info) const { - bool use_memory_map = 0xABCD == info.pid; // D431 development. Not for upstream - auto v4l_uvc_dev = (!info.has_metadata_node) ? std::make_shared(info,use_memory_map) : - std::make_shared(info,use_memory_map); + bool mipi_device = 0xABCD == info.pid; // D431 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)); return std::make_shared(v4l_uvc_dev); } diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index 69723d4e01..d2695d1c89 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -379,11 +379,10 @@ namespace librealsense class v4l_mipi_device : public v4l_uvc_device { public: - v4l_mipi_device(const uvc_device_info& info, bool use_memory_map = t); + v4l_mipi_device(const uvc_device_info& info, bool use_memory_map = true); ~v4l_mipi_device(); - void init_xu(const extension_unit&) override; bool get_pu(rs2_option opt, int32_t& value) const override; bool set_pu(rs2_option opt, int32_t value) override; bool set_xu(const extension_unit& xu, uint8_t control, const uint8_t* data, int size) override; From fa2376a8d685f979f0d9b28b90a0bf0e3700df5d Mon Sep 17 00:00:00 2001 From: Evgeni Raikhel Date: Tue, 26 Nov 2019 15:27:23 +0000 Subject: [PATCH 005/361] Enabling depth controls --- include/librealsense2/h/rs_option.h | 4 +- src/ds5/ds5-active.cpp | 7 +- src/ds5/ds5-device.cpp | 19 +++-- src/linux/backend-v4l2.cpp | 106 ++++++++++++++++++++-------- src/linux/backend-v4l2.h | 2 + 5 files changed, 96 insertions(+), 42 deletions(-) diff --git a/include/librealsense2/h/rs_option.h b/include/librealsense2/h/rs_option.h index ff9033fa7d..e88b257455 100644 --- a/include/librealsense2/h/rs_option.h +++ b/include/librealsense2/h/rs_option.h @@ -34,12 +34,12 @@ extern "C" { RS2_OPTION_ENABLE_AUTO_EXPOSURE, /**< Enable / disable color image auto-exposure*/ RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, /**< Enable / disable color image auto-white-balance*/ RS2_OPTION_VISUAL_PRESET, /**< Provide access to several recommend sets of option presets for the depth camera */ - RS2_OPTION_LASER_POWER, /**< Power of the laser emitter, with 0 meaning projector off*/ + RS2_OPTION_LASER_POWER, /**< Power of the laser emitter (mW), with 0 meaning projector turned off*/ RS2_OPTION_ACCURACY, /**< Set the number of patterns projected per frame. The higher the accuracy value the more patterns projected. Increasing the number of patterns help to achieve better accuracy. Note that this control is affecting the Depth FPS */ RS2_OPTION_MOTION_RANGE, /**< Motion vs. Range trade-off, with lower values allowing for better motion sensitivity and higher values allowing for better depth range*/ RS2_OPTION_FILTER_OPTION, /**< Set the filter to apply to each depth frame. Each one of the filter is optimized per the application requirements*/ RS2_OPTION_CONFIDENCE_THRESHOLD, /**< The confidence level threshold used by the Depth algorithm pipe to set whether a pixel will get a valid range or will be marked with invalid range*/ - RS2_OPTION_EMITTER_ENABLED, /**< Emitter select: 0 – disable all emitters. 1 – enable laser. 2 – enable auto laser. 3 – enable LED.*/ + RS2_OPTION_EMITTER_ENABLED, /**< Emitter select: 0 - disable all emitters. 1 - enable laser. 2 - enable auto laser. 3 - enable LED.*/ RS2_OPTION_FRAMES_QUEUE_SIZE, /**< Number of frames the user is allowed to keep per stream. Trying to hold-on to more frames will cause frame-drops.*/ RS2_OPTION_TOTAL_FRAME_DROPS, /**< Total number of detected frame drops from all streams */ RS2_OPTION_AUTO_EXPOSURE_MODE, /**< Auto-Exposure modes: Static, Anti-Flicker and Hybrid */ diff --git a/src/ds5/ds5-active.cpp b/src/ds5/ds5-active.cpp index e22803ebf8..c5343b9140 100644 --- a/src/ds5/ds5-active.cpp +++ b/src/ds5/ds5-active.cpp @@ -69,9 +69,10 @@ namespace librealsense } //PROJECTOR TEMPERATURE OPTION - depth_ep.register_option(RS2_OPTION_PROJECTOR_TEMPERATURE, - std::make_shared(raw_depth_ep, - RS2_OPTION_PROJECTOR_TEMPERATURE)); + if (pid != RS431_PID) + depth_ep.register_option(RS2_OPTION_PROJECTOR_TEMPERATURE, + std::make_shared(raw_depth_ep, + RS2_OPTION_PROJECTOR_TEMPERATURE)); } else { diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 37f23cf848..9a1dd9b806 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -820,17 +820,14 @@ namespace librealsense std::vector gvd_buff(HW_MONITOR_BUFFER_SIZE); - auto& depth_sensor = get_depth_sensor(); - auto& raw_depth_sensor = get_raw_depth_sensor(); - std::string optic_serial; std::string asic_serial; std::string fwv; if (!mipi_sensor) { - std::string optic_serial, asic_serial, pid_hex_str, usb_type_str; - bool advanced_mode, usb_modality; - group_multiple_fw_calls(depth_sensor, [&]() { + std::string optic_serial, asic_serial, pid_hex_str, usb_type_str; + bool advanced_mode, usb_modality; + group_multiple_fw_calls(depth_sensor, [&]() { _hw_monitor->get_gvd(gvd_buff.size(), gvd_buff.data(), GVD); // fooling tests recordings - don't remove _hw_monitor->get_gvd(gvd_buff.size(), gvd_buff.data(), GVD); @@ -853,6 +850,9 @@ namespace librealsense if (_fw_version >= firmware_version("5.10.4.0")) _device_capabilities = parse_device_capabilities(); + auto& depth_sensor = get_depth_sensor(); + auto& raw_depth_sensor = get_raw_depth_sensor(); + auto advanced_mode = is_camera_in_advanced_mode(); auto _usb_mode = usb3_type; @@ -895,12 +895,17 @@ namespace librealsense "Set the power level of the LED, with 0 meaning LED off")); } - if (_fw_version >= firmware_version("5.6.3.0")) + if ((_fw_version >= firmware_version("5.6.3.0")) || (_fw_version) == firmware_version("1.1.1.1")) // RS431 Dev + { + if (!mipi_sensor) { _is_locked = _hw_monitor->is_camera_locked(GVD, is_camera_locked_offset); + } } if (_fw_version >= firmware_version("5.5.8.0")) + //if hw_monitor was created by usb replace it with xu + // D400_IMU will remain using USB interface due to HW limitations { depth_sensor.register_option(RS2_OPTION_OUTPUT_TRIGGER_ENABLED, std::make_shared>(raw_depth_sensor, depth_xu, DS5_EXT_TRIGGER, diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 1ac3a0555c..66008544a7 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -1961,16 +1961,15 @@ namespace librealsense throw linux_backend_exception("xioctl(VIDIOC_G_EXT_CTRLS) failed"); } - //if (RS2_OPTION_ENABLE_AUTO_EXPOSURE==opt) { control.value = (V4L2_EXPOSURE_MANUAL==control.value) ? 0 : 1; } value = control.value; return true; } + bool v4l_mipi_device::set_pu(rs2_option opt, int32_t value) { v4l2_ext_control control{get_cid(opt), 0, 0, value}; v4l2_ext_controls ctrls_block { V4L2_CTRL_CLASS_CAMERA, 1, 0, {0 ,0}, &control}; - //if (RS2_OPTION_ENABLE_AUTO_EXPOSURE==opt) { control.value = value ? V4L2_EXPOSURE_APERTURE_PRIORITY : V4L2_EXPOSURE_MANUAL; } if (xioctl(_fd, VIDIOC_S_EXT_CTRLS, &ctrls_block) < 0) { if (errno == EIO || errno == EAGAIN) // TODO: Log? @@ -1981,9 +1980,19 @@ namespace librealsense return true; } + bool v4l_mipi_device::set_xu(const extension_unit& xu, uint8_t control, const uint8_t* data, int size) { - LOG_INFO("Function not implemented - " << __FUNCTION__); + v4l2_ext_control xctrl{xu_to_cid(xu,control), 0, 0, value}; + v4l2_ext_controls ctrls_block { V4L2_CTRL_CLASS_CAMERA, 1, 0, {0 ,0}, &xctrl}; + if (xioctl(_fd, VIDIOC_S_EXT_CTRLS, &ctrls_block) < 0) + { + if (errno == EIO || errno == EAGAIN) // TODO: Log? + return false; + + throw linux_backend_exception("xioctl(VIDIOC_S_EXT_CTRLS) failed"); + } + return true; } bool v4l_mipi_device::get_xu(const extension_unit& xu, uint8_t control, uint8_t* data, int size) const @@ -1991,41 +2000,78 @@ namespace librealsense LOG_INFO("Function not implemented - " << __FUNCTION__); return true; } + control_range v4l_mipi_device::get_xu_range(const extension_unit& xu, uint8_t control, int len) const { - LOG_INFO("Function not implemented - " << __FUNCTION__); - return v4l_uvc_device::get_xu_range(xu, control, len); + v4l2_query_ext_ctrl xctrl_query{}; + xctrl_query.id = xu_to_cid(xu,control); + + if(0 > ioctl(_fd,VIDIOC_QUERY_EXT_CTRL,&xctrl_query)){ + throw linux_backend_exception(to_string() << "xioctl(VIDIOC_QUERY_EXT_CTRL) failed, errno=" << errno); + } + + if ((xctrl_query.elems !=1 ) || + (xctrl_query.minimum < std::numeric_limits::min()) || + (xctrl_query.maximum > std::numeric_limits::max())) + throw linux_backend_exception(to_string() << "Mipi Control range for " << xctrl_query.name + << " is not compliant with backend interface: [min,max,default,step]:\n" + << xctrl_query.minimum << ", " << xctrl_query.maximum << ", " + << xctrl_query.default_value << ", " << xctrl_query.step + << "\n Elements = " << xctrl_query.elems); + + return { static_cast(xctrl_query.minimum), static_cast(xctrl_query.maximum), + static_cast(xctrl_query.default_value), static_cast(xctrl_query.step)}; } + control_range v4l_mipi_device::get_pu_range(rs2_option option) const { return v4l_uvc_device::get_pu_range(option); } - // D431 controls map - /* - - */ -// uint32_t v4l_mipi_device::get_cid(rs2_option option) const -// { -// switch(option) -// { -// case RS2_OPTION_BACKLIGHT_COMPENSATION: return V4L2_CID_BACKLIGHT_COMPENSATION; -// case RS2_OPTION_BRIGHTNESS: return V4L2_CID_BRIGHTNESS; -// case RS2_OPTION_CONTRAST: return V4L2_CID_CONTRAST; -// case RS2_OPTION_EXPOSURE: return V4L2_CID_EXPOSURE_ABSOLUTE; // Is this actually valid? I'm getting a lot of VIDIOC error 22s... -// case RS2_OPTION_GAIN: return V4L2_CID_GAIN; -// case RS2_OPTION_GAMMA: return V4L2_CID_GAMMA; -// case RS2_OPTION_HUE: return V4L2_CID_HUE; -// case RS2_OPTION_SATURATION: return V4L2_CID_SATURATION; -// case RS2_OPTION_SHARPNESS: return V4L2_CID_SHARPNESS; -// case RS2_OPTION_WHITE_BALANCE: return V4L2_CID_WHITE_BALANCE_TEMPERATURE; -// case RS2_OPTION_ENABLE_AUTO_EXPOSURE: return V4L2_CID_EXPOSURE_AUTO; // Automatic gain/exposure control -// case RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE: return V4L2_CID_AUTO_WHITE_BALANCE; -// case RS2_OPTION_POWER_LINE_FREQUENCY : return V4L2_CID_POWER_LINE_FREQUENCY; -// case RS2_OPTION_AUTO_EXPOSURE_PRIORITY: return V4L2_CID_EXPOSURE_AUTO_PRIORITY; -// default: throw linux_backend_exception(to_string() << "no v4l2 cid for mipi option " << option); -// } -// } + // D431 controls map- temporal solution to bypass backend interface with acual codes + // DS5 depth XU identifiers + const uint8_t RS_HWMONITOR = 1; + const uint8_t RS_DEPTH_EMITTER_ENABLED = 2; + const uint8_t RS_EXPOSURE = 3; + const uint8_t RS_LASER_POWER = 4; + const uint8_t RS_HARDWARE_PRESET = 6; + const uint8_t RS_ERROR_REPORTING = 7; + const uint8_t RS_EXT_TRIGGER = 8; + const uint8_t RS_ASIC_AND_PROJECTOR_TEMPERATURES = 9; + const uint8_t RS_ENABLE_AUTO_WHITE_BALANCE = 0xA; + const uint8_t RS_ENABLE_AUTO_EXPOSURE = 0xB; + const uint8_t RS_LED_PWR = 0xE; + + uint32_t v4l_mipi_device::xu_to_cid(const extension_unit& xu, uint8_t control) const + { + if (0==xu.subdevice) + { + switch(control) + { + case RS_DEPTH_EMITTER_ENABLED: return RS_CAMERA_CID_LASER_POWER; + case RS_LASER_POWER: return RS_CAMERA_CID_MANUAL_LASER_POWER; + // case RS2_OPTION_BACKLIGHT_COMPENSATION: return V4L2_CID_BACKLIGHT_COMPENSATION; + // case RS2_OPTION_BRIGHTNESS: return V4L2_CID_BRIGHTNESS; + // case RS2_OPTION_CONTRAST: return V4L2_CID_CONTRAST; + // case RS2_OPTION_EXPOSURE: return V4L2_CID_EXPOSURE_ABSOLUTE; // Is this actually valid? I'm getting a lot of VIDIOC error 22s... + // case RS2_OPTION_GAIN: return V4L2_CID_GAIN; + // case RS2_OPTION_GAMMA: return V4L2_CID_GAMMA; + // case RS2_OPTION_HUE: return V4L2_CID_HUE; + // case RS2_OPTION_SATURATION: return V4L2_CID_SATURATION; + // case RS2_OPTION_SHARPNESS: return V4L2_CID_SHARPNESS; + // case RS2_OPTION_WHITE_BALANCE: return V4L2_CID_WHITE_BALANCE_TEMPERATURE; + // case RS2_OPTION_ENABLE_AUTO_EXPOSURE: return V4L2_CID_EXPOSURE_AUTO; // Automatic gain/exposure control + // case RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE: return V4L2_CID_AUTO_WHITE_BALANCE; + // case RS2_OPTION_POWER_LINE_FREQUENCY : return V4L2_CID_POWER_LINE_FREQUENCY; + // case RS2_OPTION_AUTO_EXPOSURE_PRIORITY: return V4L2_CID_EXPOSURE_AUTO_PRIORITY; + + default: throw linux_backend_exception(to_string() << "no v4l2 mipi cid for XU depth control " << control); + } + } + else + throw linux_backend_exception(to_string() << "MIPI Controls mapping is for Depth XU only, requested for subdevice " << xu.subdevice); + + } std::shared_ptr v4l_backend::create_uvc_device(uvc_device_info info) const diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index d2695d1c89..f333693227 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -389,6 +389,8 @@ namespace librealsense bool get_xu(const extension_unit& xu, uint8_t control, uint8_t* data, int size) const override; control_range get_xu_range(const extension_unit& xu, uint8_t control, int len) const override; control_range get_pu_range(rs2_option option) const override; + protected: + uint32_t xu_to_cid(const extension_unit& xu, uint8_t control) const; // Find the mapping of XU to the underlying control }; class v4l_backend : public backend From 2e1af2553606040c3927482641b13035ff586d38 Mon Sep 17 00:00:00 2001 From: Evgeni raikhel Date: Wed, 27 Nov 2019 17:10:09 +0200 Subject: [PATCH 006/361] Switch to kernel 4.9. Enabling HW Monitor over v4l control --- src/ds5/ds5-device.cpp | 4 ++ src/linux/backend-v4l2.cpp | 93 ++++++++++++++++++++++---------------- src/option.cpp | 35 ++++++++++++++ src/option.h | 16 +++++++ 4 files changed, 108 insertions(+), 40 deletions(-) diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 9a1dd9b806..0f33ba90b3 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -789,6 +789,10 @@ namespace librealsense } else { + //#define V4L2_CTRL_CLASS_CAMERA 0x009a0000 /* Camera class controls */ + //#define DS5_CAMERA_CID_BASE (V4L2_CTRL_CLASS_CAMERA | DS5_STREAM_CONFIG_0) + //#define DS5_CAMERA_CID_HWMC (DS5_CAMERA_CID_BASE+15) + if (!mipi_sensor) _hw_monitor = std::make_shared( std::make_shared( diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 66008544a7..420399ef84 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -55,24 +55,42 @@ const size_t MAX_DEV_PARENT_DIR = 10; #include "../tm2/tm-boot.h" //D431 Dev. TODO -shall be refactored into the kernel headers. -const uint32_t RS_STREAM_CONFIG_0 = 0x4000; -const uint32_t RS_CAMERA_CID_BASE = (V4L2_CTRL_CLASS_CAMERA | RS_STREAM_CONFIG_0); -const uint32_t RS_CAMERA_CID_LASER_POWER = (RS_CAMERA_CID_BASE+1); -const uint32_t RS_CAMERA_CID_MANUAL_LASER_POWER = (RS_CAMERA_CID_BASE+2); -const uint32_t RS_CAMERA_DEPTH_CALIBRATION_TABLE_GET = (RS_CAMERA_CID_BASE+3); -const uint32_t RS_CAMERA_DEPTH_CALIBRATION_TABLE_SET = (RS_CAMERA_CID_BASE+4); -const uint32_t RS_CAMERA_COEFF_CALIBRATION_TABLE_GET = (RS_CAMERA_CID_BASE+5); -const uint32_t RS_CAMERA_COEFF_CALIBRATION_TABLE_SET = (RS_CAMERA_CID_BASE+6); -const uint32_t RS_CAMERA_CID_FW_VERSION = (RS_CAMERA_CID_BASE+7); -const uint32_t RS_CAMERA_CID_GVD = (RS_CAMERA_CID_BASE+8); -const uint32_t RS_CAMERA_CID_AE_ROI_GET = (RS_CAMERA_CID_BASE+9); -const uint32_t RS_CAMERA_CID_AE_ROI_SET = (RS_CAMERA_CID_BASE+10); -const uint32_t RS_CAMERA_CID_AE_SETPOINT_GET = (RS_CAMERA_CID_BASE+11); -const uint32_t RS_CAMERA_CID_AE_SETPOINT_SET = (RS_CAMERA_CID_BASE+12); -const uint32_t RS_CAMERA_CID_ERB = (RS_CAMERA_CID_BASE+13); -const uint32_t RS_CAMERA_CID_EWB = (RS_CAMERA_CID_BASE+14); -const uint32_t RS_CAMERA_CID_HWMC = (RS_CAMERA_CID_BASE+15); - +constexpr uint32_t RS_STREAM_CONFIG_0 = 0x4000; +constexpr uint32_t RS_CAMERA_CID_BASE = (V4L2_CTRL_CLASS_CAMERA | RS_STREAM_CONFIG_0); +constexpr uint32_t RS_CAMERA_CID_LASER_POWER = (RS_CAMERA_CID_BASE+1); +constexpr uint32_t RS_CAMERA_CID_MANUAL_LASER_POWER = (RS_CAMERA_CID_BASE+2); +constexpr uint32_t RS_CAMERA_DEPTH_CALIBRATION_TABLE_GET = (RS_CAMERA_CID_BASE+3); +constexpr uint32_t RS_CAMERA_DEPTH_CALIBRATION_TABLE_SET = (RS_CAMERA_CID_BASE+4); +constexpr uint32_t RS_CAMERA_COEFF_CALIBRATION_TABLE_GET = (RS_CAMERA_CID_BASE+5); +constexpr uint32_t RS_CAMERA_COEFF_CALIBRATION_TABLE_SET = (RS_CAMERA_CID_BASE+6); +constexpr uint32_t RS_CAMERA_CID_FW_VERSION = (RS_CAMERA_CID_BASE+7); +constexpr uint32_t RS_CAMERA_CID_GVD = (RS_CAMERA_CID_BASE+8); +constexpr uint32_t RS_CAMERA_CID_AE_ROI_GET = (RS_CAMERA_CID_BASE+9); +constexpr uint32_t RS_CAMERA_CID_AE_ROI_SET = (RS_CAMERA_CID_BASE+10); +constexpr uint32_t RS_CAMERA_CID_AE_SETPOINT_GET = (RS_CAMERA_CID_BASE+11); +constexpr uint32_t RS_CAMERA_CID_AE_SETPOINT_SET = (RS_CAMERA_CID_BASE+12); +constexpr uint32_t RS_CAMERA_CID_ERB = (RS_CAMERA_CID_BASE+13); +constexpr uint32_t RS_CAMERA_CID_EWB = (RS_CAMERA_CID_BASE+14); +constexpr uint32_t RS_CAMERA_CID_HWMC = (RS_CAMERA_CID_BASE+15); + +//const uint32_t RS_CAMERA_GENERIC_XU = (RS_CAMERA_CID_BASE+15); // RS_CAMERA_CID_HWMC duplicate?? +constexpr uint32_t RS_CAMERA_CID_LASER_POWER_MODE = (RS_CAMERA_CID_BASE+16); // RS_CAMERA_CID_LASER_POWER duplicate ??? +constexpr uint32_t RS_CAMERA_CID_MANUAL_EXPOSURE = (RS_CAMERA_CID_BASE+17); +constexpr uint32_t RS_CAMERA_CID_LASER_POWER_LEVEL = (RS_CAMERA_CID_BASE+18); // RS_CAMERA_CID_MANUAL_LASER_POWER ?? +constexpr uint32_t RS_CAMERA_CID_EXPOSURE_MODE = (RS_CAMERA_CID_BASE+19); +constexpr uint32_t RS_CAMERA_CID_WHITE_BALANCE_MODE = (RS_CAMERA_CID_BASE+20); // Similar to RS_CAMERA_CID_EWB ?? +constexpr uint32_t RS_CAMERA_CID_PRESET = (RS_CAMERA_CID_BASE+21); + +/* refe4rence for kernel 4.9 to be removed +#define UVC_CID_GENERIC_XU (V4L2_CID_PRIVATE_BASE+15) +#define UVC_CID_LASER_POWER_MODE (V4L2_CID_PRIVATE_BASE+16) +#define UVC_CID_MANUAL_EXPOSURE (V4L2_CID_PRIVATE_BASE+17) +#define UVC_CID_LASER_POWER_LEVEL (V4L2_CID_PRIVATE_BASE+18) +#define UVC_CID_EXPOSURE_MODE (V4L2_CID_PRIVATE_BASE+19) +#define UVC_CID_WHITE_BALANCE_MODE (V4L2_CID_PRIVATE_BASE+20) +#define UVC_CID_PRESET (V4L2_CID_PRIVATE_BASE+21) +UVC_CID_MANUAL_EXPOSURE +*/ #ifdef ANDROID // https://android.googlesource.com/platform/bionic/+/master/libc/include/bits/lockf.h @@ -650,11 +668,11 @@ namespace librealsense switch(ind) { + // D431 exposes video0 for Depth and video1 for RGB. IR is currently not available case 0: - case 1: mi = 0; break; - case 3: + case 1: mi = 3; break; default: @@ -1992,9 +2010,9 @@ namespace librealsense throw linux_backend_exception("xioctl(VIDIOC_S_EXT_CTRLS) failed"); } - return true; } + bool v4l_mipi_device::get_xu(const extension_unit& xu, uint8_t control, uint8_t* data, int size) const { LOG_INFO("Function not implemented - " << __FUNCTION__); @@ -2028,7 +2046,7 @@ namespace librealsense return v4l_uvc_device::get_pu_range(option); } - // D431 controls map- temporal solution to bypass backend interface with acual codes + // D431 controls map- temporal solution to bypass backend interface with actual codes // DS5 depth XU identifiers const uint8_t RS_HWMONITOR = 1; const uint8_t RS_DEPTH_EMITTER_ENABLED = 2; @@ -2042,38 +2060,33 @@ namespace librealsense const uint8_t RS_ENABLE_AUTO_EXPOSURE = 0xB; const uint8_t RS_LED_PWR = 0xE; + // D431 controls map- temporal solution to bypass backend interface with actual codes uint32_t v4l_mipi_device::xu_to_cid(const extension_unit& xu, uint8_t control) const { if (0==xu.subdevice) { switch(control) { + case RS_HWMONITOR: return RS_CAMERA_CID_HWMC; case RS_DEPTH_EMITTER_ENABLED: return RS_CAMERA_CID_LASER_POWER; + case RS_EXPOSURE: return RS_CAMERA_CID_MANUAL_EXPOSURE; case RS_LASER_POWER: return RS_CAMERA_CID_MANUAL_LASER_POWER; - // case RS2_OPTION_BACKLIGHT_COMPENSATION: return V4L2_CID_BACKLIGHT_COMPENSATION; - // case RS2_OPTION_BRIGHTNESS: return V4L2_CID_BRIGHTNESS; - // case RS2_OPTION_CONTRAST: return V4L2_CID_CONTRAST; - // case RS2_OPTION_EXPOSURE: return V4L2_CID_EXPOSURE_ABSOLUTE; // Is this actually valid? I'm getting a lot of VIDIOC error 22s... - // case RS2_OPTION_GAIN: return V4L2_CID_GAIN; - // case RS2_OPTION_GAMMA: return V4L2_CID_GAMMA; - // case RS2_OPTION_HUE: return V4L2_CID_HUE; - // case RS2_OPTION_SATURATION: return V4L2_CID_SATURATION; - // case RS2_OPTION_SHARPNESS: return V4L2_CID_SHARPNESS; - // case RS2_OPTION_WHITE_BALANCE: return V4L2_CID_WHITE_BALANCE_TEMPERATURE; - // case RS2_OPTION_ENABLE_AUTO_EXPOSURE: return V4L2_CID_EXPOSURE_AUTO; // Automatic gain/exposure control - // case RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE: return V4L2_CID_AUTO_WHITE_BALANCE; - // case RS2_OPTION_POWER_LINE_FREQUENCY : return V4L2_CID_POWER_LINE_FREQUENCY; - // case RS2_OPTION_AUTO_EXPOSURE_PRIORITY: return V4L2_CID_EXPOSURE_AUTO_PRIORITY; - - default: throw linux_backend_exception(to_string() << "no v4l2 mipi cid for XU depth control " << control); + case RS_ENABLE_AUTO_WHITE_BALANCE : return RS_CAMERA_CID_WHITE_BALANCE_MODE; + case RS_ENABLE_AUTO_EXPOSURE: return RS_CAMERA_CID_EXPOSURE_MODE; + case RS_HARDWARE_PRESET : return RS_CAMERA_CID_PRESET; + // D431 Missing functionality + //case RS_ERROR_REPORTING: TBD; + //case RS_EXT_TRIGGER: TBD; + //case RS_ASIC_AND_PROJECTOR_TEMPERATURES: TBD; + //case RS_LED_PWR: TBD; + + default: throw linux_backend_exception(to_string() << "no v4l2 mipi cid for XU depth control " << std::dec << int(control)); } } else throw linux_backend_exception(to_string() << "MIPI Controls mapping is for Depth XU only, requested for subdevice " << xu.subdevice); - } - std::shared_ptr v4l_backend::create_uvc_device(uvc_device_info info) const { bool mipi_device = 0xABCD == info.pid; // D431 development. Not for upstream diff --git a/src/option.cpp b/src/option.cpp index 92d16eb1ff..1e6380ca1e 100644 --- a/src/option.cpp +++ b/src/option.cpp @@ -140,6 +140,41 @@ std::vector librealsense::command_transfer_over_xu::send_receive(const }); } +std::vector librealsense::command_transfer_over_v4l_ctl::send_receive(const std::vector& data, int, bool require_response) +{ + return _uvc.invoke_powered([this, &data, require_response] + (platform::uvc_device& dev) + { + std::vector result; + std::lock_guard lock(dev); + + if (data.size() > HW_MONITOR_BUFFER_SIZE) + { + LOG_ERROR("XU command size is invalid"); + throw invalid_value_exception(to_string() << "Requested XU command size " << + std::dec << data.size() << " exceeds permitted limit " << HW_MONITOR_BUFFER_SIZE); + } + + std::vector transmit_buf(HW_MONITOR_BUFFER_SIZE, 0); + std::copy(data.begin(), data.end(), transmit_buf.begin()); + + if (!dev.set_xu(_xu, _ctrl, transmit_buf.data(), static_cast(transmit_buf.size()))) + throw invalid_value_exception(to_string() << "set_xu(ctrl=" << unsigned(_ctrl) << ") failed!" << " Last Error: " << strerror(errno)); + + if (require_response) + { + result.resize(HW_MONITOR_BUFFER_SIZE); + if (!dev.get_xu(_xu, _ctrl, result.data(), static_cast(result.size()))) + throw invalid_value_exception(to_string() << "get_xu(ctrl=" << unsigned(_ctrl) << ") failed!" << " Last Error: " << strerror(errno)); + + // Returned data size located in the last 4 bytes + auto data_size = *(reinterpret_cast(result.data() + HW_MONITOR_DATA_SIZE_OFFSET)) + SIZE_OF_HW_MONITOR_HEADER; + result.resize(data_size); + } + return result; + }); +} + librealsense::polling_errors_disable::~polling_errors_disable() { if (auto handler = _polling_error_handler.lock()) diff --git a/src/option.h b/src/option.h index 9f37d56d46..e8519507fb 100644 --- a/src/option.h +++ b/src/option.h @@ -502,6 +502,22 @@ namespace librealsense uint8_t _ctrl; }; + class command_transfer_over_v4l_ctl : public platform::command_transfer + { + public: + std::vector send_receive(const std::vector& data, int, bool require_response) override; + + command_transfer_over_v4l_ctl(uvc_sensor& uvc, + platform::extension_unit xu, uint8_t ctrl) + : _uvc(uvc), _xu(std::move(xu)), _ctrl(ctrl) + {} + + private: + uvc_sensor& _uvc; + platform::extension_unit _xu; + uint8_t _ctrl; + }; + class polling_error_handler; class polling_errors_disable : public option From 1cccfe27ce5ea1be979bc1d32a9c17748873ecec Mon Sep 17 00:00:00 2001 From: Evgeni raikhel Date: Thu, 28 Nov 2019 10:54:48 +0200 Subject: [PATCH 007/361] WIP --- src/ds5/ds5-color.cpp | 13 ++++++++++--- src/option.h | 5 ++--- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/ds5/ds5-color.cpp b/src/ds5/ds5-color.cpp index c7d49c3da7..2a05712f54 100644 --- a/src/ds5/ds5-color.cpp +++ b/src/ds5/ds5-color.cpp @@ -255,16 +255,23 @@ namespace librealsense color_ep.register_metadata(RS2_FRAME_METADATA_LOW_LIGHT_COMPENSATION, make_attribute_parser(&md_rgb_control::low_light_comp, md_rgb_control_attributes::low_light_comp_attribute, md_prop_offset)); // Starting with firmware 5.10.9, auto-exposure ROI is available for color sensor - + if (_fw_version >= firmware_version("5.10.9.0")) + { + roi_sensor_interface* roi_sensor; + if ((roi_sensor = dynamic_cast(color_ep.get()))) + roi_sensor->set_roi_method(std::make_shared(*_hw_monitor, ds::fw_cmd::SETRGBAEROI)); + } color_ep.register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_UYVY, map_supported_color_formats(RS2_FORMAT_UYVY), RS2_STREAM_COLOR)); + color_ep.register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_YUYV, map_supported_color_formats(RS2_FORMAT_YUYV), RS2_STREAM_COLOR)); } //D431 else { - // Work-around for improper enumeration given to RGB output as UYUV instead of YUYV - color_ep.register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_UYVY, map_supported_color_formats(RS2_FORMAT_UYVY), RS2_STREAM_COLOR)); + // Work-around for discrepancy between the RGB YUYV descriptor and the parser . Use UYUV parser instead + color_ep->register_processing_block(processing_block_factory::create_pbf_vector(RS2_FORMAT_UYVY, map_supported_color_formats(RS2_FORMAT_UYVY), RS2_STREAM_COLOR)); } + color_ep.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_RAW16, RS2_STREAM_COLOR)); if (_pid == ds::RS465_PID) diff --git a/src/option.h b/src/option.h index e8519507fb..8fb20dad06 100644 --- a/src/option.h +++ b/src/option.h @@ -507,9 +507,8 @@ namespace librealsense public: std::vector send_receive(const std::vector& data, int, bool require_response) override; - command_transfer_over_v4l_ctl(uvc_sensor& uvc, - platform::extension_unit xu, uint8_t ctrl) - : _uvc(uvc), _xu(std::move(xu)), _ctrl(ctrl) + command_transfer_over_v4l_ctl(uvc_sensor& uvc, uint8_t ctrl) + : _uvc(uvc), _ctrl(ctrl) {} private: From b786cf3543118576e7f284b2b7046f0e52f2f456 Mon Sep 17 00:00:00 2001 From: Evgeni raikhel Date: Mon, 2 Dec 2019 18:53:33 +0200 Subject: [PATCH 008/361] HWMon pass through acomplished --- src/ds5/ds5-device.cpp | 10 ++++------ src/linux/backend-v4l2.cpp | 3 ++- src/option.cpp | 20 +++++++++++++------- 3 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 0f33ba90b3..091c303b53 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -653,8 +653,6 @@ namespace librealsense float ds5_device::get_stereo_baseline_mm() const { using namespace ds; - if ("ABCD" == get_info(RS2_CAMERA_INFO_PRODUCT_ID)) // RS431 Development. to be removed. TODO - return 55; auto table = check_calib(*_coefficients_table_raw); return fabs(table->baseline); @@ -901,10 +899,10 @@ namespace librealsense if ((_fw_version >= firmware_version("5.6.3.0")) || (_fw_version) == firmware_version("1.1.1.1")) // RS431 Dev { - if (!mipi_sensor) - { - _is_locked = _hw_monitor->is_camera_locked(GVD, is_camera_locked_offset); - } + if (!mipi_sensor) + { + _is_locked = _hw_monitor->is_camera_locked(GVD, is_camera_locked_offset); + } } if (_fw_version >= firmware_version("5.5.8.0")) diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 420399ef84..a39bc1de89 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -2001,7 +2001,8 @@ namespace librealsense bool v4l_mipi_device::set_xu(const extension_unit& xu, uint8_t control, const uint8_t* data, int size) { - v4l2_ext_control xctrl{xu_to_cid(xu,control), 0, 0, value}; + v4l2_ext_control xctrl{xu_to_cid(xu,control), uint32_t(size), 0, 0}; + xctrl.p_u8 = const_cast(data); // TODO aggregate initialization with union v4l2_ext_controls ctrls_block { V4L2_CTRL_CLASS_CAMERA, 1, 0, {0 ,0}, &xctrl}; if (xioctl(_fd, VIDIOC_S_EXT_CTRLS, &ctrls_block) < 0) { diff --git a/src/option.cpp b/src/option.cpp index 1e6380ca1e..575af1e4ce 100644 --- a/src/option.cpp +++ b/src/option.cpp @@ -145,7 +145,7 @@ std::vector librealsense::command_transfer_over_v4l_ctl::send_receive(c return _uvc.invoke_powered([this, &data, require_response] (platform::uvc_device& dev) { - std::vector result; + std::vector result{}; std::lock_guard lock(dev); if (data.size() > HW_MONITOR_BUFFER_SIZE) @@ -155,7 +155,7 @@ std::vector librealsense::command_transfer_over_v4l_ctl::send_receive(c std::dec << data.size() << " exceeds permitted limit " << HW_MONITOR_BUFFER_SIZE); } - std::vector transmit_buf(HW_MONITOR_BUFFER_SIZE, 0); + std::vector transmit_buf(HW_MONITOR_BUFFER_SIZE+SIZE_OF_HW_MONITOR_HEADER, 0); // TBD- Elaborate 1028 std::copy(data.begin(), data.end(), transmit_buf.begin()); if (!dev.set_xu(_xu, _ctrl, transmit_buf.data(), static_cast(transmit_buf.size()))) @@ -163,13 +163,19 @@ std::vector librealsense::command_transfer_over_v4l_ctl::send_receive(c if (require_response) { - result.resize(HW_MONITOR_BUFFER_SIZE); - if (!dev.get_xu(_xu, _ctrl, result.data(), static_cast(result.size()))) - throw invalid_value_exception(to_string() << "get_xu(ctrl=" << unsigned(_ctrl) << ") failed!" << " Last Error: " << strerror(errno)); + //result.resize(HW_MONITOR_BUFFER_SIZE+SIZE_OF_HW_MONITOR_HEADER); +// if (!dev.get_xu(_xu, _ctrl, result.data(), static_cast(result.size()))) +// throw invalid_value_exception(to_string() << "get_xu(ctrl=" << unsigned(_ctrl) << ") failed!" << " Last Error: " << strerror(errno)); // Returned data size located in the last 4 bytes - auto data_size = *(reinterpret_cast(result.data() + HW_MONITOR_DATA_SIZE_OFFSET)) + SIZE_OF_HW_MONITOR_HEADER; - result.resize(data_size); + // Hard-coded override - return full buffer except for the hwm 24 bytes + //auto data_size = *(reinterpret_cast(transmit_buf.data() + HW_MONITOR_DATA_SIZE_OFFSET)) + SIZE_OF_HW_MONITOR_HEADER; + //result.resize(data_size); + result.insert(result.begin(),transmit_buf.begin()+24,transmit_buf.end()); + // Override bytes 20-24 with bytes 0-3 + //result[0] = transmit_buf[0]; + //for (int i=1; i<4; i++) + //result[i]= 0; } return result; }); From 8f60b2106c3f42adf3c5a6abce1a839912a805f1 Mon Sep 17 00:00:00 2001 From: Evgeni raikhel Date: Tue, 3 Dec 2019 17:43:14 +0200 Subject: [PATCH 009/361] Wrapping up hwmon bypass --- src/ds5/ds5-device.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 091c303b53..1a53eb612d 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -787,10 +787,6 @@ namespace librealsense } else { - //#define V4L2_CTRL_CLASS_CAMERA 0x009a0000 /* Camera class controls */ - //#define DS5_CAMERA_CID_BASE (V4L2_CTRL_CLASS_CAMERA | DS5_STREAM_CONFIG_0) - //#define DS5_CAMERA_CID_HWMC (DS5_CAMERA_CID_BASE+15) - if (!mipi_sensor) _hw_monitor = std::make_shared( std::make_shared( From d566687b3e42f37a9f8a7824e844f5e7c70d62f6 Mon Sep 17 00:00:00 2001 From: Evgeni raikhel Date: Wed, 4 Dec 2019 11:20:16 +0200 Subject: [PATCH 010/361] HWMon fix with f4xx_49_4. Disabling is_camera_in_advanced_mode() - not operational with D431 --- src/ds5/ds5-device.cpp | 74 ++++++++++++++++++++++-------------------- src/option.cpp | 13 +++----- 2 files changed, 43 insertions(+), 44 deletions(-) diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 1a53eb612d..1fb2b8699d 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -851,47 +851,49 @@ namespace librealsense auto& depth_sensor = get_depth_sensor(); auto& raw_depth_sensor = get_raw_depth_sensor(); - auto advanced_mode = is_camera_in_advanced_mode(); + //D431 Development + auto advanced_mode = mipi_sensor ? true : is_camera_in_advanced_mode(); - auto _usb_mode = usb3_type; - usb_type_str = usb_spec_names.at(_usb_mode); - usb_modality = (_fw_version >= firmware_version("5.9.8.0")); - if (usb_modality) - { - _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; - } + using namespace platform; + auto _usb_mode = usb3_type; + usb_type_str = usb_spec_names.at(_usb_mode); + usb_modality = (_fw_version >= firmware_version("5.9.8.0")); + if (usb_modality) + { + _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; + } - if (_fw_version >= firmware_version("5.12.1.1")) - { - depth_sensor.register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_Z16H, RS2_STREAM_DEPTH)); - } + if (_fw_version >= firmware_version("5.12.1.1")) + { + 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_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_Y12I }, - { {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1}, {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2} }, - []() {return std::make_shared(); } - ); - pid_hex_str = hexify(_pid); + depth_sensor.register_processing_block( + { RS2_FORMAT_Y12I }, + { {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1}, {RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2} }, + []() {return std::make_shared(); } + ); + pid_hex_str = hexify(_pid); - if ((_pid == RS416_PID || _pid == RS416_RGB_PID) && _fw_version >= firmware_version("5.12.0.1")) - { - depth_sensor.register_option(RS2_OPTION_HARDWARE_PRESET, - std::make_shared>(raw_depth_sensor, depth_xu, DS5_HARDWARE_PRESET, - "Hardware pipe configuration")); - depth_sensor.register_option(RS2_OPTION_LED_POWER, - std::make_shared>(raw_depth_sensor, depth_xu, DS5_LED_PWR, - "Set the power level of the LED, with 0 meaning LED off")); - } + if ((_pid == RS416_PID || _pid == RS416_RGB_PID) && _fw_version >= firmware_version("5.12.0.1")) + { + depth_sensor.register_option(RS2_OPTION_HARDWARE_PRESET, + std::make_shared>(raw_depth_sensor, depth_xu, DS5_HARDWARE_PRESET, + "Hardware pipe configuration")); + depth_sensor.register_option(RS2_OPTION_LED_POWER, + std::make_shared>(raw_depth_sensor, depth_xu, DS5_LED_PWR, + "Set the power level of the LED, with 0 meaning LED off")); + } if ((_fw_version >= firmware_version("5.6.3.0")) || (_fw_version) == firmware_version("1.1.1.1")) // RS431 Dev { diff --git a/src/option.cpp b/src/option.cpp index 575af1e4ce..49a3a960ac 100644 --- a/src/option.cpp +++ b/src/option.cpp @@ -163,19 +163,16 @@ std::vector librealsense::command_transfer_over_v4l_ctl::send_receive(c if (require_response) { + uint32_t data_size = *(reinterpret_cast(transmit_buf.data() + HW_MONITOR_BUFFER_SIZE)); + std::cout << " data_size =" << data_size << std::endl; + result.insert(result.begin(),transmit_buf.begin()+24,transmit_buf.begin()+24+data_size); //result.resize(HW_MONITOR_BUFFER_SIZE+SIZE_OF_HW_MONITOR_HEADER); // if (!dev.get_xu(_xu, _ctrl, result.data(), static_cast(result.size()))) // throw invalid_value_exception(to_string() << "get_xu(ctrl=" << unsigned(_ctrl) << ") failed!" << " Last Error: " << strerror(errno)); // Returned data size located in the last 4 bytes - // Hard-coded override - return full buffer except for the hwm 24 bytes - //auto data_size = *(reinterpret_cast(transmit_buf.data() + HW_MONITOR_DATA_SIZE_OFFSET)) + SIZE_OF_HW_MONITOR_HEADER; - //result.resize(data_size); - result.insert(result.begin(),transmit_buf.begin()+24,transmit_buf.end()); - // Override bytes 20-24 with bytes 0-3 - //result[0] = transmit_buf[0]; - //for (int i=1; i<4; i++) - //result[i]= 0; + //result.insert(result.begin(),transmit_buf.begin()+24,transmit_buf.end()); + } return result; }); From 2db0435818fe3ac0df6f349447cdb26de4e20447 Mon Sep 17 00:00:00 2001 From: Evgeni raikhel Date: Wed, 4 Dec 2019 16:39:19 +0200 Subject: [PATCH 011/361] Remove unsupported controls : gain hw_sync, emitter on_off Adjust default RGB resolution to FullHD in the viewer Fix get_xu_range parameters list Adjust exposure and ae to the v4l type formats instead of xu --- common/model-views.cpp | 9 +++++++++ src/ds5/ds5-device.cpp | 9 ++++++++- src/ds5/ds5-factory.cpp | 17 ++++------------- src/ds5/ds5-timestamp.cpp | 2 +- src/linux/backend-v4l2.cpp | 8 ++++---- src/option.cpp | 2 +- 6 files changed, 27 insertions(+), 20 deletions(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index 94976c100a..580b0fc3c8 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -1409,6 +1409,15 @@ namespace rs2 int selection_index{}; + // D431 dev + if ((std::string(s->get_info(RS2_CAMERA_INFO_NAME)) == "RGB Sensor") && + (std::string(s->get_info(RS2_CAMERA_INFO_PRODUCT_ID)) == "ABCD")) + { + resolution_constrain = std::make_pair(1920, 1080); + } + + std::cout << "Sensor = " << std::string(s->get_info(RS2_CAMERA_INFO_NAME)) + << " PID = " << std::string(s->get_info(RS2_CAMERA_INFO_PRODUCT_ID)) << std::endl; if (!show_single_fps_list) { for (auto fps_array : fps_values_per_stream) diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 1fb2b8699d..3853ac7256 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -939,6 +939,9 @@ namespace librealsense } + // minimal firmware version in which hdr feature is supported + firmware_version hdr_firmware_version("5.12.8.100"); + std::shared_ptr