00001
00002
00003
00004 #include <climits>
00005 #include <algorithm>
00006 #include <iomanip>
00007
00008 #include "image.h"
00009 #include "ds-private.h"
00010 #include "ds-device.h"
00011
00012 using namespace rsimpl;
00013 using namespace rsimpl::ds;
00014 using namespace rsimpl::motion_module;
00015
00016
00017
00018 #define MAX_DS_DEFAULT_X 639
00019 #define MAX_DS_DEFAULT_Y 439
00020
00021 namespace rsimpl
00022 {
00023 ds_device::ds_device(std::shared_ptr<uvc::device> device, const static_device_info & info, calibration_validator validator)
00024 : rs_device_base(device, info, validator), start_stop_pad(std::chrono::milliseconds(500))
00025 {
00026 rs_option opt[] = {RS_OPTION_R200_DEPTH_UNITS};
00027 double units;
00028 get_options(opt, 1, &units);
00029 on_update_depth_units(static_cast<int>(units));
00030 }
00031
00032 ds_device::~ds_device()
00033 {
00034
00035 }
00036
00037 bool ds_device::is_disparity_mode_enabled() const
00038 {
00039 auto & depth = get_stream_interface(RS_STREAM_DEPTH);
00040 return depth.is_enabled() && depth.get_format() == RS_FORMAT_DISPARITY16;
00041 }
00042
00043 void ds_device::on_update_depth_units(uint32_t units)
00044 {
00045 if(is_disparity_mode_enabled()) return;
00046 config.depth_scale = (float)((double)units * 0.000001);
00047 }
00048
00049 void ds_device::on_update_disparity_multiplier(double multiplier)
00050 {
00051 if(!is_disparity_mode_enabled()) return;
00052 auto & depth = get_stream_interface(RS_STREAM_DEPTH);
00053 float baseline = get_stream_interface(RS_STREAM_INFRARED2).get_extrinsics_to(depth).translation[0];
00054 config.depth_scale = static_cast<float>(depth.get_intrinsics().fx * baseline * multiplier);
00055 }
00056
00057 std::vector<supported_option> ds_device::get_ae_range_vec()
00058 {
00059 std::vector<supported_option> so_vec;
00060 std::vector<rs_option> ae_vector = { RS_OPTION_R200_AUTO_EXPOSURE_TOP_EDGE, RS_OPTION_R200_AUTO_EXPOSURE_BOTTOM_EDGE, RS_OPTION_R200_AUTO_EXPOSURE_LEFT_EDGE, RS_OPTION_R200_AUTO_EXPOSURE_RIGHT_EDGE };
00061 for (auto& opt : ae_vector)
00062 {
00063 double min, max, step, def;
00064 get_option_range(opt, min, max, step, def);
00065 so_vec.push_back({ opt, min, max, step, def });
00066 }
00067
00068 return so_vec;
00069 }
00070
00071
00072 void correct_lr_auto_exposure_params(rs_device_base * device, ae_params& params)
00073 {
00074 auto& stream = device->get_stream_interface(RS_STREAM_DEPTH);
00075 uint16_t max_x = MAX_DS_DEFAULT_X;
00076 uint16_t max_y = MAX_DS_DEFAULT_Y;
00077 if (stream.is_enabled())
00078 {
00079 auto intrinsics = stream.get_intrinsics();
00080 max_x = intrinsics.width - 1;
00081 max_y = intrinsics.height - 1;
00082 }
00083
00084
00085 params.exposure_left_edge = std::max((uint16_t)0, std::min(max_x, params.exposure_left_edge));
00086 params.exposure_right_edge = std::max((uint16_t)0, std::min(max_x, params.exposure_right_edge));
00087 params.exposure_top_edge = std::max((uint16_t)0, std::min(max_y, params.exposure_top_edge));
00088 params.exposure_bottom_edge = std::max((uint16_t)0, std::min(max_y, params.exposure_bottom_edge));
00089
00090 auto left = std::min(params.exposure_left_edge, params.exposure_right_edge);
00091 auto right = std::max(params.exposure_left_edge, params.exposure_right_edge);
00092 auto top = std::min(params.exposure_top_edge, params.exposure_bottom_edge);
00093 auto bottom = std::max(params.exposure_top_edge, params.exposure_bottom_edge);
00094
00095 if (right == left){
00096 if (left == 0) right++;
00097 else left--;
00098 }
00099 if (bottom == top) {
00100 if (top == 0) bottom++;
00101 else top--;
00102 }
00103
00104 params.exposure_left_edge = left;
00105 params.exposure_right_edge = right;
00106 params.exposure_top_edge = top;
00107 params.exposure_bottom_edge = bottom;
00108 }
00109
00110 void ds_device::set_options(const rs_option options[], size_t count, const double values[])
00111 {
00112 std::vector<rs_option> base_opt;
00113 std::vector<double> base_opt_val;
00114
00115 auto & dev = get_device();
00116 auto minmax_writer = make_struct_interface<ds::range >([&dev]() { return ds::get_min_max_depth(dev); }, [&dev](ds::range v) { ds::set_min_max_depth(dev,v); });
00117 auto disp_writer = make_struct_interface<ds::disp_mode>([&dev]() { return ds::get_disparity_mode(dev); }, [&dev](ds::disp_mode v) { ds::set_disparity_mode(dev,v); });
00118 auto ae_writer = make_struct_interface<ds::ae_params>(
00119 [&dev, this]() {
00120 auto ae = ds::get_lr_auto_exposure_params(dev, get_ae_range_vec());
00121 correct_lr_auto_exposure_params(this, ae);
00122 return ae;
00123 },
00124 [&dev, this](ds::ae_params& v) {
00125 correct_lr_auto_exposure_params(this, v);
00126 ds::set_lr_auto_exposure_params(dev, v);
00127 }
00128 );
00129 auto dc_writer = make_struct_interface<ds::dc_params>([&dev]() { return ds::get_depth_params(dev); }, [&dev](ds::dc_params v) { ds::set_depth_params(dev,v); });
00130
00131 for (size_t i = 0; i<count; ++i)
00132 {
00133 if (uvc::is_pu_control(options[i]))
00134 {
00135
00136 switch (options[i])
00137 {
00138 case RS_OPTION_COLOR_WHITE_BALANCE: disable_auto_option( 2, RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE); break;
00139 case RS_OPTION_COLOR_EXPOSURE: disable_auto_option( 2, RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE); break;
00140 default: break;
00141 }
00142
00143 uvc::set_pu_control_with_retry(get_device(), 2, options[i], static_cast<int>(values[i]));
00144 continue;
00145 }
00146
00147 switch(options[i])
00148 {
00149
00150 case RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED: ds::set_lr_exposure_mode(get_device(), static_cast<uint8_t>(values[i])); break;
00151 case RS_OPTION_R200_LR_GAIN: ds::set_lr_gain(get_device(), {get_lr_framerate(), static_cast<uint32_t>(values[i])}); break;
00152 case RS_OPTION_R200_LR_EXPOSURE: ds::set_lr_exposure(get_device(), {get_lr_framerate(), static_cast<uint32_t>(values[i])}); break;
00153 case RS_OPTION_R200_EMITTER_ENABLED: ds::set_emitter_state(get_device(), !!values[i]); break;
00154 case RS_OPTION_R200_DEPTH_UNITS: ds::set_depth_units(get_device(), static_cast<uint32_t>(values[i]));
00155 on_update_depth_units(static_cast<uint32_t>(values[i])); break;
00156
00157 case RS_OPTION_R200_DEPTH_CLAMP_MIN: minmax_writer.set(&ds::range::min, values[i]); break;
00158 case RS_OPTION_R200_DEPTH_CLAMP_MAX: minmax_writer.set(&ds::range::max, values[i]); break;
00159
00160 case RS_OPTION_R200_DISPARITY_MULTIPLIER: disp_writer.set(&ds::disp_mode::disparity_multiplier, values[i]); break;
00161 case RS_OPTION_R200_DISPARITY_SHIFT: ds::set_disparity_shift(get_device(), static_cast<uint32_t>(values[i])); break;
00162
00163 case RS_OPTION_R200_AUTO_EXPOSURE_MEAN_INTENSITY_SET_POINT: ae_writer.set(&ds::ae_params::mean_intensity_set_point, values[i]); break;
00164 case RS_OPTION_R200_AUTO_EXPOSURE_BRIGHT_RATIO_SET_POINT: ae_writer.set(&ds::ae_params::bright_ratio_set_point, values[i]); break;
00165 case RS_OPTION_R200_AUTO_EXPOSURE_KP_GAIN: ae_writer.set(&ds::ae_params::kp_gain, values[i]); break;
00166 case RS_OPTION_R200_AUTO_EXPOSURE_KP_EXPOSURE: ae_writer.set(&ds::ae_params::kp_exposure, values[i]); break;
00167 case RS_OPTION_R200_AUTO_EXPOSURE_KP_DARK_THRESHOLD: ae_writer.set(&ds::ae_params::kp_dark_threshold, values[i]); break;
00168 case RS_OPTION_R200_AUTO_EXPOSURE_TOP_EDGE: ae_writer.set(&ds::ae_params::exposure_top_edge, values[i]); break;
00169 case RS_OPTION_R200_AUTO_EXPOSURE_BOTTOM_EDGE: ae_writer.set(&ds::ae_params::exposure_bottom_edge, values[i]); break;
00170 case RS_OPTION_R200_AUTO_EXPOSURE_LEFT_EDGE: ae_writer.set(&ds::ae_params::exposure_left_edge, values[i]); break;
00171 case RS_OPTION_R200_AUTO_EXPOSURE_RIGHT_EDGE: ae_writer.set(&ds::ae_params::exposure_right_edge, values[i]); break;
00172
00173 case RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT: dc_writer.set(&ds::dc_params::robbins_munroe_minus_inc, values[i]); break;
00174 case RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT: dc_writer.set(&ds::dc_params::robbins_munroe_plus_inc, values[i]); break;
00175 case RS_OPTION_R200_DEPTH_CONTROL_MEDIAN_THRESHOLD: dc_writer.set(&ds::dc_params::median_thresh, values[i]); break;
00176 case RS_OPTION_R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD: dc_writer.set(&ds::dc_params::score_min_thresh, values[i]); break;
00177 case RS_OPTION_R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD: dc_writer.set(&ds::dc_params::score_max_thresh, values[i]); break;
00178 case RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD: dc_writer.set(&ds::dc_params::texture_count_thresh, values[i]); break;
00179 case RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD: dc_writer.set(&ds::dc_params::texture_diff_thresh, values[i]); break;
00180 case RS_OPTION_R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD: dc_writer.set(&ds::dc_params::second_peak_thresh, values[i]); break;
00181 case RS_OPTION_R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD: dc_writer.set(&ds::dc_params::neighbor_thresh, values[i]); break;
00182 case RS_OPTION_R200_DEPTH_CONTROL_LR_THRESHOLD: dc_writer.set(&ds::dc_params::lr_thresh, values[i]); break;
00183
00184 default:
00185 base_opt.push_back(options[i]); base_opt_val.push_back(values[i]); break;
00186 }
00187 }
00188
00189 minmax_writer.commit();
00190 disp_writer.commit();
00191 if(disp_writer.active) on_update_disparity_multiplier(disp_writer.struct_.disparity_multiplier);
00192 ae_writer.commit();
00193 dc_writer.commit();
00194
00195
00196 if (base_opt.size())
00197 rs_device_base::set_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00198 }
00199
00200 void ds_device::get_options(const rs_option options[], size_t count, double values[])
00201 {
00202 std::vector<rs_option> base_opt;
00203 std::vector<size_t> base_opt_index;
00204 std::vector<double> base_opt_val;
00205
00206 auto & dev = get_device();
00207 auto minmax_reader = make_struct_interface<ds::range >([&dev]() { return ds::get_min_max_depth(dev); }, [&dev](ds::range v) { ds::set_min_max_depth(dev,v); });
00208 auto disp_reader = make_struct_interface<ds::disp_mode>([&dev]() { return ds::get_disparity_mode(dev); }, [&dev](ds::disp_mode v) { ds::set_disparity_mode(dev,v); });
00209 auto ae_reader = make_struct_interface<ds::ae_params>(
00210 [&dev, this]() {
00211 auto ae = ds::get_lr_auto_exposure_params(dev, get_ae_range_vec());
00212 correct_lr_auto_exposure_params(this, ae);
00213 return ae;
00214 },
00215 [&dev, this](ds::ae_params& v) {
00216 correct_lr_auto_exposure_params(this, v);
00217 ds::set_lr_auto_exposure_params(dev, v);
00218 }
00219 );
00220 auto dc_reader = make_struct_interface<ds::dc_params>([&dev]() { return ds::get_depth_params(dev); }, [&dev](ds::dc_params v) { ds::set_depth_params(dev,v); });
00221
00222 for (size_t i = 0; i<count; ++i)
00223 {
00224
00225 if(uvc::is_pu_control(options[i]))
00226 {
00227 values[i] = uvc::get_pu_control(get_device(), 2, options[i]);
00228 continue;
00229 }
00230
00231 switch(options[i])
00232 {
00233
00234 case RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED: values[i] = ds::get_lr_exposure_mode(get_device()); break;
00235
00236 case RS_OPTION_R200_LR_GAIN:
00237 ds::set_lr_gain_discovery(get_device(), {get_lr_framerate(), 0, 0, 0, 0});
00238 values[i] = ds::get_lr_gain(get_device()).value;
00239 break;
00240 case RS_OPTION_R200_LR_EXPOSURE:
00241 ds::set_lr_exposure_discovery(get_device(), {get_lr_framerate(), 0, 0, 0, 0});
00242 values[i] = ds::get_lr_exposure(get_device()).value;
00243 break;
00244 case RS_OPTION_R200_EMITTER_ENABLED:
00245 values[i] = ds::get_emitter_state(get_device(), is_capturing(), get_stream_interface(RS_STREAM_DEPTH).is_enabled());
00246 break;
00247
00248 case RS_OPTION_R200_DEPTH_UNITS: values[i] = ds::get_depth_units(dev); break;
00249
00250 case RS_OPTION_R200_DEPTH_CLAMP_MIN: values[i] = minmax_reader.get(&ds::range::min); break;
00251 case RS_OPTION_R200_DEPTH_CLAMP_MAX: values[i] = minmax_reader.get(&ds::range::max); break;
00252
00253 case RS_OPTION_R200_DISPARITY_MULTIPLIER: values[i] = disp_reader.get(&ds::disp_mode::disparity_multiplier); break;
00254 case RS_OPTION_R200_DISPARITY_SHIFT: values[i] = ds::get_disparity_shift(dev); break;
00255
00256 case RS_OPTION_R200_AUTO_EXPOSURE_MEAN_INTENSITY_SET_POINT: values[i] = ae_reader.get(&ds::ae_params::mean_intensity_set_point); break;
00257 case RS_OPTION_R200_AUTO_EXPOSURE_BRIGHT_RATIO_SET_POINT: values[i] = ae_reader.get(&ds::ae_params::bright_ratio_set_point ); break;
00258 case RS_OPTION_R200_AUTO_EXPOSURE_KP_GAIN: values[i] = ae_reader.get(&ds::ae_params::kp_gain ); break;
00259 case RS_OPTION_R200_AUTO_EXPOSURE_KP_EXPOSURE: values[i] = ae_reader.get(&ds::ae_params::kp_exposure ); break;
00260 case RS_OPTION_R200_AUTO_EXPOSURE_KP_DARK_THRESHOLD: values[i] = ae_reader.get(&ds::ae_params::kp_dark_threshold ); break;
00261 case RS_OPTION_R200_AUTO_EXPOSURE_TOP_EDGE: values[i] = ae_reader.get(&ds::ae_params::exposure_top_edge ); break;
00262 case RS_OPTION_R200_AUTO_EXPOSURE_BOTTOM_EDGE: values[i] = ae_reader.get(&ds::ae_params::exposure_bottom_edge ); break;
00263 case RS_OPTION_R200_AUTO_EXPOSURE_LEFT_EDGE: values[i] = ae_reader.get(&ds::ae_params::exposure_left_edge ); break;
00264 case RS_OPTION_R200_AUTO_EXPOSURE_RIGHT_EDGE: values[i] = ae_reader.get(&ds::ae_params::exposure_right_edge ); break;
00265
00266 case RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT: values[i] = dc_reader.get(&ds::dc_params::robbins_munroe_minus_inc); break;
00267 case RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT: values[i] = dc_reader.get(&ds::dc_params::robbins_munroe_plus_inc ); break;
00268 case RS_OPTION_R200_DEPTH_CONTROL_MEDIAN_THRESHOLD: values[i] = dc_reader.get(&ds::dc_params::median_thresh ); break;
00269 case RS_OPTION_R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD: values[i] = dc_reader.get(&ds::dc_params::score_min_thresh ); break;
00270 case RS_OPTION_R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD: values[i] = dc_reader.get(&ds::dc_params::score_max_thresh ); break;
00271 case RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD: values[i] = dc_reader.get(&ds::dc_params::texture_count_thresh ); break;
00272 case RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD: values[i] = dc_reader.get(&ds::dc_params::texture_diff_thresh ); break;
00273 case RS_OPTION_R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD: values[i] = dc_reader.get(&ds::dc_params::second_peak_thresh ); break;
00274 case RS_OPTION_R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD: values[i] = dc_reader.get(&ds::dc_params::neighbor_thresh ); break;
00275 case RS_OPTION_R200_DEPTH_CONTROL_LR_THRESHOLD: values[i] = dc_reader.get(&ds::dc_params::lr_thresh ); break;
00276
00277 default:
00278 base_opt.push_back(options[i]); base_opt_index.push_back(i); break;
00279 }
00280 }
00281 if (base_opt.size())
00282 {
00283 base_opt_val.resize(base_opt.size());
00284 rs_device_base::get_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00285 }
00286
00287
00288 for (auto i : base_opt_index)
00289 values[i] = base_opt_val[i];
00290 }
00291
00292 void ds_device::stop(rs_source source)
00293 {
00294 start_stop_pad.stop();
00295 rs_device_base::stop(source);
00296 }
00297
00298 void ds_device::start(rs_source source)
00299 {
00300 rs_device_base::start(source);
00301 start_stop_pad.start();
00302 }
00303
00304 void ds_device::start_fw_logger(char fw_log_op_code, int grab_rate_in_ms, std::timed_mutex& mutex)
00305 {
00306 rs_device_base::start_fw_logger(fw_log_op_code, grab_rate_in_ms, mutex);
00307 }
00308
00309 void ds_device::stop_fw_logger()
00310 {
00311 rs_device_base::stop_fw_logger();
00312 }
00313
00314 void ds_device::on_before_start(const std::vector<subdevice_mode_selection> & selected_modes)
00315 {
00316 rs_option depth_units_option = RS_OPTION_R200_DEPTH_UNITS;
00317 double depth_units;
00318
00319 uint8_t streamIntent = 0;
00320 for(const auto & m : selected_modes)
00321 {
00322 switch(m.mode.subdevice)
00323 {
00324 case 0: streamIntent |= ds::STATUS_BIT_LR_STREAMING; break;
00325 case 2: streamIntent |= ds::STATUS_BIT_WEB_STREAMING; break;
00326 case 1:
00327 streamIntent |= ds::STATUS_BIT_Z_STREAMING;
00328 auto dm = ds::get_disparity_mode(get_device());
00329 switch(m.get_format(RS_STREAM_DEPTH))
00330 {
00331 default: throw std::logic_error("unsupported R200 depth format");
00332 case RS_FORMAT_Z16:
00333 dm.is_disparity_enabled = 0;
00334 get_options(&depth_units_option, 1, &depth_units);
00335 on_update_depth_units(static_cast<int>(depth_units));
00336 break;
00337 case RS_FORMAT_DISPARITY16:
00338 dm.is_disparity_enabled = 1;
00339 on_update_disparity_multiplier(static_cast<float>(dm.disparity_multiplier));
00340 break;
00341 }
00342 ds::set_disparity_mode(get_device(), dm);
00343
00344 auto ae_enabled = ds::get_lr_exposure_mode(get_device()) > 0;
00345 if (ae_enabled)
00346 {
00347 ds::set_lr_exposure_mode(get_device(), 0);
00348 ds::set_lr_exposure_mode(get_device(), 1);
00349 }
00350
00351 break;
00352 }
00353 }
00354 ds::set_stream_intent(get_device(), streamIntent);
00355 }
00356
00357 rs_stream ds_device::select_key_stream(const std::vector<rsimpl::subdevice_mode_selection> & selected_modes)
00358 {
00359
00360
00361
00362 int fps[RS_STREAM_NATIVE_COUNT] = {}, max_fps = 0;
00363 for(const auto & m : selected_modes)
00364 {
00365 for(const auto & output : m.get_outputs())
00366 {
00367 fps[output.first] = m.mode.fps;
00368 max_fps = std::max(max_fps, m.mode.fps);
00369 }
00370 }
00371
00372
00373 for(auto s : {RS_STREAM_COLOR, RS_STREAM_INFRARED2, RS_STREAM_INFRARED})
00374 {
00375 if(fps[s] == max_fps) return s;
00376 }
00377 return RS_STREAM_DEPTH;
00378 }
00379
00380 uint32_t ds_device::get_lr_framerate() const
00381 {
00382 for(auto s : {RS_STREAM_DEPTH, RS_STREAM_INFRARED, RS_STREAM_INFRARED2})
00383 {
00384 auto & stream = get_stream_interface(s);
00385 if(stream.is_enabled()) return static_cast<uint32_t>(stream.get_framerate());
00386 }
00387 return 30;
00388 }
00389
00390 void ds_device::set_common_ds_config(std::shared_ptr<uvc::device> device, static_device_info& info, const ds::ds_info& cam_info)
00391 {
00392 auto & c = cam_info.calibration;
00393 info.capabilities_vector.push_back(RS_CAPABILITIES_ENUMERATION);
00394 info.capabilities_vector.push_back(RS_CAPABILITIES_COLOR);
00395 info.capabilities_vector.push_back(RS_CAPABILITIES_DEPTH);
00396 info.capabilities_vector.push_back(RS_CAPABILITIES_INFRARED);
00397 info.capabilities_vector.push_back(RS_CAPABILITIES_INFRARED2);
00398
00399 info.stream_subdevices[RS_STREAM_DEPTH] = 1;
00400 info.stream_subdevices[RS_STREAM_COLOR] = 2;
00401 info.stream_subdevices[RS_STREAM_INFRARED] = 0;
00402 info.stream_subdevices[RS_STREAM_INFRARED2] = 0;
00403
00404
00405 for(auto fps : {30, 60, 90})
00406 {
00407
00408 for(auto pf : {pf_y8, pf_y8i, pf_y16, pf_y12i})
00409 {
00410 info.subdevice_modes.push_back({0, {640, 481}, pf, fps, c.modesLR[0], {}, {0, -6}});
00411 info.subdevice_modes.push_back({0, {640, 373}, pf, fps, c.modesLR[1], {}, {0, -6}});
00412 info.subdevice_modes.push_back({0, {640, 254}, pf, fps, c.modesLR[2], {}, {0, -6}});
00413 }
00414
00415
00416 info.subdevice_modes.push_back({ 1,{ 628, 469 }, pf_z16, fps, pad_crop_intrinsics(c.modesLR[0], -6),{},{ 0, +6 } });
00417 info.subdevice_modes.push_back({ 1,{ 628, 361 }, pf_z16, fps, pad_crop_intrinsics(c.modesLR[1], -6),{},{ 0, +6 } });
00418 info.subdevice_modes.push_back({ 1,{ 628, 242 }, pf_z16, fps, pad_crop_intrinsics(c.modesLR[2], -6),{},{ 0, +6 } });
00419 }
00420
00421
00422 info.subdevice_modes.push_back({ 2, { 640, 480 }, pf_yuy2, 60, c.intrinsicsThird[1], { c.modesThird[1][0] }, { 0 } });
00423 info.subdevice_modes.push_back({ 2, { 640, 480 }, pf_yuy2, 30, c.intrinsicsThird[1], { c.modesThird[1][0] }, { 0 } });
00424 info.subdevice_modes.push_back({ 2, { 320, 240 }, pf_yuy2, 60, scale_intrinsics(c.intrinsicsThird[1], 320, 240), { c.modesThird[1][1] }, { 0 } });
00425 info.subdevice_modes.push_back({ 2, { 320, 240 }, pf_yuy2, 30, scale_intrinsics(c.intrinsicsThird[1], 320, 240), { c.modesThird[1][1] }, { 0 } });
00426
00427 info.subdevice_modes.push_back({ 2,{ 1920, 1080 }, pf_yuy2, 15, c.intrinsicsThird[0],{ c.modesThird[0][0] },{ 0 } });
00428 info.subdevice_modes.push_back({ 2,{ 1920, 1080 }, pf_yuy2, 30, c.intrinsicsThird[0],{ c.modesThird[0][0] },{ 0 } });
00429
00430
00431
00432
00433 info.subdevice_modes.push_back({ 2,{ 1280, 720 }, pf_yuy2, 15, scale_intrinsics(c.intrinsicsThird[0], 1280, 720), { c.modesThird[0][1] }, { 0 } });
00434 info.subdevice_modes.push_back({ 2, { 960, 540 }, pf_yuy2, 15, scale_intrinsics(c.intrinsicsThird[0], 960, 540), { c.modesThird[0][1] }, { 0 } });
00435 info.subdevice_modes.push_back({ 2, { 848, 480 }, pf_yuy2, 15, scale_intrinsics(c.intrinsicsThird[0], 848, 480), { c.modesThird[0][1] }, { 0 } });
00436
00437 info.subdevice_modes.push_back({ 2, { 640, 480 }, pf_yuy2, 15, c.intrinsicsThird[1], { c.modesThird[1][0] }, { 0 } });
00438 info.subdevice_modes.push_back({ 2, { 640, 480 }, pf_rw16, 15, c.intrinsicsThird[1], { c.modesThird[1][0] }, { 0 } });
00439
00440 info.subdevice_modes.push_back({ 2, { 640, 360 }, pf_yuy2, 15, scale_intrinsics(c.intrinsicsThird[1], 640, 360), { c.modesThird[1][0] }, { 0 } });
00441 info.subdevice_modes.push_back({ 2, { 424, 240 }, pf_yuy2, 15, scale_intrinsics(c.intrinsicsThird[1], 424, 240), { c.modesThird[1][0] }, { 0 } });
00442
00443 info.subdevice_modes.push_back({ 2, { 320, 240 }, pf_yuy2, 15, scale_intrinsics(c.intrinsicsThird[1], 320, 240), { c.modesThird[1][1] }, { 0 } });
00444 info.subdevice_modes.push_back({ 2, { 320, 180 }, pf_yuy2, 15, scale_intrinsics(c.intrinsicsThird[1], 320, 180), { c.modesThird[1][1] }, { 0 } });
00445
00446
00447 for(auto ir : {RS_STREAM_INFRARED, RS_STREAM_INFRARED2})
00448 {
00449 info.interstream_rules.push_back({ RS_STREAM_DEPTH, ir, &stream_request::width, 0, 12, RS_STREAM_COUNT, false, false, false });
00450 info.interstream_rules.push_back({ RS_STREAM_DEPTH, ir, &stream_request::height, 0, 12, RS_STREAM_COUNT, false, false, false });
00451 info.interstream_rules.push_back({ RS_STREAM_DEPTH, ir, &stream_request::fps, 0, 0, RS_STREAM_COUNT, false, false, false });
00452 }
00453 info.interstream_rules.push_back({ RS_STREAM_DEPTH, RS_STREAM_COLOR, &stream_request::fps, 0, 0, RS_STREAM_DEPTH, true, false, false });
00454 info.interstream_rules.push_back({ RS_STREAM_INFRARED, RS_STREAM_INFRARED2, &stream_request::fps, 0, 0, RS_STREAM_COUNT, false, false, false });
00455 info.interstream_rules.push_back({ RS_STREAM_INFRARED, RS_STREAM_INFRARED2, &stream_request::width, 0, 0, RS_STREAM_COUNT, false, false, false });
00456 info.interstream_rules.push_back({ RS_STREAM_INFRARED, RS_STREAM_INFRARED2, &stream_request::height, 0, 0, RS_STREAM_COUNT, false, false, false });
00457 info.interstream_rules.push_back({ RS_STREAM_INFRARED, RS_STREAM_INFRARED2, nullptr, 0, 0, RS_STREAM_COUNT, false, false, true });
00458
00459 info.presets[RS_STREAM_INFRARED][RS_PRESET_BEST_QUALITY] = {true, 480, 360, RS_FORMAT_Y8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00460 info.presets[RS_STREAM_DEPTH ][RS_PRESET_BEST_QUALITY] = {true, 480, 360, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00461 info.presets[RS_STREAM_COLOR ][RS_PRESET_BEST_QUALITY] = {true, 640, 480, RS_FORMAT_RGB8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00462
00463 info.presets[RS_STREAM_INFRARED][RS_PRESET_LARGEST_IMAGE] = {true, 640, 480, RS_FORMAT_Y8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00464 info.presets[RS_STREAM_DEPTH ][RS_PRESET_LARGEST_IMAGE] = {true, 640, 480, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00465 info.presets[RS_STREAM_COLOR ][RS_PRESET_LARGEST_IMAGE] = {true, 1920, 1080, RS_FORMAT_RGB8, 30, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00466
00467 info.presets[RS_STREAM_INFRARED][RS_PRESET_HIGHEST_FRAMERATE] = {true, 320, 240, RS_FORMAT_Y8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00468 info.presets[RS_STREAM_DEPTH ][RS_PRESET_HIGHEST_FRAMERATE] = {true, 320, 240, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00469 info.presets[RS_STREAM_COLOR ][RS_PRESET_HIGHEST_FRAMERATE] = {true, 640, 480, RS_FORMAT_RGB8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00470
00471 for(int i=0; i<RS_PRESET_COUNT; ++i)
00472 info.presets[RS_STREAM_INFRARED2][i] = info.presets[RS_STREAM_INFRARED][i];
00473
00474
00475
00476 info.options.push_back({ RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED, 0, 1, 1, 0 });
00477 info.options.push_back({ RS_OPTION_R200_EMITTER_ENABLED, 0, 1, 1, 0 });
00478 info.options.push_back({ RS_OPTION_R200_DEPTH_UNITS, 0, INT_MAX, 1, 1000 });
00479 info.options.push_back({ RS_OPTION_R200_DEPTH_CLAMP_MIN, 0, USHRT_MAX, 1, 0 });
00480 info.options.push_back({ RS_OPTION_R200_DEPTH_CLAMP_MAX, 0, USHRT_MAX, 1, USHRT_MAX });
00481 info.options.push_back({ RS_OPTION_R200_AUTO_EXPOSURE_MEAN_INTENSITY_SET_POINT, 0, 4095, 1, 512 });
00482 info.options.push_back({ RS_OPTION_R200_AUTO_EXPOSURE_BRIGHT_RATIO_SET_POINT, 0, 1, 1, 0 });
00483 info.options.push_back({ RS_OPTION_R200_AUTO_EXPOSURE_KP_GAIN, 0, 1000, 1, 0 });
00484 info.options.push_back({ RS_OPTION_R200_AUTO_EXPOSURE_KP_EXPOSURE, 0, 1000, 1, 0 });
00485 info.options.push_back({ RS_OPTION_R200_AUTO_EXPOSURE_KP_DARK_THRESHOLD, 0, 1000, 1, 0 });
00486 info.options.push_back({ RS_OPTION_R200_AUTO_EXPOSURE_TOP_EDGE, 0, MAX_DS_DEFAULT_Y, 1, MAX_DS_DEFAULT_Y });
00487 info.options.push_back({ RS_OPTION_R200_AUTO_EXPOSURE_BOTTOM_EDGE, 0, MAX_DS_DEFAULT_Y, 1, MAX_DS_DEFAULT_Y});
00488 info.options.push_back({ RS_OPTION_R200_AUTO_EXPOSURE_LEFT_EDGE, 0, MAX_DS_DEFAULT_X, 1, MAX_DS_DEFAULT_X });
00489 info.options.push_back({ RS_OPTION_R200_AUTO_EXPOSURE_RIGHT_EDGE, 0, MAX_DS_DEFAULT_X, 1, MAX_DS_DEFAULT_X });
00490 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT, 0, 0xFF, 1, 5 });
00491 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT, 0, 0xFF, 1, 5 });
00492 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_MEDIAN_THRESHOLD, 0, 0x3FF, 1, 0xc0 });
00493 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD, 0, 0x3FF, 1, 1 });
00494 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD, 0, 0x3FF, 1, 0x200 });
00495 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD, 0, 0x1F, 1, 6 });
00496 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD, 0, 0x3FF, 1, 0x18 });
00497 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD, 0, 0x3FF, 1, 0x1b });
00498 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD, 0, 0x3FF, 1, 0x7 });
00499 info.options.push_back({ RS_OPTION_R200_DEPTH_CONTROL_LR_THRESHOLD, 0, 0x7FF, 1, 0x18 });
00500
00501
00502 info.stream_poses[RS_STREAM_DEPTH] = {{{1,0,0},{0,1,0},{0,0,1}},{0,0,0}};
00503 info.stream_poses[RS_STREAM_INFRARED] = {{{1,0,0},{0,1,0},{0,0,1}},{0,0,0}};
00504
00505
00506 info.stream_poses[RS_STREAM_INFRARED2] = {{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}, {c.B * 0.001f, 0, 0}};
00507
00508
00509 for(int i=0; i<3; ++i) for(int j=0; j<3; ++j)
00510 info.stream_poses[RS_STREAM_COLOR].orientation(i,j) = c.Rthird[i*3+j];
00511 for(int i=0; i<3; ++i)
00512 info.stream_poses[RS_STREAM_COLOR].position[i] = c.T[i] * 0.001f;
00513
00514
00515 info.stream_poses[RS_STREAM_COLOR].position = info.stream_poses[RS_STREAM_COLOR].orientation * info.stream_poses[RS_STREAM_COLOR].position;
00516 info.nominal_depth_scale = 0.001f;
00517 info.serial = std::to_string(c.serial_number);
00518 info.firmware_version = ds::read_firmware_version(*device);
00519
00520 auto &h = cam_info.head_content;
00521 info.camera_info[RS_CAMERA_INFO_CAMERA_FIRMWARE_VERSION] = info.firmware_version;
00522 info.camera_info[RS_CAMERA_INFO_DEVICE_SERIAL_NUMBER] = info.serial;
00523 info.camera_info[RS_CAMERA_INFO_DEVICE_NAME] = info.name;
00524
00525 info.camera_info[RS_CAMERA_INFO_ISP_FW_VERSION] = ds::read_isp_firmware_version(*device);
00526 info.camera_info[RS_CAMERA_INFO_IMAGER_MODEL_NUMBER] = to_string() << h.imager_model_number;
00527 info.camera_info[RS_CAMERA_INFO_CAMERA_TYPE] = to_string() << h.prq_type;
00528 info.camera_info[RS_CAMERA_INFO_OEM_ID] = to_string() << h.oem_id;
00529 info.camera_info[RS_CAMERA_INFO_MODULE_VERSION] = to_string() << (int)h.module_version << "." << (int)h.module_major_version << "." << (int)h.module_minor_version << "." << (int)h.module_skew_version;
00530 info.camera_info[RS_CAMERA_INFO_FOCUS_VALUE] = to_string() << h.platform_camera_focus;
00531 info.camera_info[RS_CAMERA_INFO_CONTENT_VERSION] = to_string() << h.camera_head_contents_version;
00532 info.camera_info[RS_CAMERA_INFO_LENS_TYPE] = to_string() << h.lens_type;
00533 info.camera_info[RS_CAMERA_INFO_LENS_COATING__TYPE] = to_string() << h.lens_coating_type;
00534 info.camera_info[RS_CAMERA_INFO_3RD_LENS_TYPE] = to_string() << h.lens_type_third_imager;
00535 info.camera_info[RS_CAMERA_INFO_3RD_LENS_COATING_TYPE] = to_string() << h.lens_coating_type_third_imager;
00536 info.camera_info[RS_CAMERA_INFO_NOMINAL_BASELINE] = to_string() << h.nominal_baseline << " mm";
00537 info.camera_info[RS_CAMERA_INFO_3RD_NOMINAL_BASELINE] = to_string() << h.nominal_baseline_third_imager << " mm";
00538 info.camera_info[RS_CAMERA_INFO_EMITTER_TYPE] = to_string() << h.emitter_type;
00539
00540 if (std::isnormal(h.calibration_date))
00541 info.camera_info[RS_CAMERA_INFO_CALIBRATION_DATE] = time_to_string(h.calibration_date);
00542 if (std::isnormal(h.first_program_date))
00543 info.camera_info[RS_CAMERA_INFO_PROGRAM_DATE] = time_to_string(h.first_program_date);
00544 if (std::isnormal(h.focus_alignment_date))
00545 info.camera_info[RS_CAMERA_INFO_FOCUS_ALIGNMENT_DATE] = time_to_string(h.focus_alignment_date);
00546 if (std::isnormal(h.build_date))
00547 info.camera_info[RS_CAMERA_INFO_BUILD_DATE] = time_to_string(h.build_date);
00548
00549
00550 info.num_libuvc_transfer_buffers = 4;
00551
00552 rs_device_base::update_device_info(info);
00553 }
00554
00555 bool ds_device::supports_option(rs_option option) const
00556 {
00557 std::vector<rs_option> auto_exposure_options = {
00558 RS_OPTION_R200_AUTO_EXPOSURE_BOTTOM_EDGE,
00559 RS_OPTION_R200_AUTO_EXPOSURE_TOP_EDGE,
00560 RS_OPTION_R200_AUTO_EXPOSURE_LEFT_EDGE,
00561 RS_OPTION_R200_AUTO_EXPOSURE_RIGHT_EDGE,
00562 RS_OPTION_R200_AUTO_EXPOSURE_KP_EXPOSURE,
00563 RS_OPTION_R200_AUTO_EXPOSURE_KP_GAIN,
00564 RS_OPTION_R200_AUTO_EXPOSURE_KP_DARK_THRESHOLD,
00565 RS_OPTION_R200_AUTO_EXPOSURE_BRIGHT_RATIO_SET_POINT,
00566 RS_OPTION_R200_AUTO_EXPOSURE_MEAN_INTENSITY_SET_POINT
00567 };
00568
00569 if (std::find(auto_exposure_options.begin(), auto_exposure_options.end(), option) != auto_exposure_options.end())
00570 {
00571 return ds::get_lr_exposure_mode(get_device()) > 0;
00572 }
00573
00574 std::vector<rs_option> only_when_not_streaming = {
00575 RS_OPTION_R200_DEPTH_UNITS,
00576 RS_OPTION_R200_DEPTH_CLAMP_MIN,
00577 RS_OPTION_R200_DEPTH_CLAMP_MAX,
00578 RS_OPTION_R200_DISPARITY_MULTIPLIER,
00579 RS_OPTION_R200_DISPARITY_SHIFT,
00580 };
00581
00582 if (std::find(only_when_not_streaming.begin(), only_when_not_streaming.end(), option) != only_when_not_streaming.end())
00583 {
00584 if (is_capturing()) return false;
00585 }
00586
00587
00588 return option == RS_OPTION_R200_LR_GAIN || option == RS_OPTION_R200_LR_EXPOSURE || rs_device_base::supports_option(option);
00589 }
00590
00591 void ds_device::get_option_range(rs_option option, double & min, double & max, double & step, double & def)
00592 {
00593
00594 if(option == RS_OPTION_R200_LR_GAIN)
00595 {
00596 ds::set_lr_gain_discovery(get_device(), {get_lr_framerate(), 0, 0, 0, 0});
00597 auto disc = ds::get_lr_gain_discovery(get_device());
00598 min = disc.min;
00599 max = disc.max;
00600 step = 1;
00601 def = disc.default_value;
00602 return;
00603 }
00604
00605
00606 if(option == RS_OPTION_R200_LR_EXPOSURE)
00607 {
00608 ds::set_lr_exposure_discovery(get_device(), {get_lr_framerate(), 0, 0, 0, 0});
00609 auto disc = ds::get_lr_exposure_discovery(get_device());
00610 min = disc.min;
00611 max = disc.max;
00612 step = 1;
00613 def = disc.default_value;
00614 return;
00615 }
00616
00617
00618 if(option == RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE)
00619 {
00620 rs_device_base::get_option_range(option, min, max, step, def);
00621 max = 1;
00622 step = 1;
00623 return;
00624 }
00625
00626 std::vector<rs_option> auto_exposure_options = {
00627 RS_OPTION_R200_AUTO_EXPOSURE_BOTTOM_EDGE,
00628 RS_OPTION_R200_AUTO_EXPOSURE_TOP_EDGE,
00629 RS_OPTION_R200_AUTO_EXPOSURE_LEFT_EDGE,
00630 RS_OPTION_R200_AUTO_EXPOSURE_RIGHT_EDGE,
00631 };
00632 if (std::find(auto_exposure_options.begin(), auto_exposure_options.end(), option) != auto_exposure_options.end())
00633 {
00634 auto& stream = get_stream_interface(rs_stream::RS_STREAM_DEPTH);
00635 if (option == RS_OPTION_R200_AUTO_EXPOSURE_RIGHT_EDGE)
00636 {
00637 auto max_x = MAX_DS_DEFAULT_X;
00638 if (stream.is_enabled()) max_x = stream.get_intrinsics().width - 1;
00639 min = 1; max = max_x; step = 1; def = max_x;
00640 return;
00641 }
00642 else if (option == RS_OPTION_R200_AUTO_EXPOSURE_LEFT_EDGE)
00643 {
00644 auto max_x = MAX_DS_DEFAULT_X;
00645 if (stream.is_enabled()) max_x = stream.get_intrinsics().width - 1;
00646 min = 1; max = max_x - 1; step = 1; def = 0;
00647 return;
00648 }
00649 else if (option == RS_OPTION_R200_AUTO_EXPOSURE_BOTTOM_EDGE)
00650 {
00651 auto max_y = MAX_DS_DEFAULT_Y;
00652 if (stream.is_enabled()) max_y = stream.get_intrinsics().height - 1;
00653 min = 1; max = max_y; step = 1; def = max_y;
00654 return;
00655 }
00656 else if (option == RS_OPTION_R200_AUTO_EXPOSURE_TOP_EDGE)
00657 {
00658 auto max_y = MAX_DS_DEFAULT_Y;
00659 if (stream.is_enabled()) max_y = stream.get_intrinsics().height - 1;
00660 min = 0; max = max_y - 1; step = 1; def = 0;
00661 return;
00662 }
00663 }
00664
00665
00666 rs_device_base::get_option_range(option, min, max, step, def);
00667 }
00668
00669
00670 const ds::dinghy & get_dinghy(const subdevice_mode & mode, const void * frame)
00671 {
00672 return *reinterpret_cast<const ds::dinghy *>(reinterpret_cast<const uint8_t *>(frame) + mode.pf.get_image_size(mode.native_dims.x, mode.native_dims.y-1));
00673 }
00674
00675 class dinghy_timestamp_reader : public frame_timestamp_reader
00676 {
00677 int fps;
00678 wraparound_mechanism<double> timestamp_wraparound;
00679 wraparound_mechanism<unsigned long long> frame_counter_wraparound;
00680 double last_timestamp;
00681
00682 public:
00683 dinghy_timestamp_reader(int fps) : fps(fps), last_timestamp(0), timestamp_wraparound(1, std::numeric_limits<uint32_t>::max()), frame_counter_wraparound(1, std::numeric_limits<uint32_t>::max()) {}
00684
00685 bool validate_frame(const subdevice_mode & mode, const void * frame) override
00686 {
00687
00688 auto & dinghy = get_dinghy(mode, frame);
00689 const uint32_t magic_numbers[] = {0x08070605, 0x04030201, 0x8A8B8C8D};
00690 if(dinghy.magicNumber != magic_numbers[mode.subdevice])
00691 {
00692 LOG_WARNING("Subdevice " << mode.subdevice << " bad magic number 0x" << std::hex << dinghy.magicNumber);
00693 return false;
00694 }
00695
00696
00697 if(dinghy.frameStatus != 0)
00698 {
00699 LOG_WARNING("Subdevice " << mode.subdevice << " frame status 0x" << std::hex << dinghy.frameStatus);
00700 return false;
00701 }
00702
00703 if(dinghy.VDFerrorStatus != 0)
00704 {
00705 LOG_WARNING("Subdevice " << mode.subdevice << " VDF error status 0x" << std::hex << dinghy.VDFerrorStatus);
00706 return false;
00707 }
00708
00709 if (dinghy.CAMmoduleStatus != 0 && mode.subdevice == 0)
00710 {
00711 LOG_WARNING("Subdevice " << mode.subdevice << " CAM module status 0x" << std::hex << dinghy.CAMmoduleStatus);
00712 return false;
00713 }
00714
00715
00716 return true;
00717 }
00718
00719 double get_frame_timestamp(const subdevice_mode & mode, const void * frame, double ) override
00720 {
00721 auto new_ts = timestamp_wraparound.fix(last_timestamp + 1000. / fps);
00722 last_timestamp = new_ts;
00723 return new_ts;
00724 }
00725
00726 unsigned long long get_frame_counter(const subdevice_mode & mode, const void * frame) override
00727 {
00728 auto frame_number = 0;
00729
00730 frame_number = get_dinghy(mode, frame).frameCount;
00731 return frame_counter_wraparound.fix(frame_number);
00732 }
00733 };
00734
00735 class fisheye_timestamp_reader : public frame_timestamp_reader
00736 {
00737 int get_embedded_frame_counter(const void * frame) const
00738 {
00739 int embedded_frame_counter = 0;
00740 firmware_version firmware(fw_version);
00741 if (firmware >= firmware_version("1.27.2.90"))
00742 {
00743 auto data = static_cast<const char*>(frame);
00744
00745 for (int i = 0, j = 0; i < 4; ++i, ++j)
00746 embedded_frame_counter |= ((data[i] & 0x01) << j);
00747 }
00748 else if (firmware < firmware_version("1.27.2.90"))
00749 {
00750 embedded_frame_counter = reinterpret_cast<byte_wrapping&>(*((unsigned char*)frame)).lsb;
00751 }
00752
00753 return embedded_frame_counter;
00754 }
00755
00756 std::string fw_version;
00757 std::mutex mutex;
00758 int configured_fps;
00759 unsigned last_fisheye_counter;
00760 double last_fisheye_timestamp;
00761 wraparound_mechanism<double> timestamp_wraparound;
00762 wraparound_mechanism<unsigned long long> frame_counter_wraparound;
00763 mutable bool validate;
00764
00765 public:
00766 fisheye_timestamp_reader(int in_configured_fps, const char* fw_ver) : configured_fps(in_configured_fps), last_fisheye_timestamp(0), last_fisheye_counter(0), timestamp_wraparound(1, std::numeric_limits<uint32_t>::max()), frame_counter_wraparound(0, std::numeric_limits<uint32_t>::max()), validate(true), fw_version(fw_ver){}
00767
00768 bool validate_frame(const subdevice_mode & , const void * frame) override
00769 {
00770 if (!validate)
00771 return true;
00772
00773 bool sts;
00774 auto pixel_lsb = get_embedded_frame_counter(frame);
00775 if ((sts = (pixel_lsb != 0)))
00776 validate = false;
00777
00778 return sts;
00779 }
00780
00781 struct byte_wrapping{
00782 unsigned char lsb : 4;
00783 unsigned char msb : 4;
00784 };
00785
00786 unsigned long long get_frame_counter(const subdevice_mode & , const void * frame) override
00787 {
00788 std::lock_guard<std::mutex> guard(mutex);
00789
00790 auto last_counter_lsb = reinterpret_cast<byte_wrapping&>(last_fisheye_counter).lsb;
00791 auto pixel_lsb = get_embedded_frame_counter(frame);
00792 if (last_counter_lsb == pixel_lsb)
00793 return last_fisheye_counter;
00794
00795 auto last_counter_msb = (last_fisheye_counter >> 4);
00796 auto wrap_around = reinterpret_cast<byte_wrapping&>(last_fisheye_counter).lsb;
00797 if (wrap_around == 15 || pixel_lsb < last_counter_lsb)
00798 {
00799 ++last_counter_msb;
00800 }
00801
00802 auto fixed_counter = (last_counter_msb << 4) | (pixel_lsb & 0xff);
00803
00804 last_fisheye_counter = fixed_counter;
00805 return frame_counter_wraparound.fix(fixed_counter);
00806 }
00807
00808 double get_frame_timestamp(const subdevice_mode & mode, const void * frame, double actual_fps) override
00809 {
00810 auto new_ts = timestamp_wraparound.fix(last_fisheye_timestamp + 1000. / actual_fps);
00811 last_fisheye_timestamp = new_ts;
00812 return new_ts;
00813 }
00814 };
00815
00816 class color_timestamp_reader : public frame_timestamp_reader
00817 {
00818 int fps, scale;
00819 wraparound_mechanism<double> timestamp_wraparound;
00820 wraparound_mechanism<unsigned long long> frame_counter_wraparound;
00821 bool first_frames = true;
00822 double last_timestamp;
00823 public:
00824 color_timestamp_reader(int fps, int scale) : fps(fps), scale(scale), last_timestamp(0), timestamp_wraparound(0, std::numeric_limits<uint32_t>::max()), frame_counter_wraparound(0, std::numeric_limits<uint32_t>::max()) {}
00825
00826 bool validate_frame(const subdevice_mode & mode, const void * frame) override
00827 {
00828 auto counter = get_frame_counter(mode, frame);
00829
00830 if (counter == 0 && first_frames) return false;
00831 first_frames = false;
00832 return true;
00833 }
00834
00835 unsigned long long get_frame_counter(const subdevice_mode & mode, const void * frame) override
00836 {
00837 auto frame_number = 0;
00838
00839
00840 auto data = reinterpret_cast<const uint8_t *>(frame)+((mode.native_dims.x * mode.native_dims.y) - 32) * 2;
00841 for (auto i = 0; i < 32; ++i)
00842 {
00843 frame_number |= ((*data & 1) << (i & 1 ? 32 - i : 30 - i));
00844 data += 2;
00845 }
00846
00847 frame_number /= scale;
00848
00849 return frame_counter_wraparound.fix(frame_number);
00850 }
00851
00852 double get_frame_timestamp(const subdevice_mode & mode, const void * frame, double ) override
00853 {
00854 auto new_ts = timestamp_wraparound.fix(last_timestamp + 1000. / fps);
00855 last_timestamp = new_ts;
00856 return new_ts;
00857 }
00858 };
00859
00860 class serial_timestamp_generator : public frame_timestamp_reader
00861 {
00862 int fps, serial_frame_number;
00863 double last_timestamp;
00864 double ts_step;
00865 wraparound_mechanism<double> timestamp_wraparound;
00866 wraparound_mechanism<unsigned long long> frame_counter_wraparound;
00867
00868 public:
00869 serial_timestamp_generator(int fps) : fps(fps), serial_frame_number(), last_timestamp(0), timestamp_wraparound(0, std::numeric_limits<uint32_t>::max()), frame_counter_wraparound(0, std::numeric_limits<uint32_t>::max())
00870 {
00871 assert(fps > 0);
00872 ts_step = 1000. / fps;
00873 }
00874
00875 bool validate_frame(const subdevice_mode & , const void * ) override { return true; }
00876 double get_frame_timestamp(const subdevice_mode &, const void *, double ) override
00877 {
00878 auto new_ts = timestamp_wraparound.fix(last_timestamp + ts_step);
00879 last_timestamp = new_ts;
00880 return new_ts;
00881 }
00882 unsigned long long get_frame_counter(const subdevice_mode &, const void *) override
00883 {
00884 return frame_counter_wraparound.fix(++serial_frame_number);
00885 }
00886 };
00887
00888
00889 std::shared_ptr<frame_timestamp_reader> ds_device::create_frame_timestamp_reader(int subdevice) const
00890 {
00891 auto & stream_depth = get_stream_interface(RS_STREAM_DEPTH);
00892 auto & stream_infrared = get_stream_interface(RS_STREAM_INFRARED);
00893 auto & stream_infrared2 = get_stream_interface(RS_STREAM_INFRARED2);
00894 auto & stream_fisheye = get_stream_interface(RS_STREAM_FISHEYE);
00895 auto & stream_color = get_stream_interface(RS_STREAM_COLOR);
00896
00897 switch (subdevice)
00898 {
00899 case SUB_DEVICE_DEPTH:
00900 if (stream_depth.is_enabled())
00901 return std::make_shared<dinghy_timestamp_reader>(stream_depth.get_framerate());
00902 break;
00903 case SUB_DEVICE_INFRARED:
00904 if (stream_infrared.is_enabled())
00905 return std::make_shared<dinghy_timestamp_reader>(stream_infrared.get_framerate());
00906
00907 if (stream_infrared2.is_enabled())
00908 return std::make_shared<dinghy_timestamp_reader>(stream_infrared2.get_framerate());
00909 break;
00910 case SUB_DEVICE_FISHEYE:
00911 if (stream_fisheye.is_enabled())
00912 return std::make_shared<fisheye_timestamp_reader>(stream_fisheye.get_framerate(), get_camera_info(RS_CAMERA_INFO_ADAPTER_BOARD_FIRMWARE_VERSION));
00913 break;
00914 case SUB_DEVICE_COLOR:
00915 if (stream_color.is_enabled())
00916 {
00917
00918
00919
00920 bool depth_streams_active = stream_depth.is_enabled() | stream_infrared.is_enabled() | stream_infrared2.is_enabled();
00921
00922 if (depth_streams_active && (stream_color.get_format() != rs_format::RS_FORMAT_RAW16))
00923 {
00924 auto master_fps = stream_depth.is_enabled() ? stream_depth.get_framerate() : 0;
00925 master_fps = stream_infrared.is_enabled() ? stream_infrared.get_framerate() : master_fps;
00926 master_fps = stream_infrared2.is_enabled() ? stream_infrared2.get_framerate() : master_fps;
00927 auto scale = master_fps / stream_color.get_framerate();
00928
00929 return std::make_shared<color_timestamp_reader>(stream_color.get_framerate(), scale);
00930 }
00931 else
00932 {
00933 return std::make_shared<serial_timestamp_generator>(stream_color.get_framerate());
00934 }
00935 }
00936 break;
00937 }
00938
00939
00940 return nullptr;
00941 }
00942
00943 std::vector<std::shared_ptr<frame_timestamp_reader>> ds_device::create_frame_timestamp_readers() const
00944 {
00945 return { create_frame_timestamp_reader(SUB_DEVICE_INFRARED),
00946 create_frame_timestamp_reader(SUB_DEVICE_DEPTH),
00947 create_frame_timestamp_reader(SUB_DEVICE_COLOR),
00948 create_frame_timestamp_reader(SUB_DEVICE_FISHEYE) };
00949 }
00950 }