Skip to content

Commit

Permalink
align to GVD spec 0.4
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nir-Az
Nir-Az committed Nov 5, 2023
1 parent fd45b37 commit c527dab
Show file tree
Hide file tree
Showing 5 changed files with 89 additions and 55 deletions.
46 changes: 27 additions & 19 deletions src/ds/d500/d500-device.cpp
View file
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
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
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
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
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

0 comments on commit c527dab

Please sign in to comment.