00001
00002
00003
00004 #ifdef RS_USE_LIBUVC_BACKEND
00005
00006
00007
00008 #include "uvc.h"
00009 #include "libuvc/libuvc.h"
00010 #include "libuvc/libuvc_internal.h"
00011 #include <thread>
00012
00013 namespace rsimpl
00014 {
00015 namespace uvc
00016 {
00017 static void check(const char * call, uvc_error_t status)
00018 {
00019 if (status < 0) throw std::runtime_error(to_string() << call << "(...) returned " << uvc_strerror(status));
00020 }
00021 #define CALL_UVC(name, ...) check(#name, name(__VA_ARGS__))
00022
00023 struct context
00024 {
00025 uvc_context_t * ctx;
00026 libusb_context * usb_context;
00027
00028 context() : ctx()
00029 {
00030 check("uvc_init", uvc_init(&ctx, nullptr));
00031
00032 int status = libusb_init(&usb_context);
00033 if (status < 0) throw std::runtime_error(to_string() << "libusb_init(...) returned " << libusb_error_name(status));
00034 }
00035 ~context()
00036 {
00037 libusb_exit(usb_context);
00038 if (ctx) uvc_exit(ctx);
00039 }
00040 };
00041
00042 struct subdevice
00043 {
00044 uvc_device_handle_t * handle = nullptr;
00045 uvc_stream_ctrl_t ctrl;
00046 uint8_t unit;
00047 video_channel_callback callback = nullptr;
00048 data_channel_callback channel_data_callback = nullptr;
00049
00050 void set_data_channel_cfg(data_channel_callback callback)
00051 {
00052 this->channel_data_callback = callback;
00053 }
00054
00055 static void poll_interrupts(libusb_device_handle *handle, const std::vector<subdevice *> & subdevices, uint16_t timeout)
00056 {
00057 static const unsigned short interrupt_buf_size = 0x400;
00058 uint8_t buffer[interrupt_buf_size];
00059 int num_bytes = 0;
00060
00061
00062 int res = libusb_interrupt_transfer(handle, 0x84, buffer, interrupt_buf_size, &num_bytes, timeout);
00063 if (0 == res)
00064 {
00065
00066 for (auto & sub : subdevices)
00067 if (sub->channel_data_callback)
00068 sub->channel_data_callback(buffer, num_bytes);
00069 }
00070 else
00071 {
00072 if (res == LIBUSB_ERROR_TIMEOUT)
00073 LOG_WARNING("interrupt e.p. timeout");
00074 else
00075 throw std::runtime_error(to_string() << "USB Interrupt end-point error " << libusb_strerror((libusb_error)res));
00076 }
00077 }
00078
00079 };
00080
00081 struct device
00082 {
00083 const std::shared_ptr<context> parent;
00084 uvc_device_t * uvcdevice;
00085 int vid, pid;
00086 std::vector<subdevice> subdevices;
00087 std::vector<int> claimed_interfaces;
00088
00089 std::thread data_channel_thread;
00090 volatile bool data_stop;
00091
00092
00093 std::shared_ptr<device> aux_device;
00094
00095 libusb_device_handle * usb_handle;
00096
00097 device(std::shared_ptr<context> parent, uvc_device_t * uvcdevice) : parent(parent), uvcdevice(uvcdevice), usb_handle()
00098 {
00099 get_subdevice(0);
00100
00101 uvc_device_descriptor_t * desc;
00102 CALL_UVC(uvc_get_device_descriptor, uvcdevice, &desc);
00103 vid = desc->idVendor;
00104 pid = desc->idProduct;
00105 uvc_free_device_descriptor(desc);
00106 }
00107 ~device()
00108 {
00109 for(auto interface_number : claimed_interfaces)
00110 {
00111 int status = libusb_release_interface(get_subdevice(0).handle->usb_devh, interface_number);
00112 if(status < 0) LOG_ERROR("libusb_release_interface(...) returned " << libusb_error_name(status));
00113 }
00114
00115 for(auto & sub : subdevices) if(sub.handle) uvc_close(sub.handle);
00116 if(claimed_interfaces.size()) if(uvcdevice) uvc_unref_device(uvcdevice);
00117 }
00118
00119 subdevice & get_subdevice(int subdevice_index)
00120 {
00121 if (subdevice_index >= subdevices.size()) subdevices.resize(subdevice_index + 1);
00122
00123 if (subdevice_index == 3 && aux_device)
00124 {
00125 auto& sub = aux_device->get_subdevice(0);
00126 subdevices[subdevice_index] = sub;
00127 return sub;
00128 }
00129
00130 if (!subdevices[subdevice_index].handle) check("uvc_open2", uvc_open2(uvcdevice, &subdevices[subdevice_index].handle, subdevice_index));
00131 return subdevices[subdevice_index];
00132 }
00133
00134 void start_data_acquisition()
00135 {
00136 data_stop = false;
00137 std::vector<subdevice *> data_channel_subs;
00138 for (auto & sub : subdevices)
00139 {
00140 if (sub.channel_data_callback)
00141 {
00142 data_channel_subs.push_back(&sub);
00143 }
00144 }
00145
00146
00147 if (claimed_interfaces.size())
00148 {
00149 data_channel_thread = std::thread([this, data_channel_subs]()
00150 {
00151
00152 while (!data_stop)
00153 {
00154 subdevice::poll_interrupts(this->usb_handle, data_channel_subs, 100);
00155 }
00156 });
00157 }
00158 }
00159
00160 void stop_data_acquisition()
00161 {
00162 if (data_channel_thread.joinable())
00163 {
00164 data_stop = true;
00165 data_channel_thread.join();
00166 data_stop = false;
00167 }
00168 }
00169 };
00170
00172
00174
00175 int get_vendor_id(const device & device) { return device.vid; }
00176 int get_product_id(const device & device) { return device.pid; }
00177
00178 std::string get_usb_port_id(const device & device)
00179 {
00180 std::string usb_port = std::to_string(libusb_get_bus_number(device.uvcdevice->usb_dev)) + "-" +
00181 std::to_string(libusb_get_port_number(device.uvcdevice->usb_dev));
00182 return usb_port;
00183 }
00184
00185 void get_control(const device & dev, const extension_unit & xu, uint8_t ctrl, void * data, int len)
00186 {
00187 int status = uvc_get_ctrl(const_cast<device &>(dev).get_subdevice(xu.subdevice).handle, xu.unit, ctrl, data, len, UVC_GET_CUR);
00188 if(status < 0) throw std::runtime_error(to_string() << "uvc_get_ctrl(...) returned " << libusb_error_name(status));
00189 }
00190
00191 void set_control(device & device, const extension_unit & xu, uint8_t ctrl, void * data, int len)
00192 {
00193 int status = uvc_set_ctrl(device.get_subdevice(xu.subdevice).handle, xu.unit, ctrl, data, len);
00194 if(status < 0) throw std::runtime_error(to_string() << "uvc_set_ctrl(...) returned " << libusb_error_name(status));
00195 }
00196
00197 void claim_interface(device & device, const guid & interface_guid, int interface_number)
00198 {
00199 libusb_device_handle* dev_h = nullptr;
00200
00201 if (device.pid == ZR300_CX3_PID)
00202 {
00203 dev_h = device.usb_handle;
00204 }
00205 else
00206 {
00207 dev_h = device.get_subdevice(0).handle->usb_devh;
00208 }
00209 int status = libusb_claim_interface(dev_h, interface_number);
00210 if (status < 0) throw std::runtime_error(to_string() << "libusb_claim_interface(...) returned " << libusb_error_name(status));
00211 device.claimed_interfaces.push_back(interface_number);
00212 }
00213
00214 void claim_aux_interface(device & device, const guid & interface_guid, int interface_number)
00215 {
00216 claim_interface(device, interface_guid, interface_number);
00217 }
00218
00219 void bulk_transfer(device & device, unsigned char endpoint, void * data, int length, int *actual_length, unsigned int timeout)
00220 {
00221
00222 libusb_device_handle* dev_h = nullptr;
00223
00224 if (device.pid == ZR300_CX3_PID)
00225 {
00226 dev_h = device.usb_handle;
00227 }
00228 else
00229 {
00230 dev_h = device.get_subdevice(0).handle->usb_devh;
00231 }
00232
00233 int status = libusb_bulk_transfer(dev_h, endpoint, (unsigned char *)data, length, actual_length, timeout);
00234
00235 if (status < 0) throw std::runtime_error(to_string() << "libusb_bulk_transfer(...) returned " << libusb_error_name(status));
00236
00237 }
00238
00239 void set_subdevice_mode(device & device, int subdevice_index, int width, int height, uint32_t fourcc, int fps, video_channel_callback callback)
00240 {
00241 auto & sub = device.get_subdevice(subdevice_index);
00242 check("get_stream_ctrl_format_size", uvc_get_stream_ctrl_format_size(sub.handle, &sub.ctrl, reinterpret_cast<const big_endian<uint32_t> &>(fourcc), width, height, fps));
00243 sub.callback = callback;
00244 }
00245
00246 void set_subdevice_data_channel_handler(device & device, int subdevice_index, data_channel_callback callback)
00247 {
00248 device.subdevices[subdevice_index].set_data_channel_cfg(callback);
00249 }
00250
00251 void start_streaming(device & device, int num_transfer_bufs)
00252 {
00253 for(auto i = 0; i < device.subdevices.size(); i++)
00254 {
00255 auto& sub = device.get_subdevice(i);
00256 if (sub.callback)
00257 {
00258 #if defined (ENABLE_DEBUG_SPAM)
00259 uvc_print_stream_ctrl(&sub.ctrl, stdout);
00260 #endif
00261
00262 check("uvc_start_streaming", uvc_start_streaming(sub.handle, &sub.ctrl, [](uvc_frame * frame, void * user)
00263 {
00264 reinterpret_cast<subdevice *>(user)->callback(frame->data, []{});
00265 }, &sub, 0, num_transfer_bufs));
00266 }
00267 }
00268 }
00269
00270 void stop_streaming(device & device)
00271 {
00272
00273 for(auto & sub : device.subdevices)
00274 {
00275 if(sub.handle) uvc_stop_streaming(sub.handle);
00276 sub.ctrl = {};
00277 sub.callback = {};
00278 }
00279 }
00280
00281 void start_data_acquisition(device & device)
00282 {
00283 device.start_data_acquisition();
00284 }
00285
00286 void stop_data_acquisition(device & device)
00287 {
00288 device.stop_data_acquisition();
00289 }
00290
00291 template<class T> void set_pu(uvc_device_handle_t * devh, int subdevice, uint8_t unit, uint8_t control, int value)
00292 {
00293 const int REQ_TYPE_SET = 0x21;
00294 unsigned char buffer[4];
00295 if(sizeof(T)==1) buffer[0] = value;
00296 if(sizeof(T)==2) SHORT_TO_SW(value, buffer);
00297 if(sizeof(T)==4) INT_TO_DW(value, buffer);
00298 int status = libusb_control_transfer(devh->usb_devh, REQ_TYPE_SET, UVC_SET_CUR, control << 8, unit << 8 | (subdevice * 2), buffer, sizeof(T), 0);
00299 if(status < 0) throw std::runtime_error(to_string() << "libusb_control_transfer(...) returned " << libusb_error_name(status));
00300 if(status != sizeof(T)) throw std::runtime_error("insufficient data written to usb");
00301 }
00302
00303 template<class T> int get_pu(uvc_device_handle_t * devh, int subdevice, uint8_t unit, uint8_t control, int uvc_get_thing)
00304 {
00305 const int REQ_TYPE_GET = 0xa1;
00306 unsigned char buffer[4];
00307 int status = libusb_control_transfer(devh->usb_devh, REQ_TYPE_GET, uvc_get_thing, control << 8, unit << 8 | (subdevice * 2), buffer, sizeof(T), 0);
00308 if(status < 0) throw std::runtime_error(to_string() << "libusb_control_transfer(...) returned " << libusb_error_name(status));
00309 if(status != sizeof(T)) throw std::runtime_error("insufficient data read from usb");
00310 if(sizeof(T)==1) return buffer[0];
00311 if(sizeof(T)==2) return SW_TO_SHORT(buffer);
00312 if(sizeof(T)==4) return DW_TO_INT(buffer);
00313 }
00314 template<class T> void get_pu_range(uvc_device_handle_t * devh, int subdevice, uint8_t unit, uint8_t control, int * min, int * max, int * step, int * def)
00315 {
00316 if(min) *min = get_pu<T>(devh, subdevice, unit, control, UVC_GET_MIN);
00317 if(max) *max = get_pu<T>(devh, subdevice, unit, control, UVC_GET_MAX);
00318 if(step) *step = get_pu<T>(devh, subdevice, unit, control, UVC_GET_RES);
00319 if(def) *def = get_pu<T>(devh, subdevice, unit, control, UVC_GET_DEF);
00320 }
00321
00322 void get_pu_control_range(const device & device, int subdevice, rs_option option, int * min, int * max, int * step, int * def)
00323 {
00324 auto handle = const_cast<uvc::device &>(device).get_subdevice(subdevice).handle;
00325 int ct_unit = 0, pu_unit = 0;
00326 for(auto ct = uvc_get_input_terminals(handle); ct; ct = ct->next) ct_unit = ct->bTerminalID;
00327 for(auto pu = uvc_get_processing_units(handle); pu; pu = pu->next) pu_unit = pu->bUnitID;
00328
00329 switch(option)
00330 {
00331 case RS_OPTION_COLOR_BACKLIGHT_COMPENSATION: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_BACKLIGHT_COMPENSATION_CONTROL, min, max, step, def);
00332 case RS_OPTION_COLOR_BRIGHTNESS: return get_pu_range<int16_t>(handle, subdevice, pu_unit, UVC_PU_BRIGHTNESS_CONTROL, min, max, step, def);
00333 case RS_OPTION_COLOR_CONTRAST: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_CONTRAST_CONTROL, min, max, step, def);
00334 case RS_OPTION_COLOR_EXPOSURE: return get_pu_range<uint32_t>(handle, subdevice, ct_unit, UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL, min, max, step, def);
00335 case RS_OPTION_COLOR_GAIN: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAIN_CONTROL, min, max, step, def);
00336 case RS_OPTION_COLOR_GAMMA: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAMMA_CONTROL, min, max, step, def);
00337 case RS_OPTION_COLOR_HUE: if(min) *min = 0; if(max) *max = 0; return;
00338 case RS_OPTION_COLOR_SATURATION: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SATURATION_CONTROL, min, max, step, def);
00339 case RS_OPTION_COLOR_SHARPNESS: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SHARPNESS_CONTROL, min, max, step, def);
00340 case RS_OPTION_COLOR_WHITE_BALANCE: return get_pu_range<uint16_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL, min, max, step, def);
00341 case RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE: if(min) *min = 0; if(max) *max = 1; if (step) *step = 1; if (def) *def = 1; return;
00342 case RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE: if(min) *min = 0; if(max) *max = 1; if (step) *step = 1; if (def) *def = 1; return;
00343 default: throw std::logic_error("invalid option");
00344 }
00345 }
00346
00347 void get_extension_control_range(const device & device, const extension_unit & xu, char control, int * min, int * max, int * step, int * def)
00348 {
00349 throw std::logic_error("get_extension_control_range(...) is not implemented for this backend");
00350 }
00351
00352 void set_pu_control(device & device, int subdevice, rs_option option, int value)
00353 {
00354 auto handle = device.get_subdevice(subdevice).handle;
00355 int ct_unit = 0, pu_unit = 0;
00356 for(auto ct = uvc_get_input_terminals(handle); ct; ct = ct->next) ct_unit = ct->bTerminalID;
00357 for(auto pu = uvc_get_processing_units(handle); pu; pu = pu->next) pu_unit = pu->bUnitID;
00358
00359 switch(option)
00360 {
00361 case RS_OPTION_COLOR_BACKLIGHT_COMPENSATION: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_BACKLIGHT_COMPENSATION_CONTROL, value);
00362 case RS_OPTION_COLOR_BRIGHTNESS: return set_pu<int16_t>(handle, subdevice, pu_unit, UVC_PU_BRIGHTNESS_CONTROL, value);
00363 case RS_OPTION_COLOR_CONTRAST: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_CONTRAST_CONTROL, value);
00364 case RS_OPTION_COLOR_EXPOSURE: return set_pu<uint32_t>(handle, subdevice, ct_unit, UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL, value);
00365 case RS_OPTION_COLOR_GAIN: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAIN_CONTROL, value);
00366 case RS_OPTION_COLOR_GAMMA: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAMMA_CONTROL, value);
00367 case RS_OPTION_COLOR_HUE: return;
00368 case RS_OPTION_COLOR_SATURATION: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SATURATION_CONTROL, value);
00369 case RS_OPTION_COLOR_SHARPNESS: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SHARPNESS_CONTROL, value);
00370 case RS_OPTION_COLOR_WHITE_BALANCE: return set_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL, value);
00371 case RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE: return set_pu<uint8_t>(handle, subdevice, ct_unit, UVC_CT_AE_MODE_CONTROL, value ? 2 : 1);
00372 case RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE: return set_pu<uint8_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL, value);
00373 case RS_OPTION_FISHEYE_GAIN: {
00374 assert(subdevice == 3);
00375 return set_pu<uint16_t>(handle, 0, pu_unit, UVC_PU_GAIN_CONTROL, value);
00376 }
00377 default: throw std::logic_error("invalid option");
00378 }
00379 }
00380
00381 int get_pu_control(const device & device, int subdevice, rs_option option)
00382 {
00383 auto handle = const_cast<uvc::device &>(device).get_subdevice(subdevice).handle;
00384 int ct_unit = 0, pu_unit = 0;
00385 for(auto ct = uvc_get_input_terminals(handle); ct; ct = ct->next) ct_unit = ct->bTerminalID;
00386 for(auto pu = uvc_get_processing_units(handle); pu; pu = pu->next) pu_unit = pu->bUnitID;
00387
00388 switch(option)
00389 {
00390 case RS_OPTION_COLOR_BACKLIGHT_COMPENSATION: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_BACKLIGHT_COMPENSATION_CONTROL, UVC_GET_CUR);
00391 case RS_OPTION_COLOR_BRIGHTNESS: return get_pu<int16_t>(handle, subdevice, pu_unit, UVC_PU_BRIGHTNESS_CONTROL, UVC_GET_CUR);
00392 case RS_OPTION_COLOR_CONTRAST: return get_pu<uint16_t>(handle, subdevice, pu_unit,UVC_PU_CONTRAST_CONTROL, UVC_GET_CUR);
00393 case RS_OPTION_COLOR_EXPOSURE: return get_pu<uint32_t>(handle, subdevice, ct_unit, UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL, UVC_GET_CUR);
00394 case RS_OPTION_COLOR_GAIN: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAIN_CONTROL, UVC_GET_CUR);
00395 case RS_OPTION_COLOR_GAMMA: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAMMA_CONTROL, UVC_GET_CUR);
00396 case RS_OPTION_COLOR_HUE: return 0;
00397 case RS_OPTION_COLOR_SATURATION: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SATURATION_CONTROL, UVC_GET_CUR);
00398 case RS_OPTION_COLOR_SHARPNESS: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_SHARPNESS_CONTROL, UVC_GET_CUR);
00399 case RS_OPTION_COLOR_WHITE_BALANCE: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL, UVC_GET_CUR);
00400 case RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE: return get_pu<uint8_t>(handle, subdevice, ct_unit, UVC_CT_AE_MODE_CONTROL, UVC_GET_CUR) > 1;
00401 case RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE: return get_pu<uint8_t>(handle, subdevice, pu_unit, UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL, UVC_GET_CUR);
00402 case RS_OPTION_FISHEYE_GAIN: return get_pu<uint16_t>(handle, subdevice, pu_unit, UVC_PU_GAIN_CONTROL, UVC_GET_CUR);
00403 default: throw std::logic_error("invalid option");
00404 }
00405 }
00406
00408
00410
00411 std::shared_ptr<context> create_context()
00412 {
00413 return std::make_shared<context>();
00414 }
00415
00416 bool is_device_connected(device & device, int vid, int pid)
00417 {
00418 return true;
00419 }
00420
00421 std::vector<std::shared_ptr<device>> query_devices(std::shared_ptr<context> context)
00422 {
00423 std::vector<std::shared_ptr<device>> devices;
00424
00425 uvc_device_t ** list;
00426 CALL_UVC(uvc_get_device_list, context->ctx, &list);
00427 for (auto it = list; *it; ++it)
00428 try
00429 {
00430 auto dev = std::make_shared<device>(context, *it);
00431 dev->usb_handle = libusb_open_device_with_vid_pid(context->usb_context, VID_INTEL_CAMERA, ZR300_FISHEYE_PID);
00432 devices.push_back(dev);
00433 }
00434 catch (std::runtime_error &e)
00435 {
00436 LOG_WARNING("usb:" << (int)uvc_get_bus_number(*it) << ':' << (int)uvc_get_device_address(*it) << ": " << e.what());
00437 }
00438 uvc_free_device_list(list, 1);
00439
00440 std::shared_ptr<device> fisheye = nullptr;
00441
00442 for (auto& dev : devices)
00443 {
00444 if (dev->pid == ZR300_FISHEYE_PID)
00445 {
00446 fisheye = dev;
00447 }
00448 }
00449
00450 for (auto& dev : devices)
00451 {
00452 dev->aux_device = fisheye;
00453 }
00454
00455 return devices;
00456 }
00457 }
00458 }
00459
00460 #endif