PR #12357 from Nir-Az: Align to D500 GVD spec 0.4

Nir-Az · web-flow · commit 83af55f43c0b · 2023-11-05T15:24:04.000+02:00
diff --git 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<uint8_t>& gvd_buff, ds::d500_gvd_parsed_fields* parsed_fields) const
     {
-        parsed_fields->gvd_version = *reinterpret_cast<const uint16_t*>(gvd_buff.data() + static_cast<int>(ds::d500_gvd_fields::version_offset));
-        parsed_fields->payload_size = *reinterpret_cast<const uint32_t*>(gvd_buff.data() + static_cast<int>(ds::d500_gvd_fields::payload_size_offset));
-        parsed_fields->crc32 = *reinterpret_cast<const uint32_t*>(gvd_buff.data() + static_cast<int>(ds::d500_gvd_fields::crc32_offset));
-        parsed_fields->optical_module_sn = _hw_monitor->get_module_serial_string(gvd_buff, static_cast<size_t>(ds::d500_gvd_fields::optical_module_serial_offset));
-        parsed_fields->mb_module_sn = _hw_monitor->get_module_serial_string(gvd_buff, static_cast<size_t>(ds::d500_gvd_fields::mb_module_serial_offset));
-        parsed_fields->fw_version = _hw_monitor->get_firmware_version_string(gvd_buff, static_cast<size_t>(ds::d500_gvd_fields::fw_version_offset));
-        parsed_fields->safety_sw_suite_version = _hw_monitor->get_firmware_version_string(gvd_buff, static_cast<size_t>(ds::d500_gvd_fields::safety_sw_suite_version_offset), 3);
+        parsed_fields->gvd_version[0] = *reinterpret_cast<const uint8_t*>(gvd_buff.data() + ds::d500_gvd_offsets::version_offset);
+        parsed_fields->gvd_version[1] = *reinterpret_cast<const uint8_t*>(gvd_buff.data() + ds::d500_gvd_offsets::version_offset + sizeof(uint8_t));
+        
+        parsed_fields->payload_size = *reinterpret_cast<const uint32_t*>(gvd_buff.data() + ds::d500_gvd_offsets::payload_size_offset);
+        parsed_fields->crc32 = *reinterpret_cast<const uint32_t*>(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<uint16_t>(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
@@ -43,40 +43,33 @@ namespace librealsense
         bool d500_try_fetch_usb_device(std::vector<platform::usb_device_info>& 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<ds::d500_calibration_table_id, uint32_t> 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
@@ -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<uint8_t>(gvd_buff, camera_fw_version_offset);
     }
 
     std::vector<uint8_t> ds_device_common::backup_flash(update_progress_callback_ptr callback)
diff --git 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<uint8_t>& buff, size_t index, size_t length)
-    {
-        std::stringstream formattedBuffer;
-        std::string s = "";
-        for (auto i = 1; i <= length; i++)
-        {
-            formattedBuffer << s << static_cast<int>(buff[index + (length - i)]);
-            s = ".";
-        }
-
-        return formattedBuffer.str();
-    }
 
     std::string hw_monitor::get_module_serial_string(const std::vector<uint8_t>& buff, size_t index, size_t length)
     {
diff --git a/src/hw-monitor.h b/src/hw-monitor.h
@@ -7,6 +7,8 @@
 #include <mutex>
 #include "platform/command-transfer.h"
 #include <string>
+#include <algorithm>
+#include <vector>
 
 
 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<uint8_t>& buff, size_t index, size_t length = 4);
+
+        template<typename T>
+        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<int> 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<uint8_t>& buff, size_t index, size_t length = 6);
         bool is_camera_locked(uint8_t gvd_cmd, uint32_t offset) const;
 
