00001
00002
00003
00004 #pragma once
00005 #ifndef LIBREALSENSE_IV_PRIVATE_H
00006 #define LIBREALSENSE_IV_PRIVATE_H
00007
00008 #include "uvc.h"
00009 #include <mutex>
00010
00011 namespace rsimpl {
00012 namespace ivcam {
00013
00014 const uvc::extension_unit depth_xu{ 1, 6, 1, { 0xA55751A1, 0xF3C5, 0x4A5E, { 0x8D, 0x5A, 0x68, 0x54, 0xB8, 0xFA, 0x27, 0x16 } } };
00015
00016 struct camera_calib_params
00017 {
00018 float Rmax;
00019 float Kc[3][3];
00020 float Distc[5];
00021 float Invdistc[5];
00022 float Pp[3][4];
00023 float Kp[3][3];
00024 float Rp[3][3];
00025 float Tp[3];
00026 float Distp[5];
00027 float Invdistp[5];
00028 float Pt[3][4];
00029 float Kt[3][3];
00030 float Rt[3][3];
00031 float Tt[3];
00032 float Distt[5];
00033 float Invdistt[5];
00034 float QV[6];
00035 };
00036
00037 struct cam_calibration
00038 {
00039 int uniqueNumber;
00040 int16_t TableValidation;
00041 int16_t TableVersion;
00042 ivcam::camera_calib_params CalibrationParameters;
00043 };
00044
00045 struct cam_auto_range_request
00046 {
00047 int enableMvR;
00048 int enableLaser;
00049 int16_t minMvR;
00050 int16_t maxMvR;
00051 int16_t startMvR;
00052 int16_t minLaser;
00053 int16_t maxLaser;
00054 int16_t startLaser;
00055 uint16_t ARUpperTh;
00056 uint16_t ARLowerTh;
00057 };
00058
00059 enum class fw_cmd : uint8_t
00060 {
00061 GetMEMSTemp = 0x0A,
00062 DebugFormat = 0x0B,
00063 TimeStampEnable = 0x0C,
00064 GetFWLastError = 0x0E,
00065 HWReset = 0x28,
00066 GVD = 0x3B,
00067 GetCalibrationTable = 0x3D,
00068 CheckI2cConnect = 0x4A,
00069 CheckRGBConnect = 0x4B,
00070 CheckDPTConnect = 0x4C,
00071 GetIRTemp = 0x52,
00072 GoToDFU = 0x80,
00073 OnSuspendResume = 0x91,
00074 GetWakeReason = 0x93,
00075 GetWakeConfidence = 0x92,
00076 SetAutoRange = 0xA6,
00077 SetDefaultControls = 0xA6,
00078 GetDefaultControls = 0xA7,
00079 AutoRangeSetParamsforDebug = 0xb3,
00080 UpdateCalib = 0xBC,
00081 BIST = 0xFF,
00082 GetPowerGearState = 0xFF
00083 };
00084
00085 enum class FirmwareError : int32_t
00086 {
00087 FW_ACTIVE = 0,
00088 FW_MSAFE_S1_ERR,
00089 FW_I2C_SAFE_ERR,
00090 FW_FLASH_SAFE_ERR,
00091 FW_I2C_CFG_ERR,
00092 FW_I2C_EV_ERR,
00093 FW_HUMIDITY_ERR,
00094 FW_MSAFE_S0_ERR,
00095 FW_LD_ERR,
00096 FW_PI_ERR,
00097 FW_PJCLK_ERR,
00098 FW_OAC_ERR,
00099 FW_LIGURIA_TEMPERATURE_ERR,
00100 FW_CONTINUE_SAFE_ERROR,
00101 FW_FORZA_HUNG,
00102 FW_FORZA_CONTINUES_HUNG,
00103 FW_PJ_EYESAFETY_CHKRHARD,
00104 FW_MIPI_PCAM_ERR,
00105 FW_MIPI_TCAM_ERR,
00106 FW_SYNC_DISABLED,
00107 FW_MIPI_PCAM_SVR_ERR,
00108 FW_MIPI_TCAM_SVR_ERR,
00109 FW_MIPI_PCAM_FRAME_SIZE_ERR,
00110 FW_MIPI_TCAM_FRAME_SIZE_ERR,
00111 FW_MIPI_PCAM_FRAME_RESPONSE_ERR,
00112 FW_MIPI_TCAM_FRAME_RESPONSE_ERR,
00113 FW_USB_PCAM_THROTTLED_ERR,
00114 FW_USB_TCAM_THROTTLED_ERR,
00115 FW_USB_PCAM_QOS_WAR,
00116 FW_USB_TCAM_QOS_WAR,
00117 FW_USB_PCAM_OVERFLOW,
00118 FW_USB_TCAM_OVERFLOW,
00119 FW_Flash_OEM_SECTOR,
00120 FW_Flash_CALIBRATION_RW,
00121 FW_Flash_IR_CALIBRATION,
00122 FW_Flash_RGB_CALIBRATION,
00123 FW_Flash_THERMAL_LOOP_CONFIGURATION,
00124 FW_Flash_REALTEK,
00125 FW_RGB_ISP_BOOT_FAILED,
00126 FW_PRIVACY_RGB_OFF,
00127 FW_PRIVACY_DEPTH_OFF,
00128 FW_COUNT_ERROR
00129 };
00130
00131
00132 void claim_ivcam_interface(uvc::device & device);
00133
00134
00135 size_t prepare_usb_command(uint8_t * request, size_t & requestSize, uint32_t op, uint32_t p1 = 0, uint32_t p2 = 0, uint32_t p3 = 0, uint32_t p4 = 0, uint8_t * data = 0, size_t dataLength = 0);
00136 void get_gvd(uvc::device & device, std::timed_mutex & mutex, size_t sz, char * gvd, int gvd_cmd = (int)fw_cmd::GVD);
00137 void get_firmware_version_string(uvc::device & device, std::timed_mutex & mutex, std::string & version, int gvd_cmd = (int)fw_cmd::GVD, int offset = 0);
00138 void get_module_serial_string(uvc::device & device, std::timed_mutex & mutex, std::string & serial, int offset);
00139
00140
00141 void force_hardware_reset(uvc::device & device, std::timed_mutex & mutex);
00142 void enable_timestamp(uvc::device & device, std::timed_mutex & mutex, bool colorEnable, bool depthEnable);
00143 void set_auto_range(uvc::device & device, std::timed_mutex & mutex, int enableMvR, int16_t minMvR, int16_t maxMvR, int16_t startMvR, int enableLaser, int16_t minLaser, int16_t maxLaser, int16_t startLaser, int16_t ARUpperTH, int16_t ARLowerTH);
00144
00145
00146 void get_laser_power(const uvc::device & device, uint8_t & laser_power);
00147 void set_laser_power(uvc::device & device, uint8_t laser_power);
00148 void get_accuracy(const uvc::device & device, uint8_t & accuracy);
00149 void set_accuracy(uvc::device & device, uint8_t accuracy);
00150 void get_motion_range(const uvc::device & device, uint8_t & motion_range);
00151 void set_motion_range(uvc::device & device, uint8_t motion_range);
00152 void get_filter_option(const uvc::device & device, uint8_t & filter_option);
00153 void set_filter_option(uvc::device & device, uint8_t filter_option);
00154 void get_confidence_threshold(const uvc::device & device, uint8_t & conf_thresh);
00155 void set_confidence_threshold(uvc::device & device, uint8_t conf_thresh);
00156
00157 }
00158
00159 namespace f200 {
00160
00161 struct cam_temperature_data
00162 {
00163 float LiguriaTemp;
00164 float IRTemp;
00165 float AmbientTemp;
00166 };
00167
00168 struct thermal_loop_params
00169 {
00170 float IRThermalLoopEnable = 1;
00171 float TimeOutA = 10000;
00172 float TimeOutB = 0;
00173 float TimeOutC = 0;
00174 float TransitionTemp = 3;
00175 float TempThreshold = 2;
00176 float HFOVsensitivity = 0.025f;
00177 float FcxSlopeA = -0.003696988f;
00178 float FcxSlopeB = 0.005809239f;
00179 float FcxSlopeC = 0;
00180 float FcxOffset = 0;
00181 float UxSlopeA = -0.000210918f;
00182 float UxSlopeB = 0.000034253955f;
00183 float UxSlopeC = 0;
00184 float UxOffset = 0;
00185 float LiguriaTempWeight = 1;
00186 float IrTempWeight = 0;
00187 float AmbientTempWeight = 0;
00188 float Param1 = 0;
00189 float Param2 = 0;
00190 float Param3 = 0;
00191 float Param4 = 0;
00192 float Param5 = 0;
00193 };
00194
00195
00196 std::tuple<ivcam::camera_calib_params, f200::cam_temperature_data, thermal_loop_params> read_f200_calibration(uvc::device & device, std::timed_mutex & mutex);
00197 float read_mems_temp(uvc::device & device, std::timed_mutex & mutex);
00198 int read_ir_temp(uvc::device & device, std::timed_mutex & mutex);
00199
00200
00201 void update_asic_coefficients(uvc::device & device, std::timed_mutex & mutex, const ivcam::camera_calib_params & compensated_params);
00202
00203 void get_dynamic_fps(const uvc::device & device, uint8_t & dynamic_fps);
00204 void set_dynamic_fps(uvc::device & device, uint8_t dynamic_fps);
00205
00206 }
00207
00208 namespace sr300
00209 {
00210
00211 ivcam::camera_calib_params read_sr300_calibration(uvc::device & device, std::timed_mutex & mutex);
00212 }
00213 }
00214
00215 #endif // IV_PRIVATE_H