diff --git a/common/fw-update-helper.cpp b/common/fw-update-helper.cpp index 43707dfc60..8ebe80f6b3 100644 --- a/common/fw-update-helper.cpp +++ b/common/fw-update-helper.cpp @@ -194,10 +194,61 @@ namespace rs2 return false; } + void firmware_update_manager_mipi::process_flow(std::function cleanup, invoker invoke) + { + if (!_is_signed) + { + LOG_INFO("Only Signed Firmware can be burnt on MIPI device"); + return; + } + auto dev_updatable = _dev.as(); + if(!(dev_updatable && dev_updatable.check_firmware_compatibility(_fw))) + { + fail("Firmware Update failed - fw version must be newer than version 5.13.1.1"); + return; + } + + // Enter DFU mode + auto device_debug = _dev.as(); + uint32_t dfu_opcode = 0x1e; + device_debug.build_command(dfu_opcode, 1); + + _progress = 30; + // Grant permissions for writing + // skipped for now - must be done in sudo + //chmod("/dev/d4xx-dfu504", __S_IREAD|__S_IWRITE); + + // Write signed firmware to appropriate file descritptor + std::ofstream fw_path_in_device("/dev/d4xx-dfu504", std::ios::binary); + if (fw_path_in_device) + { + fw_path_in_device.write(reinterpret_cast(_fw.data()), _fw.size()); + } + else + { + fail("Firmware Update failed - wrong path or permissions missing"); + return; + } + LOG_INFO("Firmware Update for MIPI device done."); + fw_path_in_device.close(); + + _progress = 100; + _done = true; + // need to find a way to update the fw version field in the viewer + } + void firmware_update_manager::process_flow( std::function cleanup, invoker invoke) { + auto str = _dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); + if (!strcmp(_dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID), "ABCD")) // if device is D457 + { + firmware_update_manager_mipi fw_update_mgr_mipi(_not_model, _model, _dev, _ctx, _fw, _is_signed); + fw_update_mgr_mipi.process_flow(cleanup, invoke); + return; + } + std::string serial = ""; if (_dev.supports(RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID)) serial = _dev.get_info(RS2_CAMERA_INFO_FIRMWARE_UPDATE_ID); diff --git a/common/fw-update-helper.h b/common/fw-update-helper.h index a6ee4a74b8..73f23846f6 100644 --- a/common/fw-update-helper.h +++ b/common/fw-update-helper.h @@ -4,6 +4,7 @@ #pragma once #include "notifications.h" +#include "../src/fw-update/fw-update-device-interface.h" namespace rs2 @@ -28,7 +29,7 @@ namespace rs2 const device_model& get_device_model() const { return _model; } std::shared_ptr get_protected_notification_model() { return _not_model.lock(); }; - private: + protected: void process_flow(std::function cleanup, invoker invoke) override; bool check_for( @@ -43,6 +44,23 @@ namespace rs2 device_model& _model; }; + class firmware_update_manager_mipi : public process_manager + { + public: + firmware_update_manager_mipi(std::weak_ptr not_model, device_model& model, device dev, context ctx, std::vector fw, bool is_signed) + : process_manager("Firmware Update Mipi"), _not_model(not_model), _model(model), + _fw(fw), _is_signed(is_signed), _dev(dev), _ctx(ctx) {} + void process_flow(std::function cleanup, invoker invoke); + + private: + std::weak_ptr _not_model; + device _dev; + context _ctx; + std::vector _fw; + bool _is_signed; + device_model& _model; + }; + struct fw_update_notification_model : public process_notification_model { fw_update_notification_model(std::string name, @@ -53,4 +71,4 @@ namespace rs2 void draw_expanded(ux_window& win, std::string& error_message) override; int calc_height() override; }; -} \ No newline at end of file +} diff --git a/src/ds5/ds5-color.cpp b/src/ds5/ds5-color.cpp index 9cbd35e30a..c8431ce1ab 100644 --- a/src/ds5/ds5-color.cpp +++ b/src/ds5/ds5-color.cpp @@ -240,8 +240,8 @@ namespace librealsense } if (_pid != ds::RS457_PID) { - 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)); } else { diff --git a/src/ds5/ds5-private.h b/src/ds5/ds5-private.h index 29262e2812..84a26fd0df 100644 --- a/src/ds5/ds5-private.h +++ b/src/ds5/ds5-private.h @@ -735,7 +735,8 @@ namespace librealsense {RS465_PID, "5.12.7.100" }, {RS416_RGB_PID, "5.8.15.0" }, {RS405_PID, "5.12.11.8" }, - {RS455_PID, "5.12.7.100" } + {RS455_PID, "5.12.7.100" }, + {RS457_PID, "5.13.1.1" } }; diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 33860af97c..9f47483327 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -74,7 +74,7 @@ constexpr uint32_t RS_CAMERA_CID_AE_SETPOINT_GET = (RS_CAMERA_CID_BAS 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); +constexpr uint32_t RS_CAMERA_CID_HWMC_LEGACY = (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 ??? @@ -84,6 +84,7 @@ constexpr uint32_t RS_CAMERA_CID_EXPOSURE_MODE = (RS_CAMERA_CID_BAS 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); +constexpr uint32_t RS_CAMERA_CID_HWMC = (RS_CAMERA_CID_BASE+32); /* refe4rence for kernel 4.9 to be removed #define UVC_CID_GENERIC_XU (V4L2_CID_PRIVATE_BASE+15) @@ -1181,53 +1182,82 @@ namespace librealsense } else { - if (!is_metadata_streamed()) // when metadata is not enabled at all, streaming only video + if (!_info.has_metadata_node) { - auto timestamp = (double)buf.timestamp.tv_sec * 1000.f + (double)buf.timestamp.tv_usec / 1000.f; - timestamp = monotonic_to_realtime(timestamp); - - LOG_DEBUG_V4L("no metadata streamed"); - if (buf_mgr.verify_vd_md_sync()) + if(has_metadata()) { - buffer->attach_buffer(buf); - buf_mgr.handle_buffer(e_video_buf, -1); // transfer new buffer request to the frame callback + auto timestamp = (double)buf.timestamp.tv_sec*1000.f + (double)buf.timestamp.tv_usec/1000.f; + timestamp = monotonic_to_realtime(timestamp); + + // Read metadata. Metadata node performs a blocking call to ensure video and metadata sync + acquire_metadata(buf_mgr,fds,compressed_format); + md_extracted = true; + if (wa_applied) + { + auto fn = *(uint32_t*)((char*)(buf_mgr.metadata_start())+28); + LOG_DEBUG_V4L("Extracting md buff, fn = " << fn); + } auto frame_sz = buf_mgr.md_node_present() ? buf.bytesused : - std::min(buf.bytesused - buf_mgr.metadata_size(), - buffer->get_length_frame_only()); + std::min(buf.bytesused - buf_mgr.metadata_size(), buffer->get_length_frame_only()); + frame_object fo{ frame_sz, buf_mgr.metadata_size(), + buffer->get_frame_start(), buf_mgr.metadata_start(), timestamp }; - uint8_t md_size = buf_mgr.metadata_size(); - void* md_start = buf_mgr.metadata_start(); + buffer->attach_buffer(buf); + buf_mgr.handle_buffer(e_video_buf,-1); // transfer new buffer request to the frame callback - // D457 development - hid over uvc - md size for IMU is 64 - metadata_hid_raw meta_data{}; - if (md_size == 0 && buffer->get_length_frame_only() <= 64) + if (buf_mgr.verify_vd_md_sync()) + { + //Invoke user callback and enqueue next frame + _callback(_profile, fo, [buf_mgr]() mutable { + buf_mgr.request_next_frame(); + }); + } + else { - // Populate HID IMU data - Header - meta_data.header.report_type = md_hid_report_type::hid_report_imu; - meta_data.header.length = hid_header_size + metadata_imu_report_size; - meta_data.header.timestamp = *(reinterpret_cast(buffer->get_frame_start() + offsetof(hid_mipi_data, hwTs))); - // Payload: - meta_data.report_type.imu_report.header.md_type_id = md_type::META_DATA_HID_IMU_REPORT_ID; - meta_data.report_type.imu_report.header.md_size = metadata_imu_report_size; - - md_size = sizeof(metadata_hid_raw); - md_start = &meta_data; + LOG_WARNING("Video frame dropped, video and metadata buffers inconsistency"); } + } + else // when metadata is not enabled at all, streaming only video + { + auto timestamp = (double)buf.timestamp.tv_sec * 1000.f + (double)buf.timestamp.tv_usec / 1000.f; + timestamp = monotonic_to_realtime(timestamp); - frame_object fo{ frame_sz, md_size, - buffer->get_frame_start(), md_start, timestamp }; + LOG_DEBUG_V4L("no metadata streamed"); + if (buf_mgr.verify_vd_md_sync()) + { + buffer->attach_buffer(buf); + buf_mgr.handle_buffer(e_video_buf, -1); // transfer new buffer request to the frame callback - //Invoke user callback and enqueue next frame - _callback(_profile, fo, [buf_mgr]() mutable { - buf_mgr.request_next_frame(); - }); - } - else - { - LOG_WARNING("Video frame dropped, video and metadata buffers inconsistency"); + auto frame_sz = buf_mgr.md_node_present() ? buf.bytesused : + std::min(buf.bytesused - buf_mgr.metadata_size(), + buffer->get_length_frame_only()); + + uint8_t md_size = buf_mgr.metadata_size(); + void* md_start = buf_mgr.metadata_start(); + + // D457 development - hid over uvc - md size for IMU is 64 + metadata_hid_raw meta_data{}; + if (md_size == 0 && buffer->get_length_frame_only() <= 64) + { + // Populate HID IMU data - Header + populate_imu_data(meta_data, buffer->get_frame_start(), md_size, &md_start); + } + + frame_object fo{ frame_sz, md_size, + buffer->get_frame_start(), md_start, timestamp }; + + //Invoke user callback and enqueue next frame + _callback(_profile, fo, [buf_mgr]() mutable { + buf_mgr.request_next_frame(); + }); + } + else + { + LOG_WARNING("Video frame dropped, video and metadata buffers inconsistency"); + } } } else @@ -1250,75 +1280,106 @@ namespace librealsense LOG_DEBUG("FD_ISSET: no data on video node sink"); } - // uploading to user's callback - std::shared_ptr video_v4l2_buffer; - std::shared_ptr md_v4l2_buffer; + // pulling synchronized video and metadata and uploading them to user's callback + upload_video_and_metadata_from_syncer(buf_mgr); + } + } + else // (val==0) + { + LOG_WARNING("Frames didn't arrived within 5 seconds"); + librealsense::notification n = {RS2_NOTIFICATION_CATEGORY_FRAMES_TIMEOUT, 0, RS2_LOG_SEVERITY_WARN, "Frames didn't arrived within 5 seconds"}; - if (_is_started && is_metadata_streamed()) - { - int video_fd = -1, md_fd = -1; - if (_video_md_syncer.pull_video_with_metadata(video_v4l2_buffer, md_v4l2_buffer, video_fd, md_fd)) - { - // Preparing video buffer - auto video_buffer = get_video_buffer(video_v4l2_buffer->index); - video_buffer->attach_buffer(*video_v4l2_buffer); + _error_handler(n); + } + } + } - // happens when the video did not arrive on - // the current polling iteration (was taken from the syncer's video queue) - if (buf_mgr.get_buffers()[e_video_buf]._file_desc == -1) - { - buf_mgr.handle_buffer(e_video_buf, video_fd, *video_v4l2_buffer, video_buffer); - } - buf_mgr.handle_buffer(e_video_buf, -1); // transfer new buffer request to the frame callback + void v4l_uvc_device::populate_imu_data(metadata_hid_raw& meta_data, uint8_t* frame_start, uint8_t& md_size, void** md_start) const + { + meta_data.header.report_type = md_hid_report_type::hid_report_imu; + meta_data.header.length = hid_header_size + metadata_imu_report_size; + meta_data.header.timestamp = *(reinterpret_cast(frame_start + offsetof(hid_mipi_data, hwTs))); + // Payload: + meta_data.report_type.imu_report.header.md_type_id = md_type::META_DATA_HID_IMU_REPORT_ID; + meta_data.report_type.imu_report.header.md_size = metadata_imu_report_size; - // Preparing metadata buffer - static const size_t uvc_md_start_offset = sizeof(uvc_meta_buffer::ns) + sizeof(uvc_meta_buffer::sof); - auto metadata_buffer = get_md_buffer(md_v4l2_buffer->index); - buf_mgr.set_md_attributes(md_v4l2_buffer->bytesused, - metadata_buffer->get_frame_start()); - metadata_buffer->attach_buffer(*md_v4l2_buffer); + md_size = sizeof(metadata_hid_raw); + *md_start = &meta_data; + } - if (buf_mgr.get_buffers()[e_metadata_buf]._file_desc == -1) - { - buf_mgr.handle_buffer(e_metadata_buf, md_fd, *md_v4l2_buffer, metadata_buffer); - } - buf_mgr.handle_buffer(e_metadata_buf, -1); // transfer new buffer request to the frame callback - auto frame_sz = buf_mgr.md_node_present() ? video_v4l2_buffer->bytesused : - std::min(video_v4l2_buffer->bytesused - buf_mgr.metadata_size(), - video_buffer->get_length_frame_only()); + void v4l_uvc_device::upload_video_and_metadata_from_syncer(buffers_mgr& buf_mgr) + { + // uploading to user's callback + std::shared_ptr video_v4l2_buffer; + std::shared_ptr md_v4l2_buffer; - auto timestamp = (double)video_v4l2_buffer->timestamp.tv_sec * 1000.f + (double)video_v4l2_buffer->timestamp.tv_usec / 1000.f; - timestamp = monotonic_to_realtime(timestamp); + if (_is_started && is_metadata_streamed()) + { + int video_fd = -1, md_fd = -1; + if (_video_md_syncer.pull_video_with_metadata(video_v4l2_buffer, md_v4l2_buffer, video_fd, md_fd)) + { + // Preparing video buffer + auto video_buffer = get_video_buffer(video_v4l2_buffer->index); + video_buffer->attach_buffer(*video_v4l2_buffer); - // D457 work - to work with "normal camera", use frame_sz as the first input to the following frame_object: - //frame_object fo{ buf.bytesused - MAX_META_DATA_SIZE, buf_mgr.metadata_size(), - frame_object fo{ frame_sz, buf_mgr.metadata_size(), - video_buffer->get_frame_start(), buf_mgr.metadata_start(), timestamp }; + // happens when the video did not arrive on + // the current polling iteration (was taken from the syncer's video queue) + if (buf_mgr.get_buffers()[e_video_buf]._file_desc == -1) + { + buf_mgr.handle_buffer(e_video_buf, video_fd, *video_v4l2_buffer, video_buffer); + } + buf_mgr.handle_buffer(e_video_buf, -1); // transfer new buffer request to the frame callback - //Invoke user callback and enqueue next frame - _callback(_profile, fo, [buf_mgr]() mutable { - buf_mgr.request_next_frame(); - }); - } - else - { - LOG_DEBUG("video_md_syncer - synchronized video and md could not be pulled"); - } - } + // Preparing metadata buffer + auto metadata_buffer = get_md_buffer(md_v4l2_buffer->index); + set_metadata_attributes(buf_mgr, md_v4l2_buffer->bytesused, metadata_buffer->get_frame_start()); + metadata_buffer->attach_buffer(*md_v4l2_buffer); + + if (buf_mgr.get_buffers()[e_metadata_buf]._file_desc == -1) + { + buf_mgr.handle_buffer(e_metadata_buf, md_fd, *md_v4l2_buffer, metadata_buffer); } + buf_mgr.handle_buffer(e_metadata_buf, -1); // transfer new buffer request to the frame callback + + auto frame_sz = buf_mgr.md_node_present() ? video_v4l2_buffer->bytesused : + std::min(video_v4l2_buffer->bytesused - buf_mgr.metadata_size(), + video_buffer->get_length_frame_only()); + + auto timestamp = (double)video_v4l2_buffer->timestamp.tv_sec * 1000.f + (double)video_v4l2_buffer->timestamp.tv_usec / 1000.f; + timestamp = monotonic_to_realtime(timestamp); + + // D457 work - to work with "normal camera", use frame_sz as the first input to the following frame_object: + //frame_object fo{ buf.bytesused - MAX_META_DATA_SIZE, buf_mgr.metadata_size(), + frame_object fo{ frame_sz, buf_mgr.metadata_size(), + video_buffer->get_frame_start(), buf_mgr.metadata_start(), timestamp }; + + //Invoke user callback and enqueue next frame + _callback(_profile, fo, [buf_mgr]() mutable { + buf_mgr.request_next_frame(); + }); } - else // (val==0) + else { - LOG_WARNING("Frames didn't arrived within 5 seconds"); - librealsense::notification n = {RS2_NOTIFICATION_CATEGORY_FRAMES_TIMEOUT, 0, RS2_LOG_SEVERITY_WARN, "Frames didn't arrived within 5 seconds"}; - - _error_handler(n); + LOG_DEBUG("video_md_syncer - synchronized video and md could not be pulled"); } } } - void v4l_uvc_device::acquire_metadata(buffers_mgr & buf_mgr,fd_set &, bool compressed_format) + void v4l_uvc_device::set_metadata_attributes(buffers_mgr& buf_mgr, __u32 bytesused, uint8_t* md_start) + { + size_t uvc_md_start_offset = sizeof(uvc_meta_buffer::ns) + sizeof(uvc_meta_buffer::sof); + buf_mgr.set_md_attributes(bytesused - uvc_md_start_offset, + md_start + uvc_md_start_offset); + } + + void v4l_mipi_device::set_metadata_attributes(buffers_mgr& buf_mgr, __u32 bytesused, uint8_t* md_start) + { + buf_mgr.set_md_attributes(bytesused, md_start); + } + + + void v4l_uvc_device::acquire_metadata(buffers_mgr& buf_mgr,fd_set &, bool compressed_format) { if (has_metadata()) buf_mgr.set_md_from_video_node(compressed_format); @@ -2140,7 +2201,7 @@ namespace librealsense xctrl.value = xctrl.value ? V4L2_EXPOSURE_APERTURE_PRIORITY : V4L2_EXPOSURE_MANUAL; // Extract the control group from the underlying control query - v4l2_ext_controls ctrls_block { xctrl.id&0xffff0000, 1, 0, {0, 0}, &xctrl }; + v4l2_ext_controls ctrls_block { xctrl.id&0xffff0000, 1, 0, 0, 0, &xctrl }; int retVal = xioctl(_fd, VIDIOC_S_EXT_CTRLS, &ctrls_block); if (retVal < 0) @@ -2158,7 +2219,7 @@ namespace librealsense v4l2_ext_control xctrl{xu_to_cid(xu,control), uint32_t(size), 0, 0}; xctrl.p_u8 = data; - struct v4l2_ext_controls ext {xctrl.id & 0xffff0000, 1, 0, {0, 0}, &xctrl}; + v4l2_ext_controls ext {xctrl.id & 0xffff0000, 1, 0, 0, 0, &xctrl}; // the ioctl fails once when performing send and receive right after it // it succeeds on the second time @@ -2172,14 +2233,13 @@ namespace librealsense continue; } - // TODO check if parsing for non-integer values D457 - //memcpy(data,(void*)(&ctrl.value),size); - if (control == RS_ENABLE_AUTO_EXPOSURE) xctrl.value = (V4L2_EXPOSURE_MANUAL == xctrl.value) ? 0 : 1; - // used to parse values that have size > 1 - memcpy(data,(void*)(&xctrl.value), size); + // used to parse the data when only a value is returned (e.g. laser power), + // and not a pointer to a buffer of data (e.g. gvd) + if (size < sizeof(__s64)) + memcpy(data,(void*)(&xctrl.value), size); return true; } @@ -2354,14 +2414,18 @@ namespace librealsense if (_video_queue.size() > 2) { // Enqueue of video buffer before throwing its content away - enqueue_buffer_before_throwing_it(_video_queue.front()); - _video_queue.pop(); + enqueue_front_buffer_before_throwing_it(_video_queue); } } void v4l2_video_md_syncer::push_metadata(const sync_buffer& md_buffer) { std::lock_guard lock(_syncer_mutex); + + // override front buffer if it has the same sequence that the new buffer - happens with metadata sequence 0 + if (_md_queue.size() > 0 && _md_queue.front()._v4l2_buf->sequence == md_buffer._v4l2_buf->sequence) + enqueue_front_buffer_before_throwing_it(_md_queue); + _md_queue.push(md_buffer); LOG_DEBUG_V4L("video_md_syncer - md pushed with sequence " << md_buffer._v4l2_buf->sequence); @@ -2369,8 +2433,7 @@ namespace librealsense if (_md_queue.size() > 2) { // Enqueue of md buffer before throwing its content away - enqueue_buffer_before_throwing_it(_md_queue.front()); - _md_queue.pop(); + enqueue_front_buffer_before_throwing_it(_md_queue); } } @@ -2464,6 +2527,18 @@ namespace librealsense } } + void v4l2_video_md_syncer::enqueue_front_buffer_before_throwing_it(std::queue& sync_queue) + { + // Enqueue of buffer before throwing its content away + LOG_DEBUG_V4L("video_md_syncer - Enqueue buf " << std::dec << sync_queue.front()._buffer_index << " for fd " << sync_queue.front()._fd << " before dropping it"); + if (xioctl(sync_queue.front()._fd, VIDIOC_QBUF, sync_queue.front()._v4l2_buf.get()) < 0) + { + LOG_ERROR("xioctl(VIDIOC_QBUF) failed when requesting new frame! fd: " << sync_queue.front()._fd << " error: " << strerror(errno)); + } + sync_queue.pop(); + } + + void v4l2_video_md_syncer::flush() { // Empty queues diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index 1d7e3711a2..c177866f42 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -278,6 +278,7 @@ namespace librealsense private: void enqueue_buffer_before_throwing_it(const sync_buffer& sb) const; + void enqueue_front_buffer_before_throwing_it(std::queue& sync_queue); std::mutex _syncer_mutex; std::queue _video_queue; @@ -351,9 +352,12 @@ namespace librealsense virtual void prepare_capture_buffers() override; virtual void stop_data_capture() override; virtual void acquire_metadata(buffers_mgr & buf_mgr,fd_set &fds, bool compressed_format = false) override; + virtual void set_metadata_attributes(buffers_mgr& buf_mgr, __u32 bytesused, uint8_t* md_start); void subscribe_to_ctrl_event(uint32_t control_id); void unsubscribe_from_ctrl_event(uint32_t control_id); bool pend_for_ctrl_status_event(); + void upload_video_and_metadata_from_syncer(buffers_mgr& buf_mgr); + void populate_imu_data(metadata_hid_raw& meta_data, uint8_t* frame_start, uint8_t& md_size, void** md_start) const; // checking if metadata is streamed virtual inline bool is_metadata_streamed() const { return false;} virtual inline std::shared_ptr get_video_buffer(__u32 index) const {return _buffers[index];} @@ -428,6 +432,7 @@ 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; + void set_metadata_attributes(buffers_mgr& buf_mgr, __u32 bytesused, uint8_t* md_start) override; protected: virtual uint32_t get_cid(rs2_option option) const; uint32_t xu_to_cid(const extension_unit& xu, uint8_t control) const; // Find the mapping of XU to the underlying control diff --git a/src/option.cpp b/src/option.cpp index faebb41a22..92d16eb1ff 100644 --- a/src/option.cpp +++ b/src/option.cpp @@ -120,8 +120,7 @@ std::vector librealsense::command_transfer_over_xu::send_receive(const std::dec << data.size() << " exceeds permitted limit " << HW_MONITOR_BUFFER_SIZE); } - // D457 - size of 1028 needed instead of 1024 (HW_MONITOR_BUFFER_SIZE) - std::vector transmit_buf(HW_MONITOR_BUFFER_SIZE + SIZE_OF_HW_MONITOR_HEADER, 0); + 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()))) @@ -134,14 +133,8 @@ std::vector librealsense::command_transfer_over_xu::send_receive(const 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)) ; + auto data_size = *(reinterpret_cast(result.data() + HW_MONITOR_DATA_SIZE_OFFSET)) + SIZE_OF_HW_MONITOR_HEADER; result.resize(data_size); - - // D457 code - stepping over 24 bytes: - // 4 bytes: header and magic number - // 20 bytes: input command - // this code may be removed after some bug correction in the kernel code - result.insert(result.begin(),transmit_buf.begin() + 24,transmit_buf.begin() + 24 + data_size); } return result; }); diff --git a/src/sensor.cpp b/src/sensor.cpp index ccf1f25d16..47a2a281fe 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -283,27 +283,15 @@ void log_callback_end( uint32_t fps, { auto system_time = environment::get_instance().get_time_service()->get_time(); auto fr = std::make_shared(); - byte* pix = (byte*)fo.pixels; - std::vector pixels; + + fr->set_stream(profile); + + // D457 dev - computing relevant frame size const auto&& vsp = As(profile); int width = vsp ? vsp->get_width() : 0; int height = vsp ? vsp->get_height() : 0; int bpp = get_image_bpp(profile->get_format()); - - // method should be limited to use of MIPI - not for USB - // the aim is to grab the data from a bigger buffer, which is aligned to 64 bytes, - // when the resolution's width is not aligned to 64 - if (width % 64 != 0 && fo.frame_size > compute_frame_expected_size(width, height, bpp)) - { - pixels = align_width_to_64(width, height, bpp, pix); - } - else - { - pixels = std::vector(pix, pix + fo.frame_size); - } - - fr->data = pixels; - fr->set_stream(profile); + auto frame_size = compute_frame_expected_size(width, height, bpp); frame_additional_data additional_data(0, 0, @@ -315,7 +303,7 @@ void log_callback_end( uint32_t fps, last_frame_number, false, 0, - (uint32_t)pixels.size()); + (uint32_t)frame_size); if (_metadata_modifier) _metadata_modifier(additional_data); @@ -441,10 +429,20 @@ void log_callback_end( uint32_t fps, if (fh.frame) { - assert( expected_size == sizeof(byte) * fr->data.size() || - expected_size == sizeof(byte) * fr->data.size() + 68); // added for D457 - need to understand why this happens (68 is size of md) - - memcpy((void*)fh->get_frame_data(), fr->data.data(), sizeof(byte) * fr->data.size()); + // method should be limited to use of MIPI - not for USB + // the aim is to grab the data from a bigger buffer, which is aligned to 64 bytes, + // when the resolution's width is not aligned to 64 + if ((width * bpp >> 3) % 64 != 0 && f.frame_size > expected_size) + { + std::vector pixels = align_width_to_64(width, height, bpp, (byte*)f.pixels); + assert( expected_size == sizeof(byte) * pixels.size()); + memcpy( (void *)fh->get_frame_data(), pixels.data(), expected_size ); + } + else + { + assert( expected_size == sizeof(byte) * f.frame_size ); + memcpy( (void *)fh->get_frame_data(), f.pixels, expected_size ); + } auto&& video = dynamic_cast(fh.frame); if (video) @@ -655,8 +653,9 @@ void log_callback_end( uint32_t fps, stream_profiles uvc_sensor::init_stream_profiles() { - // D457 development - std::unordered_set> profiles; - std::unordered_set> profiles; + std::unordered_set> video_profiles; + // D457 development - only via mipi imu frames com from uvc instead of hid + std::unordered_set> motion_profiles; power on(std::dynamic_pointer_cast(shared_from_this())); _uvc_profiles = _device->get_profiles(); @@ -675,7 +674,7 @@ void log_callback_end( uint32_t fps, profile->set_stream_index(0); profile->set_format(rs2_fmt); profile->set_framerate(p.fps); - profiles.insert(profile); + motion_profiles.insert(profile); } else { @@ -685,11 +684,12 @@ void log_callback_end( uint32_t fps, profile->set_stream_index(0); profile->set_format(rs2_fmt); profile->set_framerate(p.fps); - profiles.insert(profile); + video_profiles.insert(profile); } } - stream_profiles result{ profiles.begin(), profiles.end() }; + stream_profiles result{ video_profiles.begin(), video_profiles.end() }; + result.insert(result.end(), motion_profiles.begin(), motion_profiles.end()); return result; } @@ -964,7 +964,9 @@ void log_callback_end( uint32_t fps, last_frame_number = frame_counter; last_timestamp = timestamp; frame_holder frame = _source.alloc_frame(RS2_EXTENSION_MOTION_FRAME, data_size, fr->additional_data, true); - memcpy((void*)frame->get_frame_data(), fr->data.data(), sizeof(byte)*fr->data.size()); + memcpy( (void *)frame->get_frame_data(), + sensor_data.fo.pixels, + sizeof( byte ) * sensor_data.fo.frame_size ); if (!frame) { LOG_INFO("Dropped frame. alloc_frame(...) returned nullptr"); @@ -1550,8 +1552,6 @@ void log_callback_end( uint32_t fps, for (auto source : requests) add_source_profile_missing_data(source); - - const auto&& resolved_req = resolve_requests(requests); _raw_sensor->set_source_owner(this); diff --git a/unit-tests/live/frames/test-fps.py b/unit-tests/live/frames/test-fps.py new file mode 100644 index 0000000000..39e66be390 --- /dev/null +++ b/unit-tests/live/frames/test-fps.py @@ -0,0 +1,109 @@ +# License: Apache 2.0. See LICENSE file in root directory. +# Copyright(c) 2022 Intel Corporation. All Rights Reserved. + +# test:device L500* +# test:device D400* +# test:donotrun:!nightly +# test:timeout 230 + +import pyrealsense2 as rs +from rspy.stopwatch import Stopwatch +from rspy import test, log +import time +import platform + +# Start depth + color streams and measure frame frequency using sensor API. +# Verify that actual fps is as requested + +def measure_fps(sensor, profile): + """ + Wait for 2 seconds to be sure that frames are at steady state after start + Count number of received frames for 5 seconds and compare actual fps to requested fps + """ + seconds_till_steady_state = 2 + seconds_to_count_frames = 20 + + steady_state = False + frames_received = 0 + fps_stopwatch = Stopwatch() + + def frame_cb(frame): + nonlocal steady_state, frames_received + if steady_state: + frames_received += 1 + + sensor.open(profile) + sensor.start(frame_cb) + + time.sleep(seconds_till_steady_state) + + steady_state = True + fps_stopwatch.reset() + + time.sleep(seconds_to_count_frames) # Time to count frames + + steady_state = False # Stop counting + + sensor.stop() + sensor.close() + + fps = frames_received / seconds_to_count_frames + return fps + + +delta_Hz = 1 +tested_fps = [6, 15, 30, 60, 90] + +dev = test.find_first_device_or_exit() +product_line = dev.get_info(rs.camera_info.product_line) + +##################################################################################################### +test.start("Testing depth fps " + product_line + " device - "+ platform.system() + " OS") + +for requested_fps in tested_fps: + ds = dev.first_depth_sensor() + #Set auto-exposure option as it might take precedence over requested FPS + if product_line == "D400": + ds.set_option(rs.option.enable_auto_exposure, 1) + + try: + dp = next(p for p in ds.profiles + if p.fps() == requested_fps + and p.stream_type() == rs.stream.depth + and p.format() == rs.format.z16) + except StopIteration: + print("Requested fps: {:.1f} [Hz], not supported".format(requested_fps)) + else: + fps = measure_fps(ds, dp) + print("Requested fps: {:.1f} [Hz], actual fps: {:.1f} [Hz] ".format(requested_fps, fps)) + test.check(fps <= (requested_fps + delta_Hz) and fps >= (requested_fps - delta_Hz)) +test.finish() + + +##################################################################################################### +test.start("Testing color fps " + product_line + " device - "+ platform.system() + " OS") + +for requested_fps in tested_fps: + cs = dev.first_color_sensor() + #Set auto-exposure option as it might take precedence over requested FPS + if product_line == "D400": + ds.set_option(rs.option.enable_auto_exposure, 1) + elif product_line == "L500": + cs.set_option(rs.option.enable_auto_exposure, 0) + + try: + cp = next(p for p in cs.profiles + if p.fps() == requested_fps + and p.stream_type() == rs.stream.color + and p.format() == rs.format.rgb8) + except StopIteration: + print("Requested fps: {:.1f} [Hz], not supported".format(requested_fps)) + else: + fps = measure_fps(cs, cp) + print("Requested fps: {:.1f} [Hz], actual fps: {:.1f} [Hz] ".format(requested_fps, fps)) + test.check(fps <= (requested_fps + delta_Hz) and fps >= (requested_fps - delta_Hz)) + +test.finish() + +##################################################################################################### +test.print_results_and_exit()