Skip to content

Commit

Permalink
Using _debug_dev for ETH. get_calibration_table returns full table, h…
Browse files Browse the repository at this point in the history 
…eader included.
  • Loading branch information
@OhadMeir
OhadMeir committed Sep 25, 2024
1 parent 600391b commit b962969
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 58 deletions.
90 changes: 51 additions & 39 deletions src/ds/d500/d500-auto-calibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@

namespace librealsense
{
static constexpr const size_t hwm_header_size = 4;

static const std::string calibration_state_strings[] = {
"Idle",
"In Process",
Expand Down Expand Up @@ -92,7 +94,7 @@ namespace librealsense
rs2_update_progress_callback_sptr progress_callback )
{
bool is_d555 = false;
auto dev = As< device >( this );
auto dev = As< device >( _debug_dev );
std::string pid_str = dev ? dev->get_info( RS2_CAMERA_INFO_PRODUCT_ID ) : "";
if( pid_str == "0B56" || pid_str == "DDS" )
is_d555 = true;
Expand Down Expand Up @@ -169,17 +171,17 @@ namespace librealsense
LOG_DEBUG("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling);

ds_calib_common::param4 p4;
p4.scan_parameter = scan_parameter;
p4.data_sampling = data_sampling;
p4.field.scan_parameter = scan_parameter;
p4.field.data_sampling = data_sampling;

// Begin auto-calibration
auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_BEGIN, speed, 0, p4.as_uint32 );
std::vector< uint8_t > res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes?
_debug_dev->send_receive_raw_data( cmd );

ds_calib_common::dsc_check_status_result result = get_calibration_status(timeout_ms, [progress_callback, speed](int count)
{
if( progress_callback )
progress_callback->on_update_progress( count++ * ( 2.f * static_cast< int >( speed ) ) ); // Currently this number does not reflect the actual progress
progress_callback->on_update_progress( count * speed * 1.2f ); // Currently this number does not reflect the actual progress
});
std::this_thread::sleep_for(std::chrono::milliseconds(100));

Expand All @@ -189,7 +191,7 @@ namespace librealsense
ds_calib_common::handle_calibration_error( result.status );
}

res = get_calibration_results(health);
std::vector< uint8_t > res = get_calibration_results( health );

if (progress_callback)
progress_callback->on_update_progress(static_cast<float>(100));
Expand Down Expand Up @@ -327,13 +329,13 @@ namespace librealsense
auto p2 = static_cast< uint32_t >( ground_truth_mm ) * 100;

ds_calib_common::param3 p3;
p3.average_step_count = average_step_count;
p3.step_count = step_count;
p3.accuracy = accuracy;
p3.field.average_step_count = average_step_count;
p3.field.step_count = step_count;
p3.field.accuracy = accuracy;

ds_calib_common::param4 p4;
p4.scan_parameter = scan_parameter;
p4.data_sampling = data_sampling;
p4.field.scan_parameter = scan_parameter;
p4.field.data_sampling = data_sampling;

// Log the current preset
//auto advanced_mode = dynamic_cast< ds_advanced_mode_base * >( this );
Expand All @@ -345,7 +347,7 @@ namespace librealsense
//}

auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_BEGIN, p2, p3.as_uint32, p4.as_uint32 );
_debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes?
_debug_dev->send_receive_raw_data( cmd );

ds_calib_common::TareCalibrationResult result;

Expand All @@ -365,12 +367,13 @@ namespace librealsense
try
{
auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::TARE_CALIB_CHECK_STATUS );
res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes?
if( res.size() < sizeof( ds_calib_common::TareCalibrationResult ) )
res = _debug_dev->send_receive_raw_data( cmd );
if( res.size() < sizeof( ds_calib_common::TareCalibrationResult ) + hwm_header_size )
{
throw std::runtime_error( "Not enough data from CALIB_STATUS!" );
}

res.erase( res.begin(), res.begin() + hwm_header_size ); // Slicing HWM command header
result = *reinterpret_cast< ds_calib_common::TareCalibrationResult * >( res.data() );
done = result.status != ds_calib_common::STATUS_RESULT_NOT_READY;
}
Expand Down Expand Up @@ -433,17 +436,28 @@ namespace librealsense

void d500_auto_calibrated::set_calibration_table(const std::vector<uint8_t>& calibration)
{
if (_curr_calibration.size() != sizeof(ds::table_header) && // First time setting table, only header set by get_calibration_table
_curr_calibration.size() != sizeof(ds::d500_coefficients_table)) // Table was previously set
throw std::runtime_error(rsutils::string::from() <<
"Current calibration table has unexpected size " << _curr_calibration.size());

if (calibration.size() != sizeof(ds::d500_coefficients_table) - sizeof(ds::table_header))
throw std::runtime_error(rsutils::string::from() <<
"Setting calibration table with unexpected size" << calibration.size());

_curr_calibration.resize(sizeof(ds::table_header)); // Remove previously set calibration, keep header.
_curr_calibration.insert(_curr_calibration.end(), calibration.begin(), calibration.end());
auto table_header = reinterpret_cast< const ds::table_header * >( calibration.data() );

size_t expected_size = sizeof( ds::d500_coefficients_table );
if( table_header->table_type == static_cast< uint16_t >( ds::d500_calibration_table_id::rgb_calibration_id ) )
expected_size = sizeof( ds::d500_rgb_calibration_table );

if( calibration.size() != expected_size )
throw std::runtime_error( rsutils::string::from()
<< "Setting calibration table with unexpected size" << calibration.size()
<< " while expecting " << expected_size );

_curr_calibration = calibration;

// Send updated calibration to RAM so it can be used. Will be saved only on write_calibration()
auto cmd = _debug_dev->build_command( ds::SET_HKR_CONFIG_TABLE,
static_cast< int >( ds::d500_calib_location::d500_calib_ram_memory ),
static_cast< int >( table_header->table_type ),
static_cast< int >( ds::d500_calib_type::d500_calib_dynamic ),
0,
_curr_calibration.data(),
_curr_calibration.size() );
_debug_dev->send_receive_raw_data( cmd );
}

void d500_auto_calibrated::reset_to_factory_calibration() const
Expand Down Expand Up @@ -472,10 +486,8 @@ namespace librealsense
std::vector< uint8_t > ret;
const float correction_factor = 0.5f;

auto table_data = get_calibration_table(); // Table is returned without the header
auto full_table = _curr_calibration; // During get_calibration_table header is saved in _curr_calibration
full_table.insert( full_table.end(), table_data.begin(), table_data.end() );
auto table = reinterpret_cast< librealsense::ds::d500_coefficients_table * >( full_table.data() );
auto calib_table = get_calibration_table();
auto table = reinterpret_cast< librealsense::ds::d500_coefficients_table * >( calib_table.data() );

float ratio_to_apply = ds_calib_common::get_focal_length_correction_factor( left_rect_sides,
right_rect_sides,
Expand All @@ -501,9 +513,7 @@ namespace librealsense
table->right_coefficients_table.base_instrinsics.fy *= ratio_to_apply;
}

//Return data without header
table_data.assign( full_table.begin() + sizeof( ds::table_header ), full_table.end() );
return table_data;
return calib_table;
}

std::vector<uint8_t> d500_auto_calibrated::run_uv_map_calibration(rs2_frame_queue* left, rs2_frame_queue* color, rs2_frame_queue* depth, int py_px_only,
Expand Down Expand Up @@ -579,14 +589,15 @@ namespace librealsense
{
// Check calibration status
auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::PY_RX_CALIB_CHECK_STATUS );
auto res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes?
if( res.size() < sizeof( ds_calib_common::dsc_check_status_result ) )
auto res = _debug_dev->send_receive_raw_data( cmd );
if( res.size() < sizeof( ds_calib_common::dsc_check_status_result ) + hwm_header_size )
{
if( ! ( ( retries++ ) % 5 ) ) // Add log debug once a sec
LOG_DEBUG( "Not enough data from CALIB_STATUS!" );
}
else
{
res.erase( res.begin(), res.begin() + hwm_header_size ); // Slicing HWM command header
result = *reinterpret_cast< ds_calib_common::dsc_check_status_result * >( res.data() );
done = ! wait_for_final_results || result.status != ds_calib_common::STATUS_RESULT_NOT_READY;
}
Expand All @@ -598,7 +609,7 @@ namespace librealsense

if( progress_func )
{
progress_func( count );
progress_func( count++ );
}

now = std::chrono::high_resolution_clock::now();
Expand All @@ -617,10 +628,11 @@ namespace librealsense
{
// Get new calibration from the firmware
auto cmd = _debug_dev->build_command( ds::AUTO_CALIB, ds_calib_common::GET_CALIBRATION_RESULT );
auto res = _debug_dev->send_receive_raw_data( cmd ); // TODO - need to remove res first 4 bytes?
if( res.size() < sizeof( ds_calib_common::dsc_result ) )
auto res = _debug_dev->send_receive_raw_data( cmd );
if( res.size() < sizeof( ds_calib_common::dsc_result ) + hwm_header_size )
throw std::runtime_error( "Not enough data from CALIB_STATUS!" );

res.erase( res.begin(), res.begin() + hwm_header_size ); // Slicing HWM command header
auto * header = reinterpret_cast< ds::table_header * >( res.data() + sizeof( ds_calib_common::dsc_result ) );
if( res.size() < sizeof( ds_calib_common::dsc_result ) + sizeof( ds::table_header ) + header->table_size )
throw std::runtime_error( "Table truncated in CALIB_STATUS!" );
Expand All @@ -629,9 +641,9 @@ namespace librealsense
calib.resize( sizeof( ds::table_header ) + header->table_size, 0 );
memcpy( calib.data(), header, calib.size() ); // Copy to new_calib

auto reslt = reinterpret_cast< ds_calib_common::dsc_result * >( res.data() );
auto result = reinterpret_cast< ds_calib_common::dsc_result * >( res.data() );
if( health )
*health = reslt->healthCheck;
*health = result->healthCheck;

return calib;
}
Expand Down
26 changes: 9 additions & 17 deletions src/ds/d500/d500-debug-protocol-calibration-engine.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,14 +76,13 @@ int8_t d500_debug_protocol_calibration_engine::get_triggered_calibration_progres
std::vector<uint8_t> d500_debug_protocol_calibration_engine::get_calibration_table(std::vector<uint8_t>& current_calibration) const
{
// Getting depth calibration table. RGB table is currently not supported by auto_calibrated_interface API
std::vector< uint8_t > res;

// prepare command
using namespace ds;
auto cmd = _dev->build_command(ds::GET_HKR_CONFIG_TABLE,
static_cast<int>(d500_calib_location::d500_calib_flash_memory),
static_cast<int>(d500_calibration_table_id::depth_calibration_id),
static_cast<int>(ds::d500_calib_type::d500_calib_dynamic));
static_cast<int>(d500_calib_location::d500_calib_flash_memory),
static_cast<int>(d500_calibration_table_id::depth_calibration_id),
static_cast<int>(ds::d500_calib_type::d500_calib_dynamic));

// sending command
auto calib = _dev->send_receive_raw_data(cmd);
Expand All @@ -95,32 +94,25 @@ std::vector<uint8_t> d500_debug_protocol_calibration_engine::get_calibration_tab
calib.erase(calib.begin(), calib.begin() + 4);

auto header = (ds::table_header*)(calib.data());

if (calib.size() < sizeof(ds::table_header) + header->table_size)
throw std::runtime_error("GET_HKR_CONFIG_TABLE response is smaller then expected table size!");

// Backwards compatibility dictates that we will return the table without the header, but we need the header
// details like versions to later set back the table. Save it at the start of _curr_calibration.
current_calibration.assign(calib.begin(), calib.begin() + sizeof(ds::table_header));

res.assign(calib.begin() + sizeof(ds::table_header), calib.end());

return res;
return calib;
}

void d500_debug_protocol_calibration_engine::write_calibration(std::vector<uint8_t>& current_calibration) const
{
auto table_header = reinterpret_cast<ds::table_header*>(current_calibration.data());
table_header->crc32 = rsutils::number::calc_crc32(current_calibration.data() + sizeof(ds::table_header),
current_calibration.size() - sizeof(ds::table_header));
current_calibration.size() - sizeof(ds::table_header));

// prepare command
using namespace ds;
auto cmd = _dev->build_command(ds::SET_HKR_CONFIG_TABLE,
static_cast<int>(ds::d500_calib_location::d500_calib_flash_memory),
static_cast<int>(table_header->table_type),
static_cast<int>(ds::d500_calib_type::d500_calib_dynamic), 0,
current_calibration.data(), current_calibration.size());
static_cast<int>(ds::d500_calib_location::d500_calib_flash_memory),
static_cast<int>(table_header->table_type),
static_cast<int>(ds::d500_calib_type::d500_calib_dynamic), 0,
current_calibration.data(), current_calibration.size());

// sending command
_dev->send_receive_raw_data(cmd);
Expand Down
4 changes: 2 additions & 2 deletions src/ds/ds-calib-common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,7 @@ namespace librealsense
uint8_t step_count;
uint8_t accuracy;
uint8_t reserved;
};
} field;

uint32_t as_uint32 = 0;
};
Expand All @@ -144,7 +144,7 @@ namespace librealsense
uint8_t scan_parameter : 1;
uint8_t reserved : 2;
uint8_t data_sampling : 1;
};
} field;

uint32_t as_uint32 = 0;
};
Expand Down

0 comments on commit b962969

Please sign in to comment.