00001
00002
00003
00004 #include <climits>
00005 #include <algorithm>
00006
00007 #include "image.h"
00008 #include "sr300.h"
00009
00010 namespace rsimpl
00011 {
00012 const std::vector <std::pair<rs_option, char>> eu_SR300_depth_controls = {{rs_option::RS_OPTION_F200_LASER_POWER, 0x01},
00013 {rs_option::RS_OPTION_F200_ACCURACY, 0x02},
00014 {rs_option::RS_OPTION_F200_MOTION_RANGE, 0x03},
00015 {rs_option::RS_OPTION_F200_FILTER_OPTION, 0x05},
00016 {rs_option::RS_OPTION_F200_CONFIDENCE_THRESHOLD, 0x06}};
00017
00018 static const cam_mode sr300_color_modes[] = {
00019 {{1920, 1080}, { 10,30 } },
00020 {{1280, 720}, { 10,30,60 } },
00021 {{ 960, 540}, { 10,30,60 } },
00022 {{ 848, 480}, { 10,30,60 } },
00023 {{ 640, 480}, { 10,30,60 } },
00024 {{ 640, 360}, { 10,30,60 } },
00025 {{ 424, 240}, { 10,30,60 } },
00026 {{ 320, 240}, { 10,30,60 } },
00027 {{ 320, 180}, { 10,30,60 } },
00028 };
00029
00030 static const cam_mode sr300_depth_modes[] = {
00031 {{640, 480}, {10,30,60}},
00032 {{640, 240}, {10,30,60,110}}
00033 };
00034
00035 static const cam_mode sr300_ir_only_modes[] = {
00036 {{640, 480}, {30,60,120,200}}
00037 };
00038
00039 static static_device_info get_sr300_info(std::shared_ptr<uvc::device> , const ivcam::camera_calib_params & c)
00040 {
00041 LOG_INFO("Connecting to Intel RealSense SR300");
00042
00043 static_device_info info;
00044 info.name = "Intel RealSense SR300";
00045
00046
00047 info.stream_subdevices[RS_STREAM_COLOR] = 0;
00048 for(auto & m : sr300_color_modes)
00049 {
00050 for(auto fps : m.fps)
00051 {
00052 info.subdevice_modes.push_back({0, m.dims, pf_yuy2, fps, MakeColorIntrinsics(c, m.dims), {}, {0}});
00053 }
00054 }
00055
00056
00057 info.stream_subdevices[RS_STREAM_DEPTH] = 1;
00058 info.stream_subdevices[RS_STREAM_INFRARED] = 1;
00059 for(auto & m : sr300_ir_only_modes)
00060 {
00061 for(auto fps : m.fps)
00062 {
00063 info.subdevice_modes.push_back({1, m.dims, pf_sr300_invi, fps, MakeDepthIntrinsics(c, m.dims), {}, {0}});
00064 }
00065 }
00066 for(auto & m : sr300_depth_modes)
00067 {
00068 for(auto fps : m.fps)
00069 {
00070 info.subdevice_modes.push_back({1, m.dims, pf_invz, fps, MakeDepthIntrinsics(c, m.dims), {}, {0}});
00071 info.subdevice_modes.push_back({1, m.dims, pf_sr300_inzi, fps, MakeDepthIntrinsics(c, m.dims), {}, {0}});
00072 }
00073 }
00074
00075 for(int i=0; i<RS_PRESET_COUNT; ++i)
00076 {
00077 info.presets[RS_STREAM_COLOR ][i] = {true, 640, 480, RS_FORMAT_RGB8, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00078 info.presets[RS_STREAM_DEPTH ][i] = {true, 640, 480, RS_FORMAT_Z16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00079 info.presets[RS_STREAM_INFRARED][i] = {true, 640, 480, RS_FORMAT_Y16, 60, RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS};
00080 }
00081
00082 info.options = {
00083 {RS_OPTION_SR300_AUTO_RANGE_ENABLE_MOTION_VERSUS_RANGE, 0.0, 2.0, 1.0, -1.0},
00084 {RS_OPTION_SR300_AUTO_RANGE_ENABLE_LASER, 0.0, 1.0, 1.0, -1.0},
00085 {RS_OPTION_SR300_AUTO_RANGE_MIN_MOTION_VERSUS_RANGE, (double)SHRT_MIN, (double)SHRT_MAX, 1.0, -1.0},
00086 {RS_OPTION_SR300_AUTO_RANGE_MAX_MOTION_VERSUS_RANGE, (double)SHRT_MIN, (double)SHRT_MAX, 1.0, -1.0},
00087 {RS_OPTION_SR300_AUTO_RANGE_START_MOTION_VERSUS_RANGE, (double)SHRT_MIN, (double)SHRT_MAX, 1.0, -1.0},
00088 {RS_OPTION_SR300_AUTO_RANGE_MIN_LASER, (double)SHRT_MIN, (double)SHRT_MAX, 1.0, -1.0},
00089 {RS_OPTION_SR300_AUTO_RANGE_MAX_LASER, (double)SHRT_MIN, (double)SHRT_MAX, 1.0, -1.0},
00090 {RS_OPTION_SR300_AUTO_RANGE_START_LASER, (double)SHRT_MIN, (double)SHRT_MAX, 1.0, -1.0},
00091 {RS_OPTION_SR300_AUTO_RANGE_UPPER_THRESHOLD, 0.0, (double)USHRT_MAX, 1.0, -1.0},
00092 {RS_OPTION_SR300_AUTO_RANGE_LOWER_THRESHOLD, 0.0, (double)USHRT_MAX, 1.0, -1.0},
00093 {RS_OPTION_HARDWARE_LOGGER_ENABLED, 0, 1, 1, 0 }
00094 };
00095
00096
00097
00098
00099
00100 info.options.push_back({ RS_OPTION_F200_LASER_POWER, 0, 16, 1, 16 });
00101 info.options.push_back({ RS_OPTION_F200_ACCURACY, 1, 3, 1, 1 });
00102 info.options.push_back({ RS_OPTION_F200_MOTION_RANGE, 0, 220, 1, 9 });
00103 info.options.push_back({ RS_OPTION_F200_FILTER_OPTION, 0, 7, 1, 5 });
00104 info.options.push_back({ RS_OPTION_F200_CONFIDENCE_THRESHOLD, 0, 15, 1, 3 });
00105
00106 rsimpl::pose depth_to_color = {transpose((const float3x3 &)c.Rt), (const float3 &)c.Tt * 0.001f};
00107 info.stream_poses[RS_STREAM_DEPTH] = info.stream_poses[RS_STREAM_INFRARED] = inverse(depth_to_color);
00108 info.stream_poses[RS_STREAM_COLOR] = {{{1,0,0},{0,1,0},{0,0,1}}, {0,0,0}};
00109
00110 info.nominal_depth_scale = (c.Rmax / 0xFFFF) * 0.001f;
00111 info.num_libuvc_transfer_buffers = 1;
00112 rs_device_base::update_device_info(info);
00113 return info;
00114 }
00115
00116 sr300_camera::sr300_camera(std::shared_ptr<uvc::device> device, const static_device_info & info, const ivcam::camera_calib_params & calib) :
00117 iv_camera(device, info, calib)
00118 {
00119
00120 arr.enableMvR = 1;
00121 arr.enableLaser = 1;
00122 arr.minMvR = 180;
00123 arr.maxMvR = 605;
00124 arr.startMvR = 303;
00125 arr.minLaser = 2;
00126 arr.maxLaser = 16;
00127 arr.startLaser = -1;
00128 arr.ARUpperTh = 1250;
00129 arr.ARLowerTh = 650;
00130 }
00131
00132 void sr300_camera::start_fw_logger(char fw_log_op_code, int grab_rate_in_ms, std::timed_mutex& mutex)
00133 {
00134 iv_camera::start_fw_logger(fw_log_op_code, grab_rate_in_ms, mutex);
00135 }
00136
00137 void sr300_camera::stop_fw_logger()
00138 {
00139 iv_camera::stop_fw_logger();
00140 }
00141
00142 void sr300_camera::set_fw_logger_option(double value)
00143 {
00144 if (value >= 1)
00145 {
00146 if (!rs_device_base::keep_fw_logger_alive)
00147 start_fw_logger(0x35, 100, usbMutex);
00148 }
00149 else
00150 {
00151 if (rs_device_base::keep_fw_logger_alive)
00152 stop_fw_logger();
00153 }
00154 }
00155
00156 unsigned sr300_camera::get_fw_logger_option()
00157 {
00158 return rs_device_base::keep_fw_logger_alive;
00159 }
00160
00161 void sr300_camera::set_options(const rs_option options[], size_t count, const double values[])
00162 {
00163 std::vector<rs_option> base_opt;
00164 std::vector<double> base_opt_val;
00165
00166 auto arr_writer = make_struct_interface<ivcam::cam_auto_range_request>([this]() { return arr; }, [this](ivcam::cam_auto_range_request r) {
00167 ivcam::set_auto_range(get_device(), usbMutex, r.enableMvR, r.minMvR, r.maxMvR, r.startMvR, r.enableLaser, r.minLaser, r.maxLaser, r.startLaser, r.ARUpperTh, r.ARLowerTh);
00168 arr = r;
00169 });
00170
00171 for(size_t i=0; i<count; ++i)
00172 {
00173 switch(options[i])
00174 {
00175 case RS_OPTION_SR300_AUTO_RANGE_ENABLE_MOTION_VERSUS_RANGE: arr_writer.set(&ivcam::cam_auto_range_request::enableMvR, values[i]); break;
00176 case RS_OPTION_SR300_AUTO_RANGE_ENABLE_LASER: arr_writer.set(&ivcam::cam_auto_range_request::enableLaser, values[i]); break;
00177 case RS_OPTION_SR300_AUTO_RANGE_MIN_MOTION_VERSUS_RANGE: arr_writer.set(&ivcam::cam_auto_range_request::minMvR, values[i]); break;
00178 case RS_OPTION_SR300_AUTO_RANGE_MAX_MOTION_VERSUS_RANGE: arr_writer.set(&ivcam::cam_auto_range_request::maxMvR, values[i]); break;
00179 case RS_OPTION_SR300_AUTO_RANGE_START_MOTION_VERSUS_RANGE: arr_writer.set(&ivcam::cam_auto_range_request::startMvR, values[i]); break;
00180 case RS_OPTION_SR300_AUTO_RANGE_MIN_LASER: arr_writer.set(&ivcam::cam_auto_range_request::minLaser, values[i]); break;
00181 case RS_OPTION_SR300_AUTO_RANGE_MAX_LASER: arr_writer.set(&ivcam::cam_auto_range_request::maxLaser, values[i]); break;
00182 case RS_OPTION_SR300_AUTO_RANGE_START_LASER: arr_writer.set(&ivcam::cam_auto_range_request::startLaser, values[i]); break;
00183 case RS_OPTION_SR300_AUTO_RANGE_UPPER_THRESHOLD: arr_writer.set(&ivcam::cam_auto_range_request::ARUpperTh, values[i]); break;
00184 case RS_OPTION_SR300_AUTO_RANGE_LOWER_THRESHOLD: arr_writer.set(&ivcam::cam_auto_range_request::ARLowerTh, values[i]); break;
00185 case RS_OPTION_HARDWARE_LOGGER_ENABLED: set_fw_logger_option(values[i]); break;
00186
00187
00188 default: base_opt.push_back(options[i]); base_opt_val.push_back(values[i]); break;
00189 }
00190 }
00191
00192 arr_writer.commit();
00193
00194
00195 if (base_opt.size())
00196 iv_camera::set_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00197 }
00198
00199 void sr300_camera::get_options(const rs_option options[], size_t count, double values[])
00200 {
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 arr_reader = make_struct_interface<ivcam::cam_auto_range_request>([this]() { return arr; }, [this](ivcam::cam_auto_range_request) {});
00207
00208
00209 for(size_t i=0; i<count; ++i)
00210 {
00211 LOG_INFO("Reading option " << options[i]);
00212
00213 if(uvc::is_pu_control(options[i]))
00214 {
00215 values[i] = uvc::get_pu_control_with_retry(get_device(), 0, options[i]);
00216 continue;
00217 }
00218
00219 switch(options[i])
00220 {
00221 case RS_OPTION_SR300_AUTO_RANGE_ENABLE_MOTION_VERSUS_RANGE: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::enableMvR); break;
00222 case RS_OPTION_SR300_AUTO_RANGE_ENABLE_LASER: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::enableLaser); break;
00223 case RS_OPTION_SR300_AUTO_RANGE_MIN_MOTION_VERSUS_RANGE: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::minMvR); break;
00224 case RS_OPTION_SR300_AUTO_RANGE_MAX_MOTION_VERSUS_RANGE: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::maxMvR); break;
00225 case RS_OPTION_SR300_AUTO_RANGE_START_MOTION_VERSUS_RANGE: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::startMvR); break;
00226 case RS_OPTION_SR300_AUTO_RANGE_MIN_LASER: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::minLaser); break;
00227 case RS_OPTION_SR300_AUTO_RANGE_MAX_LASER: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::maxLaser); break;
00228 case RS_OPTION_SR300_AUTO_RANGE_START_LASER: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::startLaser); break;
00229 case RS_OPTION_SR300_AUTO_RANGE_UPPER_THRESHOLD: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::ARUpperTh); break;
00230 case RS_OPTION_SR300_AUTO_RANGE_LOWER_THRESHOLD: values[i] = arr_reader.get(&ivcam::cam_auto_range_request::ARLowerTh); break;
00231 case RS_OPTION_HARDWARE_LOGGER_ENABLED: values[i] = get_fw_logger_option(); break;
00232
00233
00234 default: base_opt.push_back(options[i]); base_opt_index.push_back(i); break;
00235 }
00236 }
00237
00238
00239 if (base_opt.size())
00240 {
00241 base_opt_val.resize(base_opt.size());
00242 iv_camera::get_options(base_opt.data(), base_opt.size(), base_opt_val.data());
00243 }
00244
00245
00246 for (auto i : base_opt_index)
00247 values[i] = base_opt_val[i];
00248 }
00249
00250 std::shared_ptr<rs_device> make_sr300_device(std::shared_ptr<uvc::device> device)
00251 {
00252 std::timed_mutex mutex;
00253 ivcam::claim_ivcam_interface(*device);
00254 auto calib = sr300::read_sr300_calibration(*device, mutex);
00255 ivcam::enable_timestamp(*device, mutex, true, true);
00256
00257 uvc::set_pu_control_with_retry(*device, 0, rs_option::RS_OPTION_COLOR_BACKLIGHT_COMPENSATION, 0);
00258 uvc::set_pu_control_with_retry(*device, 0, rs_option::RS_OPTION_COLOR_BRIGHTNESS, 0);
00259 uvc::set_pu_control_with_retry(*device, 0, rs_option::RS_OPTION_COLOR_CONTRAST, 50);
00260 uvc::set_pu_control_with_retry(*device, 0, rs_option::RS_OPTION_COLOR_GAMMA, 300);
00261 uvc::set_pu_control_with_retry(*device, 0, rs_option::RS_OPTION_COLOR_HUE, 0);
00262 uvc::set_pu_control_with_retry(*device, 0, rs_option::RS_OPTION_COLOR_SATURATION, 64);
00263 uvc::set_pu_control_with_retry(*device, 0, rs_option::RS_OPTION_COLOR_SHARPNESS, 50);
00264 uvc::set_pu_control_with_retry(*device, 0, rs_option::RS_OPTION_COLOR_GAIN, 64);
00265
00266
00267
00268 auto info = get_sr300_info(device, calib);
00269
00270 ivcam::get_module_serial_string(*device, mutex, info.serial, 132);
00271 ivcam::get_firmware_version_string(*device, mutex, info.firmware_version);
00272
00273 info.camera_info[RS_CAMERA_INFO_CAMERA_FIRMWARE_VERSION] = info.firmware_version;
00274 info.camera_info[RS_CAMERA_INFO_DEVICE_SERIAL_NUMBER] = info.serial;
00275 info.camera_info[RS_CAMERA_INFO_DEVICE_NAME] = info.name;
00276
00277 info.capabilities_vector.push_back(RS_CAPABILITIES_ENUMERATION);
00278 info.capabilities_vector.push_back(RS_CAPABILITIES_COLOR);
00279 info.capabilities_vector.push_back(RS_CAPABILITIES_DEPTH);
00280 info.capabilities_vector.push_back(RS_CAPABILITIES_INFRARED);
00281
00282 return std::make_shared<sr300_camera>(device, info, calib);
00283 }
00284
00285 }