00001
00002
00003
00004 #pragma once
00005 #ifndef LIBREALSENSE_DS_PRIVATE_H
00006 #define LIBREALSENSE_DS_PRIVATE_H
00007
00008 #include "uvc.h"
00009 #include <algorithm>
00010 #include <ctime>
00011 #include <cmath>
00012
00013 namespace rsimpl
00014 {
00015 namespace ds
00016 {
00017 const uvc::extension_unit lr_xu = {0, 2, 1, {0x18682d34, 0xdd2c, 0x4073, {0xad, 0x23, 0x72, 0x14, 0x73, 0x9a, 0x07, 0x4c}}};
00018
00019 const int STATUS_BIT_Z_STREAMING = 1 << 0;
00020 const int STATUS_BIT_LR_STREAMING = 1 << 1;
00021 const int STATUS_BIT_WEB_STREAMING = 1 << 2;
00022
00023 struct ds_calibration
00024 {
00025 int version;
00026 uint32_t serial_number;
00027 rs_intrinsics modesLR[3];
00028 rs_intrinsics intrinsicsThird[2];
00029 rs_intrinsics modesThird[2][2];
00030 float Rthird[9], T[3], B;
00031 };
00032
00033 enum ds_lens_type : uint32_t
00034 {
00035 DS_LENS_UNKNOWN = 0,
00036 DS_LENS_DSL103 = 1,
00037 DS_LENS_DSL821C = 2,
00038 DS_LENS_DSL202A = 3,
00039 DS_LENS_DSL203 = 4,
00040 DS_LENS_PENTAX2514 = 5,
00041 DS_LENS_DSL924A = 6,
00042 DS_LENS_AZW58 = 7,
00043 DS_LENS_Largan9386 = 8,
00044 DS_LENS_DS6100 = 9,
00045 DS_LENS_DS6177 = 10,
00046 DS_LENS_DS6237 = 11,
00047 DS_LENS_DS6233 = 12,
00048 DS_LENS_DS917 = 13,
00049 DS_LENS_AEOT_1LS0901L = 14,
00050 DS_LENS_COUNT = 15
00051 };
00052
00053
00054 inline std::ostream & operator<<(std::ostream & out, ds_lens_type type)
00055 {
00056 switch (type)
00057 {
00058 case DS_LENS_UNKNOWN: return out << "Unknown lens type";
00059 case DS_LENS_DSL103: return out << "Sunex DSL103: Internal standard";
00060 case DS_LENS_DSL821C: return out << "Sunex DSL 821C";
00061 case DS_LENS_DSL202A: return out << "Sunex DSL 202A";
00062 case DS_LENS_DSL203: return out << "Sunex DSL 203";
00063 case DS_LENS_PENTAX2514: return out << "Pentax cmount lens 25mm";
00064 case DS_LENS_DSL924A: return out << "Sunex DSL 924a";
00065 case DS_LENS_AZW58: return out << "58 degree lenses on the AZureWave boards (DS-526)";
00066 case DS_LENS_Largan9386: return out << "50 HFOV 38 VFOV (60DFOV): CTM2/6 Module L&R";
00067 case DS_LENS_DS6100: return out << "Newmax 67.8 x 41.4 degs in 1080p";
00068 case DS_LENS_DS6177: return out << "Newmax 71.7 x 44.2 degs in 1080p";
00069 case DS_LENS_DS6237: return out << "Newmax 58.9 x 45.9 degs in VGA";
00070 case DS_LENS_AEOT_1LS0901L: return out << "AEOT";
00071 default: return out << "Other lens type (" << (int)type << "), application needs update";
00072 }
00073 }
00074
00075 enum ds_lens_coating_type : uint32_t
00076 {
00077 DS_LENS_COATING_UNKNOWN = 0,
00078 DS_LENS_COATING_IR_CUT = 1,
00079 DS_LENS_COATING_ALL_PASS = 2,
00080 DS_LENS_COATING_IR_PASS = 3,
00081 DS_LENS_COATING_IR_PASS_859_43 = 4,
00082 DS_LENS_COATING_COUNT = 5
00083 };
00084
00085
00086 inline std::ostream & operator<<(std::ostream & out, ds_lens_coating_type type)
00087 {
00088 switch (type)
00089 {
00090 case DS_LENS_COATING_UNKNOWN: return out << "Unknown lens coating type";
00091 case DS_LENS_COATING_IR_CUT: return out << "IR coating";
00092 case DS_LENS_COATING_ALL_PASS: return out << "No IR coating";
00093 case DS_LENS_COATING_IR_PASS: return out << "Visible-light block / IR pass";
00094 case DS_LENS_COATING_IR_PASS_859_43: return out << "Visible-light block / IR pass 43 nm width";
00095 default: return out << "Other lens coating type (" << (int)type << "), application needs update";
00096 }
00097 }
00098
00099 enum ds_emitter_type : uint32_t
00100 {
00101 DS_EMITTER_NONE = 0,
00102 DS_EMITTER_LD2 = 1,
00103 DS_EMITTER_LD3 = 2,
00104 DS_EMITTER_LD4_1 = 3,
00105 DS_EMITTER_COUNT = 4
00106 };
00107
00108 inline std::ostream & operator<<(std::ostream & out, ds_emitter_type type)
00109 {
00110 switch (type)
00111 {
00112 case DS_EMITTER_NONE: return out << "No emitter";
00113 case DS_EMITTER_LD2: return out << "Laser Driver 2, NO PWM Controls";
00114 case DS_EMITTER_LD3: return out << "Laser Driver 3";
00115 case DS_EMITTER_LD4_1: return out << "Laser Driver 4.1";
00116 default:
00117 return out << "Other emitter type (" << (int)type << "), application needs update";
00118 }
00119 }
00120
00121 enum ds_oem_id : uint32_t
00122 {
00123 DS_OEM_NONE = 0
00124 };
00125
00126 inline std::ostream & operator<<(std::ostream & out, ds_oem_id type)
00127 {
00128 switch (type)
00129 {
00130 case DS_OEM_NONE: return out << "OEM None";
00131 default:
00132 return out << "Unercognized OEM type (" << (int)type << "), application needs update";
00133 }
00134 }
00135
00136 enum ds_prq_type : uint8_t
00137 {
00138 DS_PRQ_READY= 1
00139 };
00140
00141 inline std::ostream & operator<<(std::ostream & out, ds_prq_type type)
00142 {
00143 switch (type)
00144 {
00145 case DS_PRQ_READY: return out << "PRQ-Ready";
00146 default:
00147 return out << "Non-PRQ type (" << (int)type << ")";
00148 }
00149 }
00150
00151 inline std::string time_to_string(double val)
00152 {
00153 std::string date("Undefined value");
00154
00155
00156 if (std::isnormal(val) && std::isfinite(val) && (!std::isnan(val)) )
00157 {
00158 auto time = time_t(val);
00159 std::vector<char> outstr;
00160 outstr.resize(200);
00161 strftime(outstr.data(),outstr.size(),"%Y-%m-%d %H:%M:%S",std::gmtime(&time));
00162 date = to_string()<< outstr.data() << " UTC";
00163 }
00164 return date;
00165 }
00166
00167 #pragma pack(push,1)
00168
00169 struct ds_head_content
00170 {
00171 enum { DS_HEADER_VERSION_NUMBER = 12 };
00172 uint32_t serial_number;
00173 uint32_t imager_model_number;
00174 uint32_t module_revision_number;
00175 uint8_t model_data[64];
00176 double build_date;
00177 double first_program_date;
00178 double focus_alignment_date;
00179 int32_t nominal_baseline_third_imager;
00180 uint8_t module_version;
00181 uint8_t module_major_version;
00182 uint8_t module_minor_version;
00183 uint8_t module_skew_version;
00184 ds_lens_type lens_type_third_imager;
00185 ds_oem_id oem_id;
00186 ds_lens_coating_type lens_coating_type_third_imager;
00187 uint8_t platform_camera_support;
00188 ds_prq_type prq_type;
00189 uint8_t reserved1[2];
00190 ds_emitter_type emitter_type;
00191 uint8_t reserved2[4];
00192 uint32_t camera_fpga_version;
00193 uint32_t platform_camera_focus;
00194 double calibration_date;
00195 uint32_t calibration_type;
00196 double calibration_x_error;
00197 double calibration_y_error;
00198 double rectification_data_qres[54];
00199 double rectification_data_padding[26];
00200 double cx_qres;
00201 double cy_qres;
00202 double cz_qres;
00203 double kx_qres;
00204 double ky_qres;
00205 uint32_t camera_head_contents_version;
00206 uint32_t camera_head_contents_size_bytes;
00207 double cx_big;
00208 double cy_big;
00209 double cz_big;
00210 double kx_big;
00211 double ky_big;
00212 double cx_special;
00213 double cy_special;
00214 double cz_special;
00215 double kx_special;
00216 double ky_special;
00217 uint8_t camera_head_data_little_endian;
00218 double rectification_data_big[54];
00219 double rectification_data_special[54];
00220 uint8_t camera_options_1;
00221 uint8_t camera_options_2;
00222 uint8_t body_serial_number[20];
00223 double dx;
00224 double dy;
00225 double dz;
00226 double theta_x;
00227 double theta_y;
00228 double theta_z;
00229 double registration_date;
00230 double registration_rotation[9];
00231 double registration_translation[3];
00232 uint32_t nominal_baseline;
00233 ds_lens_type lens_type;
00234 ds_lens_coating_type lens_coating_type;
00235 int32_t nominal_baseline_platform[3];
00236 uint32_t lens_type_platform;
00237 uint32_t imager_type_platform;
00238 uint32_t the_last_word;
00239 uint8_t reserved3[37];
00240 };
00241
00242 #pragma pack(pop)
00243
00244 struct ds_info
00245 {
00246 ds_head_content head_content;
00247 ds_calibration calibration;
00248 };
00249
00250
00251 ds_info read_camera_info(uvc::device & device);
00252 std::string read_firmware_version(uvc::device & device);
00253 std::string read_isp_firmware_version(uvc::device & device);
00254
00258
00259 enum class control
00260 {
00261 command_response = 1,
00262 fisheye_xu_strobe = 1,
00263 iffley = 2,
00264 fisheye_xu_ext_trig = 2,
00265 stream_intent = 3,
00266 fisheye_exposure = 3,
00267 depth_units = 4,
00268 min_max = 5,
00269 disparity = 6,
00270 rectification = 7,
00271 emitter = 8,
00272 temperature = 9,
00273 depth_params = 10,
00274 last_error = 12,
00275 embedded_count = 13,
00276 lr_exposure = 14,
00277 lr_autoexposure_parameters = 15,
00278 sw_reset = 16,
00279 lr_gain = 17,
00280 lr_exposure_mode = 18,
00281 disparity_shift = 19,
00282 status = 20,
00283 lr_exposure_discovery = 21,
00284 lr_gain_discovery = 22,
00285 hw_timestamp = 23,
00286 };
00287
00288 void xu_read(const uvc::device & device, uvc::extension_unit xu, control xu_ctrl, void * buffer, uint32_t length);
00289 void xu_write(uvc::device & device, uvc::extension_unit xu, control xu_ctrl, void * buffer, uint32_t length);
00290
00291 template<class T> T xu_read(const uvc::device & dev, uvc::extension_unit xu, control ctrl) { T val; xu_read(dev, xu, ctrl, &val, sizeof(val)); return val; }
00292 template<class T> void xu_write(uvc::device & dev, uvc::extension_unit xu, control ctrl, const T & value) { T val = value; xu_write(dev, xu, ctrl, &val, sizeof(val)); }
00293
00294 #pragma pack(push, 1)
00295 struct ae_params
00296 {
00297 float mean_intensity_set_point;
00298 float bright_ratio_set_point;
00299 float kp_gain;
00300 float kp_exposure;
00301 float kp_dark_threshold;
00302 uint16_t exposure_top_edge;
00303 uint16_t exposure_bottom_edge;
00304 uint16_t exposure_left_edge;
00305 uint16_t exposure_right_edge;
00306 };
00307
00308 struct dc_params
00309 {
00310 uint32_t robbins_munroe_minus_inc;
00311 uint32_t robbins_munroe_plus_inc;
00312 uint32_t median_thresh;
00313 uint32_t score_min_thresh;
00314 uint32_t score_max_thresh;
00315 uint32_t texture_count_thresh;
00316 uint32_t texture_diff_thresh;
00317 uint32_t second_peak_thresh;
00318 uint32_t neighbor_thresh;
00319 uint32_t lr_thresh;
00320
00321 enum { MAX_PRESETS = 6 };
00322 static const dc_params presets[MAX_PRESETS];
00323 };
00324
00325 class time_pad
00326 {
00327 std::chrono::high_resolution_clock::duration _duration;
00328 std::chrono::high_resolution_clock::time_point _start_time;
00329
00330 public:
00331 time_pad(std::chrono::high_resolution_clock::duration duration) : _duration(duration) {}
00332 void start()
00333 {
00334 _start_time = std::chrono::high_resolution_clock::now();
00335 }
00336
00337 void stop()
00338 {
00339 auto elapsed = std::chrono::high_resolution_clock::now() - _start_time;
00340 if (elapsed < _duration)
00341 {
00342 auto left = _duration - elapsed;
00343 std::this_thread::sleep_for(left);
00344 }
00345 }
00346 };
00347
00348 struct range { uint16_t min, max; };
00349 struct disp_mode { uint32_t is_disparity_enabled; double disparity_multiplier; };
00350 struct rate_value { uint32_t rate, value; };
00351 struct temperature { int8_t current, min, max, min_fault; };
00352 struct discovery { uint32_t fps, min, max, default_value, resolution; };
00353 #pragma pack(pop)
00354
00355 void set_stream_intent(uvc::device & device, uint8_t & intent);
00356 void get_stream_status(const uvc::device & device, uint32_t & status);
00357 void force_firmware_reset(uvc::device & device);
00358 bool get_emitter_state(const uvc::device & device, bool is_streaming, bool is_depth_enabled);
00359 void set_emitter_state(uvc::device & device, bool state);
00360
00361 void get_register_value(uvc::device & device, uint32_t reg, uint32_t & value);
00362 void set_register_value(uvc::device & device, uint32_t reg, uint32_t value);
00363
00364
00365 inline uint32_t get_depth_units (const uvc::device & device) { return xu_read<uint32_t >(device, lr_xu, control::depth_units); }
00366 inline range get_min_max_depth (const uvc::device & device) { return xu_read<range >(device, lr_xu, control::min_max); }
00367 inline disp_mode get_disparity_mode (const uvc::device & device) { return xu_read<disp_mode >(device, lr_xu, control::disparity); }
00368 inline temperature get_temperature (const uvc::device & device) { return xu_read<temperature>(device, lr_xu, control::temperature); }
00369 inline dc_params get_depth_params (const uvc::device & device) { return xu_read<dc_params >(device, lr_xu, control::depth_params); }
00370 inline uint8_t get_last_error (const uvc::device & device) { return xu_read<uint8_t >(device, lr_xu, control::last_error); }
00371 inline rate_value get_lr_exposure (const uvc::device & device) { return xu_read<rate_value >(device, lr_xu, control::lr_exposure); }
00372 inline ae_params get_lr_auto_exposure_params(const uvc::device & device, std::vector<supported_option> ae_vec) {
00373 auto ret_val = xu_read<ae_params >(device, lr_xu, control::lr_autoexposure_parameters);
00374
00375 for (auto& elem : ae_vec)
00376 {
00377 switch (elem.option)
00378 {
00379 case RS_OPTION_R200_AUTO_EXPOSURE_TOP_EDGE:
00380 ret_val.exposure_top_edge = std::min((uint16_t)elem.max, ret_val.exposure_top_edge);
00381 break;
00382
00383 case RS_OPTION_R200_AUTO_EXPOSURE_BOTTOM_EDGE:
00384 ret_val.exposure_bottom_edge = std::min((uint16_t)elem.max, ret_val.exposure_bottom_edge);
00385 break;
00386
00387 case RS_OPTION_R200_AUTO_EXPOSURE_LEFT_EDGE:
00388 ret_val.exposure_left_edge = std::min((uint16_t)elem.max, ret_val.exposure_left_edge);
00389 break;
00390
00391 case RS_OPTION_R200_AUTO_EXPOSURE_RIGHT_EDGE:
00392 ret_val.exposure_right_edge = std::min((uint16_t)elem.max, ret_val.exposure_right_edge);
00393 break;
00394 default :
00395 break;
00396 }
00397 }
00398 return ret_val;
00399 }
00400 inline rate_value get_lr_gain (const uvc::device & device) { return xu_read<rate_value >(device, lr_xu, control::lr_gain); }
00401 inline uint8_t get_lr_exposure_mode (const uvc::device & device) { return xu_read<uint8_t >(device, lr_xu, control::lr_exposure_mode); }
00402 inline uint32_t get_disparity_shift (const uvc::device & device) { return xu_read<uint32_t >(device, lr_xu, control::disparity_shift); }
00403 inline discovery get_lr_exposure_discovery (const uvc::device & device) { return xu_read<discovery >(device, lr_xu, control::lr_exposure_discovery); }
00404 inline discovery get_lr_gain_discovery (const uvc::device & device) { return xu_read<discovery >(device, lr_xu, control::lr_gain_discovery); }
00405
00406 inline void set_depth_units (uvc::device & device, uint32_t units) { xu_write(device, lr_xu, control::depth_units, units); }
00407 inline void set_min_max_depth (uvc::device & device, range min_max) { xu_write(device, lr_xu, control::min_max, min_max); }
00408 inline void set_disparity_mode (uvc::device & device, disp_mode mode) { xu_write(device, lr_xu, control::disparity, mode); }
00409 inline void set_temperature (uvc::device & device, temperature temp) { xu_write(device, lr_xu, control::temperature, temp); }
00410 inline void set_depth_params (uvc::device & device, dc_params params) { xu_write(device, lr_xu, control::depth_params, params); }
00411 inline void set_lr_auto_exposure_params (uvc::device & device, ae_params params) {
00412 if (params.exposure_top_edge >= params.exposure_bottom_edge || params.exposure_left_edge >= params.exposure_right_edge)
00413 throw std::logic_error("set_lr_auto_exposure_params failed.");
00414 xu_write(device, lr_xu, control::lr_autoexposure_parameters, params); }
00415 inline void set_lr_exposure_mode (uvc::device & device, uint8_t mode) { xu_write(device, lr_xu, control::lr_exposure_mode, mode); }
00416 inline void set_disparity_shift (uvc::device & device, uint32_t shift) { xu_write(device, lr_xu, control::disparity_shift, shift); }
00417 inline void set_lr_exposure_discovery (uvc::device & device, discovery disc) { xu_write(device, lr_xu, control::lr_exposure_discovery, disc); }
00418 inline void set_lr_gain_discovery (uvc::device & device, discovery disc) { xu_write(device, lr_xu, control::lr_gain_discovery, disc); }
00419 inline void set_lr_exposure (uvc::device & device, rate_value exposure) { if (get_lr_exposure_mode(device)) set_lr_exposure_mode(device,0); xu_write(device, lr_xu, control::lr_exposure, exposure);}
00420 inline void set_lr_gain (uvc::device & device, rate_value gain) { if (get_lr_exposure_mode(device)) set_lr_exposure_mode(device,0); xu_write(device, lr_xu, control::lr_gain, gain);}
00421
00422
00423 #pragma pack(push, 1)
00424 struct dinghy
00425 {
00426 uint32_t magicNumber;
00427 uint32_t frameCount;
00428 uint32_t frameStatus;
00429 uint32_t exposureLeftSum;
00430 uint32_t exposureLeftDarkCount;
00431 uint32_t exposureLeftBrightCount;
00432 uint32_t exposureRightSum;
00433 uint32_t exposureRightDarkCount;
00434 uint32_t exposureRightBrightCount;
00435 uint32_t CAMmoduleStatus;
00436 uint32_t pad0;
00437 uint32_t pad1;
00438 uint32_t pad2;
00439 uint32_t pad3;
00440 uint32_t VDFerrorStatus;
00441 uint32_t pad4;
00442 };
00443 #pragma pack(pop)
00444 }
00445
00446 namespace zr300
00447 {
00448 const uvc::extension_unit fisheye_xu = { 3, 3, 2,{ 0xf6c3c3d1, 0x5cde, 0x4477,{ 0xad, 0xf0, 0x41, 0x33, 0xf5, 0x8d, 0xa6, 0xf4 } } };
00449
00450
00451 void claim_motion_module_interface(uvc::device & device);
00452
00453 uint8_t get_fisheye_strobe(const uvc::device & device);
00454 void set_fisheye_strobe(uvc::device & device, uint8_t strobe);
00455 uint8_t get_fisheye_external_trigger(const uvc::device & device);
00456 void set_fisheye_external_trigger(uvc::device & device, uint8_t ext_trig);
00457 uint16_t get_fisheye_exposure(const uvc::device & device);
00458 void set_fisheye_exposure(uvc::device & device, uint16_t exposure);
00459
00460 }
00461 }
00462
00463 #endif // DS_PRIVATE_H