Skip to content

Align to D500 GVD spec 0.4 #12357

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Nir-Az merged 1 commit into from
Nov 5, 2023
Merged

Align to D500 GVD spec 0.4 #12357

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 27 additions & 19 deletions src/ds/d500/d500-device.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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 );
}

}
}
40 changes: 18 additions & 22 deletions src/ds/d500/d500-private.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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},
Expand Down Expand Up @@ -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];
Expand Down
2 changes: 1 addition & 1 deletion src/ds/ds-device-common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
12 changes: 0 additions & 12 deletions src/hw-monitor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
44 changes: 43 additions & 1 deletion src/hw-monitor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@
#include <mutex>
#include "platform/command-transfer.h"
#include <string>
#include <algorithm>
#include <vector>


namespace librealsense
Expand Down Expand Up @@ -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;

Expand Down