Skip to content

Commit

Permalink
PR #12161 from remibettan: Cherry pick PR #12128 - UCAL - restore opt…
Browse files Browse the repository at this point in the history 
…ions
  • Loading branch information
@Nir-Az
Nir-Az authored Sep 4, 2023
2 parents 76d2c10 + fba9797 commit 8daa427
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 41 deletions.
74 changes: 42 additions & 32 deletions common/on-chip-calib.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,12 +74,34 @@ namespace rs2
}
}

void on_chip_calib_manager::save_options_controlled_by_calib()
{
// Calibration might fail and be restarted, save only once, or we will keep chaged options.
if( ! _options_saved )
{
save_laser_emitter_state();
save_thermal_loop_state();
_options_saved = true;
}
}

void on_chip_calib_manager::restore_options_controlled_by_calib()
{
// Restore might be called several times, restore only if options where actually saved.
if( _options_saved )
{
restore_laser_emitter_state();
restore_thermal_loop_state();
_options_saved = false;
}
}

void on_chip_calib_manager::save_laser_emitter_state()
{
auto it = _sub->options_metadata.find( RS2_OPTION_EMITTER_ENABLED );
if ( it != _sub->options_metadata.end() ) //Option supported
{
laser_status_prev = _sub->s->get_option( RS2_OPTION_EMITTER_ENABLED );
_laser_status_prev = _sub->s->get_option( RS2_OPTION_EMITTER_ENABLED );
}
}

Expand All @@ -88,18 +110,18 @@ namespace rs2
auto it = _sub->options_metadata.find( RS2_OPTION_THERMAL_COMPENSATION );
if( it != _sub->options_metadata.end() ) // Option supported
{
thermal_loop_prev = _sub->s->get_option( RS2_OPTION_THERMAL_COMPENSATION );
_thermal_loop_prev = _sub->s->get_option( RS2_OPTION_THERMAL_COMPENSATION );
}
}

void on_chip_calib_manager::restore_laser_emitter_state()
{
set_laser_emitter_state( laser_status_prev );
set_laser_emitter_state( _laser_status_prev );
}

void on_chip_calib_manager::restore_thermal_loop_state()
{
set_thermal_loop_state( thermal_loop_prev );
set_thermal_loop_state( _thermal_loop_prev );
}

void on_chip_calib_manager::set_laser_emitter_state( float value )
Expand Down Expand Up @@ -1417,6 +1439,19 @@ namespace rs2

_restored = false;

save_options_controlled_by_calib(); // Restored by GUI thread on dismiss or apply.

// Emitter on by default, off for GT/FL calib and for D415 model
float emitter_value = on_value;
if( action == RS2_CALIB_ACTION_FL_CALIB ||
action == RS2_CALIB_ACTION_TARE_GROUND_TRUTH ||
device_name_string == std::string( "Intel RealSense D415" ) )
emitter_value = off_value;
set_laser_emitter_state( emitter_value );

// Thermal loop should be off during calibration as to not change calibration tables during calibration
set_thermal_loop_state( off_value );

auto fps = 30;
if (_sub->dev.supports(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR))
{
Expand Down Expand Up @@ -1465,40 +1500,18 @@ namespace rs2

if ( action == RS2_CALIB_ACTION_TARE_GROUND_TRUTH )
{
// Laser should be turned off during ground truth calculation
save_laser_emitter_state();
set_laser_emitter_state( off_value );

get_ground_truth();

restore_laser_emitter_state();
}
else
{
try
{
// Save options that are going to change during the calibration
save_laser_emitter_state();
save_thermal_loop_state();

// Emitter on by default, off for FL calib and for D415 model
float emitter_value = on_value;
if( action == RS2_CALIB_ACTION_FL_CALIB || device_name_string == std::string( "Intel RealSense D415" ) )
emitter_value = off_value;
set_laser_emitter_state( emitter_value );

// Thermal loop should be off during calibration as to not change calibration tables during calibration
set_thermal_loop_state( off_value );

if (action == RS2_CALIB_ACTION_FL_CALIB)
calibrate_fl();
else if (action == RS2_CALIB_ACTION_UVMAPPING_CALIB)
calibrate_uv_mapping();
else
calibrate();

restore_laser_emitter_state();
restore_thermal_loop_state();
}
catch (...)
{
Expand All @@ -1515,11 +1528,6 @@ namespace rs2
_ui_color.reset();
}

//Restore options that were changed during the calibration.
//When calibration is successful options are restored in autocalib_notification_model::draw_content()
restore_laser_emitter_state();
restore_thermal_loop_state();

if (_was_streaming)
start_viewer(0, 0, 0, invoke);

Expand Down Expand Up @@ -2861,6 +2869,7 @@ namespace rs2
dismiss(false);
}

get_manager().restore_options_controlled_by_calib();
get_manager().restore_workspace([](std::function<void()> a) { a(); });
}

Expand Down Expand Up @@ -2979,7 +2988,8 @@ namespace rs2
if (!use_new_calib && get_manager().done())
get_manager().apply_calib(false);

get_manager().restore_workspace([](std::function<void()> a){ a(); });
get_manager().restore_options_controlled_by_calib();
get_manager().restore_workspace( []( std::function< void() > a ) { a(); } );

if (update_state != RS2_CALIB_STATE_TARE_INPUT)
update_state = RS2_CALIB_STATE_INITIAL_PROMPT;
Expand Down
21 changes: 12 additions & 9 deletions common/on-chip-calib.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,12 +112,8 @@ namespace rs2
void turn_roi_on();
void turn_roi_off();

void save_laser_emitter_state();
void save_thermal_loop_state();
void restore_laser_emitter_state();
void restore_thermal_loop_state();
void set_laser_emitter_state( float value );
void set_thermal_loop_state( float value );
void save_options_controlled_by_calib();
void restore_options_controlled_by_calib();

void start_gt_viewer();
void start_fl_viewer();
Expand All @@ -127,6 +123,13 @@ namespace rs2
void reset_device() { _dev.hardware_reset(); }

private:
void save_laser_emitter_state();
void save_thermal_loop_state();
void restore_laser_emitter_state();
void restore_thermal_loop_state();
void set_laser_emitter_state( float value );
void set_thermal_loop_state( float value );

std::vector<uint8_t> safe_send_command(const std::vector<uint8_t>& cmd, const std::string& name);
rs2::depth_frame fetch_depth_frame(invoker invoke, int timeout_ms = 5000); // Wait for next depth frame and return it
std::pair<float, float> get_depth_metrics(invoker invoke);
Expand All @@ -141,9 +144,9 @@ namespace rs2

device _dev;


float laser_status_prev = 0.0f;
float thermal_loop_prev = 0.0f;
bool _options_saved = false;
float _laser_status_prev = 0.0f;
float _thermal_loop_prev = 0.0f;

bool _was_streaming = false;
bool _synchronized = false;
Expand Down

0 comments on commit 8daa427

Please sign in to comment.