diff --git a/src/ds/d500/CMakeLists.txt b/src/ds/d500/CMakeLists.txt index 253217d7ae6..b36adf1b7d1 100644 --- a/src/ds/d500/CMakeLists.txt +++ b/src/ds/d500/CMakeLists.txt @@ -11,6 +11,7 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/hw_monitor_extended_buffers.cpp" "${CMAKE_CURRENT_LIST_DIR}/d500-options.cpp" "${CMAKE_CURRENT_LIST_DIR}/d500-fw-update-device.cpp" + "${CMAKE_CURRENT_LIST_DIR}/d500-auto-calibration.cpp" "${CMAKE_CURRENT_LIST_DIR}/d500-device.h" "${CMAKE_CURRENT_LIST_DIR}/d500-color.h" "${CMAKE_CURRENT_LIST_DIR}/d500-motion.h" @@ -20,4 +21,5 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/d500-options.h" "${CMAKE_CURRENT_LIST_DIR}/d500-info.h" "${CMAKE_CURRENT_LIST_DIR}/d500-fw-update-device.h" + "${CMAKE_CURRENT_LIST_DIR}/d500-auto-calibration.h" ) diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp new file mode 100644 index 00000000000..905d5a5a0d0 --- /dev/null +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -0,0 +1,280 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + + +#include "d500-auto-calibration.h" +#include "d500-private.h" +#include + + +namespace librealsense +{ + static const std::string d500_calibration_state_strings[] = { + "Idle", + "In Process", + "Done Success", + "Done Failure", + "Flash Update", + "Complete" + }; + + static const std::string d500_calibration_result_strings[] = { + "Unkown", + "Success", + "Failed to Converge", + "Failed to Run" + }; + +#pragma pack(push, 1) + struct d500_calibration_answer + { + uint8_t calibration_state; + int8_t calibration_progress; + uint8_t calibration_result; + ds::d500_coefficients_table depth_calibration; + }; +#pragma pack(pop) + + + d500_auto_calibrated::d500_auto_calibrated() : + _mode (d500_calibration_mode::RS2_D500_CALIBRATION_MODE_RESERVED), + _state (d500_calibration_state::RS2_D500_CALIBRATION_STATE_IDLE), + _result(d500_calibration_result::RS2_D500_CALIBRATION_RESULT_UNKNOWN) + {} + + bool d500_auto_calibrated::check_buffer_size_from_get_calib_status(std::vector res) const + { + // the GET_CALIB_STATUS command will return: + // - 3 bytes during the whole process + // - 515 bytes (3 bytes + 512 bytes of the depth calibration) when the state is Complete + + bool is_size_ok = false; + if (res.size() > 1) + { + if (res[0] < static_cast(d500_calibration_state::RS2_D500_CALIBRATION_STATE_COMPLETE) && + res.size() == (sizeof(d500_calibration_answer) - sizeof(ds::d500_coefficients_table))) + is_size_ok = true; + + if (res[0] == static_cast(d500_calibration_state::RS2_D500_CALIBRATION_STATE_COMPLETE) && + res.size() == sizeof(d500_calibration_answer)) + is_size_ok = true; + } + return is_size_ok; + } + + void d500_auto_calibrated::check_preconditions_and_set_state() + { + // moreover, relevant only for d585s, not for d555e + if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_RUN || + _mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_DRY_RUN) + { + // calibration state to be IDLE or COMPLETE + auto res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + + // checking size of received buffer + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + d500_calibration_answer calib_result = *reinterpret_cast(res.data()); + _state = static_cast(calib_result.calibration_state); + if (!(_state == d500_calibration_state::RS2_D500_CALIBRATION_STATE_IDLE || + _state == d500_calibration_state::RS2_D500_CALIBRATION_STATE_COMPLETE)) + { + LOG_ERROR("Calibration State is not Idle nor Complete - pleare restart the device"); + throw std::runtime_error("OCC triggerred when Calibration State is not Idle not Complete"); + } + } + + if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_ABORT) + { + // calibration state to be IN_PROCESS + auto res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + d500_calibration_answer calib_result = *reinterpret_cast(res.data()); + _state = static_cast(calib_result.calibration_state); + if (!(_state == d500_calibration_state::RS2_D500_CALIBRATION_STATE_PROCESS)) + { + LOG_ERROR("Calibration State is not In Process - so it could not be aborted"); + throw std::runtime_error("OCC aborted when Calibration State is not In Process"); + } + } + } + + void d500_auto_calibrated::get_mode_from_json(const std::string& json) + { + _mode = d500_calibration_mode::RS2_D500_CALIBRATION_MODE_RUN; + if (json.find("dry run") != std::string::npos) + _mode = d500_calibration_mode::RS2_D500_CALIBRATION_MODE_DRY_RUN; + else if (json.find("abort") != std::string::npos) + _mode = d500_calibration_mode::RS2_D500_CALIBRATION_MODE_ABORT; + } + + std::vector d500_auto_calibrated::run_on_chip_calibration(int timeout_ms, std::string json, + float* const health, rs2_update_progress_callback_sptr progress_callback) + { + std::vector res; + try + { + get_mode_from_json(json); + + // checking preconditions + check_preconditions_and_set_state(); + + // sending command to start calibration + res = _hw_monitor->send(command{ ds::SET_CALIB_MODE, static_cast(_mode), 1 /*always*/}); + + if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_RUN || + _mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_DRY_RUN) + { + res = update_calibration_status(timeout_ms, progress_callback); + } + else if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_ABORT) + { + res = update_abort_status(); + } + } + catch(...) + { + std::string error_message_prefix = "\nRUN OCC "; + if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_DRY_RUN) + error_message_prefix = "\nDRY RUN OCC "; + else if (_mode == d500_calibration_mode::RS2_D500_CALIBRATION_MODE_ABORT) + error_message_prefix = "\nABORT OCC "; + + throw std::runtime_error(rsutils::string::from() << error_message_prefix + "Could not be triggered"); + } + return res; + } + + std::vector d500_auto_calibrated::update_calibration_status(int timeout_ms, + rs2_update_progress_callback_sptr progress_callback) + { + auto start_time = std::chrono::high_resolution_clock::now(); + std::vector res; + d500_calibration_answer calib_answer; + do + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + calib_answer = *reinterpret_cast(res.data()); + _state = static_cast(calib_answer.calibration_state); + _result = static_cast(calib_answer.calibration_result); + LOG_INFO("Calibration in progress - State = " << d500_calibration_state_strings[static_cast(_state)] + << ", progress = " << static_cast(calib_answer.calibration_progress) + << ", result = " << d500_calibration_result_strings[static_cast(_result)]); + if (progress_callback) + { + progress_callback->on_update_progress(calib_answer.calibration_progress); + } + bool is_timed_out(std::chrono::high_resolution_clock::now() - start_time > std::chrono::milliseconds(timeout_ms)); + if (is_timed_out) + { + throw std::runtime_error("OCC Calibration Timeout"); + } + } while (_state != d500_calibration_state::RS2_D500_CALIBRATION_STATE_COMPLETE); + + // printing new calibration to log + if (_result == d500_calibration_result::RS2_D500_CALIBRATION_RESULT_SUCCESS) + { + auto depth_calib = *reinterpret_cast(&calib_answer.depth_calibration); + LOG_INFO("Depth new Calibration = \n" + depth_calib.to_string()); + } + else + { + LOG_ERROR("Calibration completed but algorithm failed"); + throw std::runtime_error("Calibration completed but algorithm failed"); + } + + return res; + } + + std::vector d500_auto_calibrated::update_abort_status() + { + std::vector ans; + auto res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + d500_calibration_answer calib_answer = *reinterpret_cast(res.data()); + if (calib_answer.calibration_state == static_cast(d500_calibration_state::RS2_D500_CALIBRATION_STATE_PROCESS)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + res = _hw_monitor->send(command{ ds::GET_CALIB_STATUS }); + if (!check_buffer_size_from_get_calib_status(res)) + throw std::runtime_error("GET_CALIB_STATUS returned struct with wrong size"); + + calib_answer = *reinterpret_cast(res.data()); + } + if (calib_answer.calibration_state == static_cast(d500_calibration_state::RS2_D500_CALIBRATION_STATE_IDLE)) + { + LOG_INFO("Depth Calibration Successfully Aborted"); + // returning success + ans.push_back(1); + } + else + { + LOG_INFO("Depth Calibration Could not be Aborted"); + // returning failure + ans.push_back(0); + } + return ans; + } + + std::vector d500_auto_calibrated::run_tare_calibration(int timeout_ms, float ground_truth_mm, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "Tare Calibration not applicable for this device"); + } + + std::vector d500_auto_calibrated::process_calibration_frame(int timeout_ms, const rs2_frame* f, float* const health, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "Process Calibration Frame not applicable for this device"); + } + + std::vector d500_auto_calibrated::get_calibration_table() const + { + throw std::runtime_error(rsutils::string::from() << "Get Calibration Table not applicable for this device"); + } + + void d500_auto_calibrated::write_calibration() const + { + throw std::runtime_error(rsutils::string::from() << "Write Calibration not applicable for this device"); + } + + void d500_auto_calibrated::set_calibration_table(const std::vector& calibration) + { + throw std::runtime_error(rsutils::string::from() << "Set Calibration Table not applicable for this device"); + } + + void d500_auto_calibrated::reset_to_factory_calibration() const + { + throw std::runtime_error(rsutils::string::from() << "Tare Calibration not applicable for this device"); + } + + std::vector d500_auto_calibrated::run_focal_length_calibration(rs2_frame_queue* left, rs2_frame_queue* right, float target_w, float target_h, + int adjust_both_sides, float *ratio, float * angle, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "Focal Length Calibration not applicable for this device"); + } + + std::vector d500_auto_calibrated::run_uv_map_calibration(rs2_frame_queue* left, rs2_frame_queue* color, rs2_frame_queue* depth, int py_px_only, + float* const health, int health_size, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "UV Map Calibration not applicable for this device"); + } + + float d500_auto_calibrated::calculate_target_z(rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, + float target_w, float target_h, rs2_update_progress_callback_sptr progress_callback) + { + throw std::runtime_error(rsutils::string::from() << "Calculate T not applicable for this device"); + } + + void d500_auto_calibrated::set_hw_monitor_for_auto_calib(std::shared_ptr hwm) + { + _hw_monitor = hwm; + } +} diff --git a/src/ds/d500/d500-auto-calibration.h b/src/ds/d500/d500-auto-calibration.h new file mode 100644 index 00000000000..93fd016a093 --- /dev/null +++ b/src/ds/d500/d500-auto-calibration.h @@ -0,0 +1,73 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "auto-calibrated-device.h" +#include "../../core/advanced_mode.h" + + +namespace librealsense +{ +#pragma pack(push, 1) +#pragma pack(1) + +#pragma pack(pop) + + class d500_auto_calibrated : public auto_calibrated_interface + { + public: + d500_auto_calibrated(); + void write_calibration() const override; + std::vector run_on_chip_calibration(int timeout_ms, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) override; + std::vector run_tare_calibration(int timeout_ms, float ground_truth_mm, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback) override; + std::vector process_calibration_frame(int timeout_ms, const rs2_frame* f, float* const health, rs2_update_progress_callback_sptr progress_callback) override; + std::vector get_calibration_table() const override; + void set_calibration_table(const std::vector& calibration) override; + void reset_to_factory_calibration() const override; + std::vector run_focal_length_calibration(rs2_frame_queue* left, rs2_frame_queue* right, float target_w, float target_h, + int adjust_both_sides, float* ratio, float* angle, rs2_update_progress_callback_sptr progress_callback) override; + std::vector run_uv_map_calibration(rs2_frame_queue* left, rs2_frame_queue* color, rs2_frame_queue* depth, int py_px_only, + float* const health, int health_size, rs2_update_progress_callback_sptr progress_callback) override; + float calculate_target_z(rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, + float target_width, float target_height, rs2_update_progress_callback_sptr progress_callback) override; + void set_hw_monitor_for_auto_calib(std::shared_ptr hwm); + + enum class d500_calibration_state + { + RS2_D500_CALIBRATION_STATE_IDLE = 0, + RS2_D500_CALIBRATION_STATE_PROCESS, + RS2_D500_CALIBRATION_STATE_DONE_SUCCESS, + RS2_D500_CALIBRATION_STATE_DONE_FAILURE, + RS2_D500_CALIBRATION_STATE_FLASH_UPDATE, + RS2_D500_CALIBRATION_STATE_COMPLETE + }; + + enum class d500_calibration_result + { + RS2_D500_CALIBRATION_RESULT_UNKNOWN = 0, + RS2_D500_CALIBRATION_RESULT_SUCCESS, + RS2_D500_CALIBRATION_RESULT_FAILED_TO_CONVERGE, + RS2_D500_CALIBRATION_RESULT_FAILED_TO_RUN + }; + + private: + enum class d500_calibration_mode + { + RS2_D500_CALIBRATION_MODE_RESERVED = 0, + RS2_D500_CALIBRATION_MODE_RUN, + RS2_D500_CALIBRATION_MODE_ABORT, + RS2_D500_CALIBRATION_MODE_DRY_RUN + }; + + void check_preconditions_and_set_state(); + bool check_buffer_size_from_get_calib_status(std::vector res) const; + void get_mode_from_json(const std::string& json); + std::vector update_calibration_status(int timeout_ms, rs2_update_progress_callback_sptr progress_callback); + std::vector update_abort_status(); + std::shared_ptr _hw_monitor; + d500_calibration_mode _mode; + d500_calibration_state _state; + d500_calibration_result _result; + }; +}