From c527dabb47dc01bbbe3d59c676154fc39141c4a1 Mon Sep 17 00:00:00 2001 From: Nir Azkiel Date: Sun, 5 Nov 2023 12:52:47 +0200 Subject: [PATCH] align to GVD spec 0.4 --- src/ds/d500/d500-device.cpp | 46 ++++++++++++++++++++++--------------- src/ds/d500/d500-private.h | 40 +++++++++++++++----------------- src/ds/ds-device-common.cpp | 2 +- src/hw-monitor.cpp | 12 ---------- src/hw-monitor.h | 44 ++++++++++++++++++++++++++++++++++- 5 files changed, 89 insertions(+), 55 deletions(-) diff --git a/src/ds/d500/d500-device.cpp b/src/ds/d500/d500-device.cpp index 5ecdae50480..363627944b3 100644 --- a/src/ds/d500/d500-device.cpp +++ b/src/ds/d500/d500-device.cpp @@ -455,22 +455,14 @@ namespace librealsense group_multiple_fw_calls(depth_sensor, [&]() { _hw_monitor->get_gvd(gvd_buff.size(), gvd_buff.data(), ds::fw_cmd::GVD); - - constexpr auto gvd_header_size = 8; get_gvd_details(gvd_buff, &gvd_parsed_fields); - auto gvd_payload_data = gvd_buff.data() + gvd_header_size; - auto computed_crc = calc_crc32(gvd_payload_data, gvd_parsed_fields.payload_size); - LOG_INFO("gvd version = " << gvd_parsed_fields.gvd_version); - LOG_INFO("gvd payload size = " << gvd_parsed_fields.payload_size); - LOG_INFO("gvd crc = " << gvd_parsed_fields.crc32); - LOG_INFO("gvd optical module sn = " << gvd_parsed_fields.optical_module_sn); - if (computed_crc != gvd_parsed_fields.crc32) - LOG_ERROR("CRC mismatch in D500 GVD - received CRC = " << gvd_parsed_fields.crc32 << ", computed CRC = " << computed_crc); - + _device_capabilities = ds_caps::CAP_ACTIVE_PROJECTOR | ds_caps::CAP_RGB_SENSOR | ds_caps::CAP_IMU_SENSOR | ds_caps::CAP_BMI_085 | ds_caps::CAP_GLOBAL_SHUTTER | ds_caps::CAP_INTERCAM_HW_SYNC; - _fw_version = firmware_version(gvd_parsed_fields.fw_version); + // TODO Currently D500 version format does not match the expected format of rsutils::version + // This will need some special handling + //_fw_version = firmware_version(gvd_parsed_fields.fw_version); auto _usb_mode = usb3_type; usb_type_str = usb_spec_names.at(_usb_mode); @@ -799,12 +791,28 @@ namespace librealsense void d500_device::get_gvd_details(const std::vector& gvd_buff, ds::d500_gvd_parsed_fields* parsed_fields) const { - parsed_fields->gvd_version = *reinterpret_cast(gvd_buff.data() + static_cast(ds::d500_gvd_fields::version_offset)); - parsed_fields->payload_size = *reinterpret_cast(gvd_buff.data() + static_cast(ds::d500_gvd_fields::payload_size_offset)); - parsed_fields->crc32 = *reinterpret_cast(gvd_buff.data() + static_cast(ds::d500_gvd_fields::crc32_offset)); - parsed_fields->optical_module_sn = _hw_monitor->get_module_serial_string(gvd_buff, static_cast(ds::d500_gvd_fields::optical_module_serial_offset)); - parsed_fields->mb_module_sn = _hw_monitor->get_module_serial_string(gvd_buff, static_cast(ds::d500_gvd_fields::mb_module_serial_offset)); - parsed_fields->fw_version = _hw_monitor->get_firmware_version_string(gvd_buff, static_cast(ds::d500_gvd_fields::fw_version_offset)); - parsed_fields->safety_sw_suite_version = _hw_monitor->get_firmware_version_string(gvd_buff, static_cast(ds::d500_gvd_fields::safety_sw_suite_version_offset), 3); + parsed_fields->gvd_version[0] = *reinterpret_cast(gvd_buff.data() + ds::d500_gvd_offsets::version_offset); + parsed_fields->gvd_version[1] = *reinterpret_cast(gvd_buff.data() + ds::d500_gvd_offsets::version_offset + sizeof(uint8_t)); + + parsed_fields->payload_size = *reinterpret_cast(gvd_buff.data() + ds::d500_gvd_offsets::payload_size_offset); + parsed_fields->crc32 = *reinterpret_cast(gvd_buff.data() + ds::d500_gvd_offsets::crc32_offset); + parsed_fields->optical_module_sn = _hw_monitor->get_module_serial_string(gvd_buff, ds::d500_gvd_offsets::optical_module_serial_offset); + parsed_fields->mb_module_sn = _hw_monitor->get_module_serial_string(gvd_buff, ds::d500_gvd_offsets::mb_module_serial_offset); + parsed_fields->fw_version = _hw_monitor->get_firmware_version_string(gvd_buff, ds::d500_gvd_offsets::fw_version_offset, 4, false); + + constexpr size_t gvd_header_size = 8; + auto gvd_payload_data = gvd_buff.data() + gvd_header_size; + auto computed_crc = calc_crc32( gvd_payload_data, parsed_fields->payload_size ); + LOG_INFO( "D500 GVD version is: " << static_cast< int >( parsed_fields->gvd_version[0] ) + << "." + << static_cast< int >( parsed_fields->gvd_version[1] ) ); + LOG_INFO( "D500 GVD payload_size is: " << parsed_fields->payload_size ); + + if( computed_crc != parsed_fields->crc32 ) + { + LOG_ERROR( "CRC mismatch in D500 GVD - received CRC = " + << parsed_fields->crc32 << ", computed CRC = " << computed_crc ); + } + } } diff --git a/src/ds/d500/d500-private.h b/src/ds/d500/d500-private.h index f6f91580fc0..e1abb5d83db 100644 --- a/src/ds/d500/d500-private.h +++ b/src/ds/d500/d500-private.h @@ -43,40 +43,33 @@ namespace librealsense bool d500_try_fetch_usb_device(std::vector& devices, const platform::uvc_device_info& info, platform::usb_device_info& result); - // Keep sorted - enum class d500_gvd_fields // gvd fields for Safety Camera + namespace d500_gvd_offsets { - version_offset = 0, //ES1 - payload_size_offset = 0x2, //ES1 - crc32_offset = 0x4, //ES1 - optical_module_serial_offset = 0x54, //ES1 - mb_module_serial_offset = 0x7a, //ES2 - fw_version_offset = 0xba, //ES2 - safety_sw_suite_version_offset = 0x10F //ES2 - //rgb_sensor = 0x17, - //imu_sensor = 0x19, - //active_projector = 0x1a, - //camera_fw_version_offset = 0x8c, - //is_camera_locked_offset = 0x9e, - }; + constexpr size_t version_offset = 0; + constexpr size_t payload_size_offset = 0x2; + constexpr size_t crc32_offset = 0x4; + constexpr size_t optical_module_serial_offset = 0x54; + constexpr size_t mb_module_serial_offset = 0x7a; + constexpr size_t fw_version_offset = 0xba; + }; // namespace d500_gvd_offsets struct d500_gvd_parsed_fields { - uint16_t gvd_version; + uint8_t gvd_version[2]; uint16_t payload_size; uint32_t crc32; std::string optical_module_sn; std::string mb_module_sn; std::string fw_version; - std::string safety_sw_suite_version; }; enum class d500_calibration_table_id { depth_eeprom_toc_id = 0xb0, - module_info_id = 0xb1, + module_info_id = 0x1b1, rgb_lens_shading_id = 0xb2, - str_lens_shading_id = 0xb3, + left_lens_shading_id = 0x1b3, + right_lens_shading_id = 0x2b3, depth_calibration_id = 0xb4, left_x_lut_id = 0xb5, left_y_lut_id = 0xb6, @@ -91,9 +84,10 @@ namespace librealsense const std::map d500_calibration_tables_size = { {d500_calibration_table_id::depth_eeprom_toc_id, 640}, - {d500_calibration_table_id::module_info_id, 320}, + {d500_calibration_table_id::module_info_id, 512}, {d500_calibration_table_id::rgb_lens_shading_id, 1088}, - {d500_calibration_table_id::str_lens_shading_id, 1088}, + {d500_calibration_table_id::left_lens_shading_id, 576}, + {d500_calibration_table_id::right_lens_shading_id, 512}, {d500_calibration_table_id::depth_calibration_id, 512}, {d500_calibration_table_id::left_x_lut_id, 4160}, {d500_calibration_table_id::left_y_lut_id, 4160}, @@ -146,7 +140,9 @@ namespace librealsense single_sensor_coef_table left_coefficients_table; single_sensor_coef_table right_coefficients_table; float baseline; // the baseline between the cameras in mm units - uint16_t translation_dir; + uint8_t translation_dir; + uint8_t realignement_essential; // 1/0 - indicates whether the vertical alignement + // is required to avoiid overflow in the REC buffer int16_t vertical_shift; // in pixels mini_intrinsics rectified_intrinsics; uint8_t reserved[148]; diff --git a/src/ds/ds-device-common.cpp b/src/ds/ds-device-common.cpp index 333a980b3ff..667a83b8600 100644 --- a/src/ds/ds-device-common.cpp +++ b/src/ds/ds-device-common.cpp @@ -124,7 +124,7 @@ namespace librealsense { 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); + fwv = _hw_monitor->get_firmware_version_string(gvd_buff, camera_fw_version_offset); } std::vector ds_device_common::backup_flash(update_progress_callback_ptr callback) diff --git a/src/hw-monitor.cpp b/src/hw-monitor.cpp index b24b7e62372..4fb81abc5c1 100644 --- a/src/hw-monitor.cpp +++ b/src/hw-monitor.cpp @@ -9,18 +9,6 @@ namespace librealsense { - std::string hw_monitor::get_firmware_version_string(const std::vector& buff, size_t index, size_t length) - { - std::stringstream formattedBuffer; - std::string s = ""; - for (auto i = 1; i <= length; i++) - { - formattedBuffer << s << static_cast(buff[index + (length - i)]); - s = "."; - } - - return formattedBuffer.str(); - } std::string hw_monitor::get_module_serial_string(const std::vector& buff, size_t index, size_t length) { diff --git a/src/hw-monitor.h b/src/hw-monitor.h index 91611070497..0b19de26022 100644 --- a/src/hw-monitor.h +++ b/src/hw-monitor.h @@ -7,6 +7,8 @@ #include #include "platform/command-transfer.h" #include +#include +#include namespace librealsense @@ -350,7 +352,47 @@ namespace librealsense size_t dataLength = 0); void get_gvd(size_t sz, unsigned char* gvd, uint8_t gvd_cmd) const; - static std::string get_firmware_version_string(const std::vector& buff, size_t index, size_t length = 4); + + template + std::string get_firmware_version_string( const std::vector< uint8_t > & buff, + size_t index, + size_t length = 4, + bool reversed = true ) + { + std::stringstream formattedBuffer; + auto component_bytes_size = sizeof( T ); + std::string s = ""; + if( buff.size() < index + ( length * component_bytes_size )) + { + // Don't throw as we want to be back compatible even w/o a working version + LOG_ERROR( "GVD FW version cannot be read!" ); + return formattedBuffer.str(); + } + + // We iterate through the version components (major.minor.patch.build) and append each + // string value to the result string + std::vector components_value; + for( auto i = 0; i < length; i++ ) + { + size_t component_index = index + ( i * component_bytes_size ); + + // We use int on purpose as types like uint8_t doesn't work as expected with << operator + int component_value = *reinterpret_cast< const T * >( buff.data() + component_index ); + components_value.push_back(component_value); + } + + if( reversed ) + std::reverse( components_value.begin(), components_value.end() ); + + for( auto & element : components_value ) + { + formattedBuffer << s << element; + s = "."; + } + + return formattedBuffer.str(); + } + static std::string get_module_serial_string(const std::vector& buff, size_t index, size_t length = 6); bool is_camera_locked(uint8_t gvd_cmd, uint32_t offset) const;