Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * v4l2.cpp - Video4Linux 2 camera access 00004 * 00005 * Created: Sat Jul 5 20:40:20 2008 00006 * Copyright 2008 Tobias Kellner 00007 * 2010 Tim Niemueller 00008 * 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. A runtime exception applies to 00015 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00016 * 00017 * This program is distributed in the hope that it will be useful, 00018 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00019 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00020 * GNU Library General Public License for more details. 00021 * 00022 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00023 */ 00024 00025 #include <cams/v4l2.h> 00026 00027 #include <core/exception.h> 00028 #include <core/exceptions/software.h> 00029 #include <utils/logging/liblogger.h> 00030 #include <fvutils/system/camargp.h> 00031 00032 #include <fcntl.h> 00033 #include <sys/ioctl.h> 00034 #include <sys/mman.h> 00035 #include <iostream> 00036 #include <cstring> 00037 #include <cerrno> 00038 #include <cstdlib> 00039 #include <linux/version.h> 00040 00041 using std::cout; 00042 using std::endl; 00043 using std::string; 00044 using fawkes::Exception; 00045 using fawkes::MissingParameterException; 00046 using fawkes::NotImplementedException; 00047 using fawkes::LibLogger; 00048 00049 #ifdef HAVE_LIBV4L2 00050 # include <libv4l2.h> 00051 #else 00052 # define v4l2_fd_open(fd, flags) (fd) 00053 # define v4l2_close ::close 00054 # define v4l2_dup dup 00055 # define v4l2_ioctl ioctl 00056 # define v4l2_read read 00057 # define v4l2_mmap mmap 00058 # define v4l2_munmap munmap 00059 #endif 00060 00061 namespace firevision { 00062 #if 0 /* just to make Emacs auto-indent happy */ 00063 } 00064 #endif 00065 00066 /// @cond INTERNALS 00067 class V4L2CameraData 00068 { 00069 public: 00070 v4l2_capability caps; //< Device capabilites 00071 }; 00072 00073 /// @endcond 00074 00075 00076 /** @class V4L2Camera <cams/v4l2.h> 00077 * Video4Linux 2 camera access implementation. 00078 * 00079 * @todo UPTR method 00080 * @todo Standards queries (VIDIOC_ENUMSTD) 00081 * @todo v4l2_pix_format.field 00082 * @author Tobias Kellner 00083 */ 00084 00085 00086 /** Constructor. 00087 * @param device_name device file name (e.g. /dev/video0) 00088 */ 00089 V4L2Camera::V4L2Camera(const char *device_name) 00090 { 00091 _opened = _started = false; 00092 _nao_hacks = _switch_u_v = false; 00093 _width = _height = _bytes_per_line = _fps = _buffers_length = 0; 00094 _current_buffer = -1; 00095 _brightness.set = _contrast.set = _saturation.set = _hue.set = 00096 _red_balance.set = _blue_balance.set = _exposure.set = _gain.set = 00097 _lens_x.set = _lens_y.set = false; 00098 _aec = _awb = _agc = _h_flip = _v_flip = NOT_SET; 00099 _read_method = MMAP; 00100 memset(_format, 0, 5); 00101 _frame_buffers = NULL; 00102 _capture_time = NULL; 00103 _device_name = strdup(device_name); 00104 _data = new V4L2CameraData(); 00105 } 00106 00107 00108 /** Constructor. 00109 * Initialize camera with parameters from camera argument parser. 00110 * Supported arguments: 00111 * *Required: 00112 * - device=DEV, device file, for example /dev/video0 (required) 00113 * *Optional: 00114 * - read_method=METHOD, preferred read method 00115 * READ: read() 00116 * MMAP: memory mapping 00117 * UPTR: user pointer 00118 * - format=FOURCC, preferred format 00119 * - size=WIDTHxHEIGHT, preferred image size 00120 * - switch_u_v=true/false, switch U and V channels 00121 * - fps=FPS, frames per second 00122 * - aec=true/false, Auto Exposition enabled [warning: only valid on nao] 00123 * - awb=true/false, Auto White Balance enabled 00124 * - agc=true/false, Auto Gain enabled 00125 * - h_flip=true/false, Horizontal mirror 00126 * - v_flip=true/false, Vertical mirror 00127 * - brightness=BRIGHT, Brightness [0-255] (def. 128) 00128 * - contrast=CONTR, Contrast [0-127] (def. 64) 00129 * - saturation=SAT, Saturation [0-256] (def. 128) 00130 * - hue=HUE, Hue [-180-180] (def. 0) 00131 * - red_balance=RB, Red Balance [0-255] (def. 128) 00132 * - blue_balance=BB, Blue Balance [0-255] (def. 128) 00133 * - exposure=EXP, Exposure [0-65535] (def. 60) 00134 * - gain=GAIN, Gain [0-255] (def. 0) 00135 * - lens_x=CORR, Lens Correction X [0-255] (def. 0) 00136 * - lens_y=CORR, Lens Correction Y [0-255] (def. 0) 00137 * @param cap camera argument parser 00138 */ 00139 V4L2Camera::V4L2Camera(const CameraArgumentParser *cap) 00140 { 00141 _opened = _started = false; 00142 _nao_hacks = false; 00143 _width = _height = _bytes_per_line = _buffers_length = 0; 00144 _current_buffer = -1; 00145 _frame_buffers = NULL; 00146 _capture_time = NULL; 00147 _data = new V4L2CameraData(); 00148 00149 if (cap->has("device")) _device_name = strdup(cap->get("device").c_str()); 00150 else throw MissingParameterException("V4L2Cam: Missing device"); 00151 00152 00153 if (cap->has("read_method")) 00154 { 00155 string rm = cap->get("read_method"); 00156 if (rm.compare("READ") == 0) _read_method = READ; 00157 else if (rm.compare("MMAP") == 0) _read_method = MMAP; 00158 else if (rm.compare("UPTR") == 0) _read_method = UPTR; 00159 else throw Exception("V4L2Cam: Invalid read method"); 00160 } 00161 else 00162 { 00163 _read_method = MMAP; 00164 } 00165 00166 00167 if (cap->has("format")) 00168 { 00169 string fmt = cap->get("format"); 00170 if (fmt.length() != 4) throw Exception("V4L2Cam: Invalid format fourcc"); 00171 strncpy(_format, fmt.c_str(), 4); 00172 _format[4] = '\0'; 00173 } 00174 else 00175 { 00176 memset(_format, 0, 5); 00177 } 00178 00179 00180 if (cap->has("size")) 00181 { 00182 string size = cap->get("size"); 00183 string::size_type pos; 00184 if ((pos = size.find('x')) == string::npos) throw Exception("V4L2Cam: invalid image size string"); 00185 if ((pos == (size.length() - 1))) throw Exception("V4L2Cam: invalid image size string"); 00186 00187 unsigned int mult = 1; 00188 for (string::size_type i = pos - 1; i != string::npos; --i) 00189 { 00190 _width += (size.at(i) - '0') * mult; 00191 mult *= 10; 00192 } 00193 00194 mult = 1; 00195 for (string::size_type i = size.length() - 1; i > pos; --i) 00196 { 00197 _height += (size.at(i) - '0') * mult; 00198 mult *= 10; 00199 } 00200 } 00201 00202 00203 if (cap->has("switch_u_v")) 00204 { 00205 _switch_u_v = (cap->get("switch_u_v").compare("true") == 0); 00206 } 00207 else 00208 { 00209 _switch_u_v = false; 00210 } 00211 00212 00213 if (cap->has("fps")) 00214 { 00215 if ((_fps = atoi(cap->get("fps").c_str())) == 0) throw Exception("V4L2Cam: invalid fps string"); 00216 } 00217 else 00218 { 00219 _fps = 0; 00220 } 00221 00222 00223 if (cap->has("aec")) 00224 { 00225 _aec = (cap->get("aec").compare("true") == 0 ? TRUE : FALSE); 00226 } 00227 else 00228 { 00229 _aec = NOT_SET; 00230 } 00231 00232 00233 if (cap->has("awb")) 00234 { 00235 _awb = (cap->get("awb").compare("true") == 0 ? TRUE : FALSE); 00236 } 00237 else 00238 { 00239 _awb = NOT_SET; 00240 } 00241 00242 00243 if (cap->has("agc")) 00244 { 00245 _agc = (cap->get("agc").compare("true") == 0 ? TRUE : FALSE); 00246 } 00247 else 00248 { 00249 _agc = NOT_SET; 00250 } 00251 00252 00253 if (cap->has("h_flip")) 00254 { 00255 _h_flip = (cap->get("h_flip").compare("true") == 0 ? TRUE : FALSE); 00256 } 00257 else 00258 { 00259 _h_flip = NOT_SET; 00260 } 00261 00262 00263 if (cap->has("v_flip")) 00264 { 00265 _v_flip = (cap->get("v_flip").compare("true") == 0 ? TRUE : FALSE); 00266 } 00267 else 00268 { 00269 _v_flip = NOT_SET; 00270 } 00271 00272 00273 if (cap->has("brightness")) 00274 { 00275 _brightness.set = true; 00276 _brightness.value = atoi(cap->get("brightness").c_str()); 00277 } 00278 else 00279 { 00280 _brightness.set = false; 00281 } 00282 00283 00284 if (cap->has("contrast")) 00285 { 00286 _contrast.set = true; 00287 _contrast.value = atoi(cap->get("contrast").c_str()); 00288 } 00289 else 00290 { 00291 _contrast.set = false; 00292 } 00293 00294 00295 if (cap->has("saturation")) 00296 { 00297 _saturation.set = true; 00298 _saturation.value = atoi(cap->get("saturation").c_str()); 00299 } 00300 else 00301 { 00302 _saturation.set = false; 00303 } 00304 00305 00306 if (cap->has("hue")) 00307 { 00308 _hue.set = true; 00309 _hue.value = atoi(cap->get("hue").c_str()); 00310 } 00311 else 00312 { 00313 _hue.set = false; 00314 } 00315 00316 00317 if (cap->has("red_balance")) 00318 { 00319 _red_balance.set = true; 00320 _red_balance.value = atoi(cap->get("red_balance").c_str()); 00321 } 00322 else 00323 { 00324 _red_balance.set = false; 00325 } 00326 00327 00328 if (cap->has("blue_balance")) 00329 { 00330 _blue_balance.set = true; 00331 _blue_balance.value = atoi(cap->get("blue_balance").c_str()); 00332 } 00333 else 00334 { 00335 _blue_balance.set = false; 00336 } 00337 00338 00339 if (cap->has("exposure")) 00340 { 00341 _exposure.set = true; 00342 _exposure.value = atoi(cap->get("exposure").c_str()); 00343 } 00344 else 00345 { 00346 _exposure.set = false; 00347 } 00348 00349 00350 if (cap->has("gain")) 00351 { 00352 _gain.set = true; 00353 _gain.value = atoi(cap->get("gain").c_str()); 00354 } 00355 else 00356 { 00357 _gain.set = false; 00358 } 00359 00360 00361 if (cap->has("lens_x")) 00362 { 00363 _lens_x.set = true; 00364 _lens_x.value = atoi(cap->get("lens_x").c_str()); 00365 } 00366 else 00367 { 00368 _lens_x.set = false; 00369 } 00370 00371 00372 if (cap->has("lens_y")) 00373 { 00374 _lens_y.set = true; 00375 _lens_y.value = atoi(cap->get("lens_y").c_str()); 00376 } 00377 else 00378 { 00379 _lens_y.set = false; 00380 } 00381 } 00382 00383 00384 /** Protected Constructor. 00385 * Gets called from V4LCamera, when the device has already been opened 00386 * and determined to be a V4L2 device. 00387 * @param device_name device file name (e.g. /dev/video0) 00388 * @param dev file descriptor of the opened device 00389 */ 00390 V4L2Camera::V4L2Camera(const char *device_name, int dev) 00391 { 00392 _opened = true; 00393 _started = false; 00394 _nao_hacks = _switch_u_v = false; 00395 _width = _height = _bytes_per_line = _buffers_length = _fps = 0; 00396 _current_buffer = -1; 00397 _brightness.set = _contrast.set = _saturation.set = _hue.set = 00398 _red_balance.set = _blue_balance.set = _exposure.set = _gain.set = 00399 _lens_x.set = _lens_y.set = false; 00400 _aec = _awb = _agc = _h_flip = _v_flip = NOT_SET; 00401 _read_method = UPTR; 00402 memset(_format, 0, 5); 00403 _frame_buffers = NULL; 00404 _capture_time = NULL; 00405 _device_name = strdup(device_name); 00406 _data = new V4L2CameraData(); 00407 00408 _dev = dev; 00409 00410 // getting capabilities 00411 if (v4l2_ioctl(_dev, VIDIOC_QUERYCAP, &_data->caps)) 00412 { 00413 close(); 00414 throw Exception("V4L2Cam: Could not get capabilities - probably not a v4l2 device"); 00415 } 00416 00417 post_open(); 00418 } 00419 00420 /** Destructor. */ 00421 V4L2Camera::~V4L2Camera() 00422 { 00423 if (_started) stop(); 00424 if (_opened) close(); 00425 00426 free(_device_name); 00427 delete _data; 00428 } 00429 00430 void 00431 V4L2Camera::open() 00432 { 00433 if (_started) stop(); 00434 if(_opened) close(); 00435 00436 _dev = ::open(_device_name, O_RDWR); 00437 int libv4l2_fd = v4l2_fd_open(_dev, 0); 00438 if (libv4l2_fd != -1) _dev = libv4l2_fd; 00439 /* Note the v4l2_xxx functions are designed so that if they get passed an 00440 unknown fd, the will behave exactly as their regular xxx counterparts, so 00441 if v4l2_fd_open fails, we continue as normal (missing the libv4l2 custom 00442 cam format to normal formats conversion). Chances are big we will still 00443 fail then though, as normally v4l2_fd_open only fails if the device is not 00444 a v4l2 device. */ 00445 if (_dev < 0) throw Exception("V4L2Cam: Could not open device"); 00446 00447 _opened = true; 00448 00449 // getting capabilities 00450 if (v4l2_ioctl(_dev, VIDIOC_QUERYCAP, &_data->caps)) 00451 { 00452 close(); 00453 throw Exception("V4L2Cam: Could not get capabilities - probably not a v4l2 device"); 00454 } 00455 00456 post_open(); 00457 } 00458 00459 /** 00460 * Post-open() operations. 00461 * Precondition: _dev (file desc) and _data->caps (capabilities) are set. 00462 * @param dev file descriptor of the opened device 00463 */ 00464 void 00465 V4L2Camera::post_open() 00466 { 00467 //LibLogger::log_debug("V4L2Cam", "select_read_method()"); 00468 select_read_method(); 00469 00470 //LibLogger::log_debug("V4L2Cam", "select_format()"); 00471 select_format(); 00472 00473 if (_fps) 00474 { 00475 //LibLogger::log_debug("V4L2Cam", "set_fps()"); 00476 set_fps(); 00477 } 00478 00479 //LibLogger::log_debug("V4L2Cam", "set_controls()"); 00480 set_controls(); 00481 00482 //LibLogger::log_debug("V4L2Cam", "create_buffer()"); 00483 create_buffer(); 00484 00485 //LibLogger::log_debug("V4L2Cam", "reset_cropping()"); 00486 reset_cropping(); 00487 00488 } 00489 00490 /** 00491 * Find suitable reading method. 00492 * The one set in _read_method is preferred. 00493 * Postconditions: 00494 * - _read_method and _buffers_length are set 00495 */ 00496 void 00497 V4L2Camera::select_read_method() 00498 { 00499 /* try preferred method */ 00500 if (!(_data->caps.capabilities & 00501 (_read_method == READ ? V4L2_CAP_READWRITE : V4L2_CAP_STREAMING))) 00502 { 00503 /* preferred read method not supported - try next */ 00504 _read_method = (_read_method == READ ? MMAP : READ); 00505 if (!(_data->caps.capabilities & 00506 (_read_method == READ ? V4L2_CAP_READWRITE : V4L2_CAP_STREAMING))) 00507 { 00508 close(); 00509 throw Exception("V4L2Cam: Neither read() nor streaming IO supported"); 00510 } 00511 } 00512 00513 if (_read_method != READ) 00514 { 00515 v4l2_requestbuffers buf; 00516 00517 /* Streaming IO - Try 1st method, and if that fails 2nd */ 00518 for (int i = 0; i < 2; ++i) 00519 { 00520 if (_read_method == MMAP) 00521 { 00522 _buffers_length = MMAP_NUM_BUFFERS; 00523 buf.count = _buffers_length; 00524 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00525 buf.memory = V4L2_MEMORY_MMAP; 00526 } 00527 else /* UPTR */ 00528 { 00529 _buffers_length = 0; 00530 buf.count = 0; 00531 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00532 buf.memory = V4L2_MEMORY_USERPTR; 00533 } 00534 00535 if (v4l2_ioctl(_dev, VIDIOC_REQBUFS, &buf)) 00536 { 00537 if (errno != EINVAL) 00538 { 00539 close(); 00540 throw Exception("V4L2Cam: REQBUFS query failed"); 00541 } 00542 00543 /* Not supported */ 00544 if (i == 1) 00545 { 00546 close(); 00547 throw Exception("V4L2Cam: Neither memory mapped nor user pointer IO supported"); 00548 } 00549 00550 /* try other method */ 00551 _read_method = (_read_method == MMAP ? UPTR : MMAP); 00552 continue; 00553 } 00554 00555 /* Method supported */ 00556 if ((_read_method == MMAP) && (buf.count < _buffers_length)) 00557 { 00558 close(); 00559 throw Exception("V4L2Cam: Not enough memory for the buffers"); 00560 } 00561 00562 break; 00563 } 00564 } 00565 else /* Read IO */ 00566 { 00567 _buffers_length = 1; 00568 } 00569 00570 switch (_read_method) 00571 { 00572 case READ: 00573 LibLogger::log_debug("V4L2Cam", "Using read() method"); 00574 break; 00575 00576 case MMAP: 00577 LibLogger::log_debug("V4L2Cam", "Using memory mapping method"); 00578 break; 00579 00580 case UPTR: 00581 LibLogger::log_debug("V4L2Cam", "Using user pointer method"); 00582 //TODO 00583 throw Exception("V4L2Cam: user pointer method not supported yet"); 00584 break; 00585 } 00586 } 00587 00588 /** 00589 * Find suitable image format. 00590 * The one set in _format (if any) is preferred. 00591 * Postconditions: 00592 * - _format is set (and selected) 00593 * - _colorspace is set accordingly 00594 * - _width, _height, and _bytes_per_line are set 00595 */ 00596 void 00597 V4L2Camera::select_format() 00598 { 00599 bool preferred_found = false; 00600 v4l2_fmtdesc format_desc; 00601 00602 char fourcc[5] = " "; 00603 00604 if (strcmp(_format, "")) 00605 { 00606 /* Try to select preferred format */ 00607 memset(&format_desc, 0, sizeof(format_desc)); 00608 format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00609 for (format_desc.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUM_FMT, &format_desc) == 0; format_desc.index++) 00610 { 00611 fourcc[0] = static_cast<char>(format_desc.pixelformat & 0xFF); 00612 fourcc[1] = static_cast<char>((format_desc.pixelformat >> 8) & 0xFF); 00613 fourcc[2] = static_cast<char>((format_desc.pixelformat >> 16) & 0xFF); 00614 fourcc[3] = static_cast<char>((format_desc.pixelformat >> 24) & 0xFF); 00615 00616 if (strcmp(_format, fourcc) == 0) 00617 { 00618 preferred_found = true; 00619 break; 00620 } 00621 } 00622 } 00623 00624 if (!preferred_found) 00625 { 00626 /* Preferred format not found (or none selected) 00627 -> just take first available format */ 00628 memset(&format_desc, 0, sizeof(format_desc)); 00629 format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00630 format_desc.index = 0; 00631 if (v4l2_ioctl(_dev, VIDIOC_ENUM_FMT, &format_desc)) 00632 { 00633 close(); 00634 throw Exception("V4L2Cam: No image format found"); 00635 } 00636 00637 fourcc[0] = static_cast<char>(format_desc.pixelformat & 0xFF); 00638 fourcc[1] = static_cast<char>((format_desc.pixelformat >> 8) & 0xFF); 00639 fourcc[2] = static_cast<char>((format_desc.pixelformat >> 16) & 0xFF); 00640 fourcc[3] = static_cast<char>((format_desc.pixelformat >> 24) & 0xFF); 00641 } 00642 00643 /* Now set this format */ 00644 v4l2_format format; 00645 memset(&format, 0, sizeof(format)); 00646 format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00647 if (v4l2_ioctl(_dev, VIDIOC_G_FMT, &format)) 00648 { 00649 close(); 00650 throw Exception("V4L2Cam: Format query failed"); 00651 } 00652 00653 //LibLogger::log_debug("V4L2Cam", "setting %dx%d (%s) - type %d", _width, _height, fourcc, format.type); 00654 00655 format.fmt.pix.pixelformat = v4l2_fourcc(fourcc[0], fourcc[1], fourcc[2], fourcc[3]); 00656 format.fmt.pix.field = V4L2_FIELD_ANY; 00657 if (_width) 00658 format.fmt.pix.width = _width; 00659 if (_height) 00660 format.fmt.pix.height = _height; 00661 00662 if (v4l2_ioctl(_dev, VIDIOC_S_FMT, &format)) 00663 { 00664 //throw Exception(errno, "Failed to set video format"); 00665 //} 00666 00667 // Nao workaround (Hack alert) 00668 LibLogger::log_warn("V4L2Cam", "Format setting failed (driver sucks) - %d: %s", errno, strerror(errno)); 00669 LibLogger::log_info("V4L2Cam", "Trying workaround"); 00670 _nao_hacks = true; 00671 00672 v4l2_std_id std; 00673 if (v4l2_ioctl(_dev, VIDIOC_G_STD, &std)) 00674 { 00675 close(); 00676 throw Exception("V4L2Cam: Standard query (workaround) failed"); 00677 } 00678 00679 if ((_width == 320) && (_height == 240)) 00680 { 00681 std = 0x04000000UL; // QVGA 00682 } 00683 else 00684 { 00685 std = 0x08000000UL; // VGA 00686 _width = 640; 00687 _height = 480; 00688 } 00689 if (v4l2_ioctl(_dev, VIDIOC_S_STD, &std)) 00690 { 00691 close(); 00692 throw Exception("V4L2Cam: Standard setting (workaround) failed"); 00693 } 00694 00695 format.fmt.pix.width = _width; 00696 format.fmt.pix.height = _height; 00697 format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; 00698 format.fmt.pix.field = V4L2_FIELD_ANY; 00699 00700 if (v4l2_ioctl(_dev, VIDIOC_S_FMT, &format)) 00701 { 00702 close(); 00703 throw Exception("V4L2Cam: Format setting (workaround) failed"); 00704 } 00705 00706 if (_switch_u_v) _colorspace = YVY2; 00707 } 00708 00709 /* ...and store final values */ 00710 _format[0] = static_cast<char>(format.fmt.pix.pixelformat & 0xFF); 00711 _format[1] = static_cast<char>((format.fmt.pix.pixelformat >> 8) & 0xFF); 00712 _format[2] = static_cast<char>((format.fmt.pix.pixelformat >> 16) & 0xFF); 00713 _format[3] = static_cast<char>((format.fmt.pix.pixelformat >> 24) & 0xFF); 00714 00715 if (!_nao_hacks || !_switch_u_v) 00716 { 00717 if (strcmp(_format, "RGB3") == 0) _colorspace = RGB; 00718 else if (strcmp(_format, "Y41P") == 0) _colorspace = YUV411_PACKED; //different byte ordering 00719 else if (strcmp(_format, "411P") == 0) _colorspace = YUV411_PLANAR; 00720 else if (strcmp(_format, "YUYV") == 0) _colorspace = YUY2; 00721 else if (strcmp(_format, "BGR3") == 0) _colorspace = BGR; 00722 else if (strcmp(_format, "UYVY") == 0) _colorspace = YUV422_PACKED; 00723 else if (strcmp(_format, "422P") == 0) _colorspace = YUV422_PLANAR; 00724 else if (strcmp(_format, "GREY") == 0) _colorspace = GRAY8; 00725 else if (strcmp(_format, "RGB4") == 0) _colorspace = RGB_WITH_ALPHA; 00726 else if (strcmp(_format, "BGR4") == 0) _colorspace = BGR_WITH_ALPHA; 00727 else if (strcmp(_format, "BA81") == 0) _colorspace = BAYER_MOSAIC_BGGR; 00728 else if (strcmp(_format, "Y16 ") == 0) _colorspace = MONO16; 00729 else _colorspace = CS_UNKNOWN; 00730 } 00731 00732 if (!_nao_hacks) 00733 { 00734 _width = format.fmt.pix.width; 00735 _height = format.fmt.pix.height; 00736 } 00737 00738 _bytes_per_line = format.fmt.pix.bytesperline; 00739 00740 /* Hack for bad drivers */ 00741 if (_bytes_per_line == 0) 00742 { 00743 LibLogger::log_warn("V4L2Cam", "bytesperline is 0 (driver sucks)"); 00744 _bytes_per_line = colorspace_buffer_size(_colorspace, _width, _height) / _height; 00745 } 00746 00747 LibLogger::log_debug("V4L2Cam", "w%d h%d bpl%d cs%d fmt%s", _width, _height, _bytes_per_line, _colorspace, _format); 00748 } 00749 00750 /** 00751 * Set desired FPS count. 00752 */ 00753 void 00754 V4L2Camera::set_fps() 00755 { 00756 v4l2_streamparm param; 00757 param.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00758 if (v4l2_ioctl(_dev, VIDIOC_G_PARM, ¶m)) 00759 { 00760 close(); 00761 throw Exception("V4L2Cam: Streaming parameter query failed"); 00762 } 00763 00764 if (!(param.parm.capture.capability & V4L2_CAP_TIMEPERFRAME)) 00765 { 00766 LibLogger::log_warn("V4L2Cam", "FPS change not supported"); 00767 return; 00768 } 00769 00770 param.parm.capture.timeperframe.numerator = 1; 00771 param.parm.capture.timeperframe.denominator = _fps; 00772 if (v4l2_ioctl(_dev, VIDIOC_S_PARM, ¶m)) 00773 { 00774 close(); 00775 throw Exception("V4L2Cam: Streaming parameter setting failed"); 00776 } 00777 else 00778 { 00779 LibLogger::log_debug("V4L2Cam", "FPS set - %d/%d", 00780 param.parm.capture.timeperframe.numerator, 00781 param.parm.capture.timeperframe.denominator); 00782 } 00783 } 00784 00785 /** 00786 * Set Camera controls. 00787 */ 00788 void 00789 V4L2Camera::set_controls() 00790 { 00791 if (_aec != NOT_SET) set_auto_exposure(_aec == TRUE); 00792 if (_awb != NOT_SET) set_auto_white_balance(_awb == TRUE); 00793 if (_agc != NOT_SET) set_auto_gain(_agc == TRUE); 00794 00795 if (_h_flip != NOT_SET) set_horiz_mirror(_h_flip == TRUE); 00796 if (_v_flip != NOT_SET) set_vert_mirror(_v_flip == TRUE); 00797 00798 if (_brightness.set) set_brightness(_brightness.value); 00799 if (_contrast.set) set_contrast(_contrast.value); 00800 if (_saturation.set) set_saturation(_saturation.value); 00801 if (_hue.set) set_hue(_hue.value); 00802 if (_red_balance.set) set_red_balance(_red_balance.value); 00803 if (_blue_balance.set) set_blue_balance(_blue_balance.value); 00804 if (_exposure.set) set_exposure(_exposure.value); 00805 if (_gain.set) set_gain(_gain.value); 00806 if (_lens_x.set) set_lens_x_corr(_lens_x.value); 00807 if (_lens_y.set) set_lens_y_corr(_lens_y.value); 00808 } 00809 00810 /** 00811 * Set one Camera control value. 00812 * @param ctrl name of the value 00813 * @param id ID of the value 00814 * @param value value to set 00815 */ 00816 void 00817 V4L2Camera::set_one_control(const char *ctrl, unsigned int id, int value) 00818 { 00819 v4l2_queryctrl queryctrl; 00820 v4l2_control control; 00821 00822 memset(&queryctrl, 0, sizeof(queryctrl)); 00823 queryctrl.id = id; 00824 00825 if (v4l2_ioctl(_dev, VIDIOC_QUERYCTRL, &queryctrl)) 00826 { 00827 if (errno == EINVAL) 00828 { 00829 LibLogger::log_error("V4L2Cam", "Control %s not supported", ctrl); 00830 return; 00831 } 00832 00833 close(); 00834 throw Exception("V4L2Cam: %s Control query failed", ctrl); 00835 } 00836 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) 00837 { 00838 LibLogger::log_error("V4L2Cam", "Control %s disabled", ctrl); 00839 return; 00840 } 00841 00842 memset(&control, 0, sizeof(control)); 00843 control.id = id; 00844 control.value = value; 00845 00846 if (v4l2_ioctl(_dev, VIDIOC_S_CTRL, &control)) 00847 { 00848 close(); 00849 throw Exception("V4L2Cam: %s Control setting failed", ctrl); 00850 } 00851 } 00852 00853 /** 00854 * Get one Camera control value. 00855 * @param ctrl name of the value 00856 * @param id ID of the value 00857 * @return current value 00858 */ 00859 int 00860 V4L2Camera::get_one_control(const char *ctrl, unsigned int id) 00861 { 00862 v4l2_queryctrl queryctrl; 00863 v4l2_control control; 00864 00865 memset(&queryctrl, 0, sizeof(queryctrl)); 00866 queryctrl.id = id; 00867 00868 if (v4l2_ioctl(_dev, VIDIOC_QUERYCTRL, &queryctrl)) 00869 { 00870 if (errno == EINVAL) 00871 { 00872 LibLogger::log_error("V4L2Cam", "Control %s not supported", ctrl); 00873 return 0; 00874 } 00875 00876 close(); 00877 throw Exception("V4L2Cam: %s Control query failed", ctrl); 00878 } 00879 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) 00880 { 00881 LibLogger::log_error("V4L2Cam", "Control %s disabled", ctrl); 00882 return 0; 00883 } 00884 00885 memset(&control, 0, sizeof(control)); 00886 control.id = id; 00887 00888 if (v4l2_ioctl(_dev, VIDIOC_G_CTRL, &control)) 00889 { 00890 close(); 00891 throw Exception("V4L2Cam: %s Control value reading failed", ctrl); 00892 } 00893 00894 return control.value; 00895 } 00896 00897 /** 00898 * Create a buffer for image transfer. 00899 * Preconditions: 00900 * - _read_method is set 00901 * - _height and _bytes_per_line are set 00902 * - _buffers_length is set 00903 * Postconditions: 00904 * - _frame_buffers is set up 00905 */ 00906 void 00907 V4L2Camera::create_buffer() 00908 { 00909 _frame_buffers = new FrameBuffer[_buffers_length]; 00910 00911 switch (_read_method) 00912 { 00913 case READ: 00914 { 00915 _frame_buffers[0].size = _bytes_per_line * _height; 00916 _frame_buffers[0].buffer = static_cast<unsigned char *>(malloc(_frame_buffers[0].size)); 00917 if (_frame_buffers[0].buffer == NULL) 00918 { 00919 close(); 00920 throw Exception("V4L2Cam: Out of memory"); 00921 } 00922 break; 00923 } 00924 00925 case MMAP: 00926 { 00927 for (unsigned int i = 0; i < _buffers_length; ++i) 00928 { 00929 /* Query status of buffer */ 00930 v4l2_buffer buffer; 00931 00932 memset(&buffer, 0, sizeof (buffer)); 00933 buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00934 buffer.memory = V4L2_MEMORY_MMAP; 00935 buffer.index = i; 00936 00937 if (v4l2_ioctl(_dev, VIDIOC_QUERYBUF, &buffer)) 00938 { 00939 close(); 00940 throw Exception("V4L2Cam: Buffer query failed"); 00941 } 00942 00943 _frame_buffers[i].size = buffer.length; 00944 _frame_buffers[i].buffer = static_cast<unsigned char *>( 00945 v4l2_mmap(NULL, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, _dev, buffer.m.offset) 00946 ); 00947 if (_frame_buffers[i].buffer == MAP_FAILED) 00948 { 00949 close(); 00950 throw Exception("V4L2Cam: Memory mapping failed"); 00951 } 00952 } 00953 00954 break; 00955 } 00956 00957 case UPTR: 00958 /* not supported yet */ 00959 break; 00960 } 00961 } 00962 00963 /** 00964 * Reset cropping parameters. 00965 */ 00966 void 00967 V4L2Camera::reset_cropping() 00968 { 00969 v4l2_cropcap cropcap; 00970 v4l2_crop crop; 00971 00972 memset(&cropcap, 0, sizeof(cropcap)); 00973 cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00974 00975 if (v4l2_ioctl(_dev, VIDIOC_CROPCAP, &cropcap)) 00976 { 00977 LibLogger::log_warn("V4L2Cam", "cropcap query failed (driver sucks) - %d: %s", errno, strerror(errno)); 00978 } 00979 00980 memset(&crop, 0, sizeof(crop)); 00981 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00982 crop.c = cropcap.defrect; 00983 00984 /* Ignore if cropping is not supported (EINVAL). */ 00985 if (v4l2_ioctl(_dev, VIDIOC_S_CROP, &crop) && errno != EINVAL) 00986 { 00987 LibLogger::log_warn("V4L2Cam", "cropping query failed (driver sucks) - %d: %s", errno, strerror(errno)); 00988 } 00989 } 00990 00991 void 00992 V4L2Camera::close() 00993 { 00994 //LibLogger::log_debug("V4L2Cam", "close()"); 00995 00996 if (_started) stop(); 00997 00998 if (_frame_buffers) 00999 { 01000 switch (_read_method) 01001 { 01002 case READ: 01003 { 01004 free(_frame_buffers[0].buffer); 01005 break; 01006 } 01007 01008 case MMAP: 01009 { 01010 for (unsigned int i = 0; i < _buffers_length; ++i) 01011 { 01012 v4l2_munmap(_frame_buffers[i].buffer, _frame_buffers[i].size); 01013 } 01014 break; 01015 } 01016 01017 case UPTR: 01018 /* not supported yet */ 01019 break; 01020 } 01021 delete[] _frame_buffers; 01022 _frame_buffers = NULL; 01023 _current_buffer = -1; 01024 } 01025 01026 if (_opened) 01027 { 01028 v4l2_close(_dev); 01029 _opened = false; 01030 _dev = 0; 01031 } 01032 01033 if (_capture_time) 01034 { 01035 delete _capture_time; 01036 _capture_time = 0; 01037 } 01038 } 01039 01040 void 01041 V4L2Camera::start() 01042 { 01043 //LibLogger::log_info("V4L2Cam", "start()"); 01044 01045 if (!_opened) throw Exception("VL42Cam: Camera not opened"); 01046 01047 if (_started) stop(); 01048 01049 switch (_read_method) 01050 { 01051 case READ: 01052 /* nothing to do here */ 01053 break; 01054 01055 case MMAP: 01056 { 01057 /* enqueue buffers */ 01058 for (unsigned int i = 0; i < _buffers_length; ++i) 01059 { 01060 v4l2_buffer buffer; 01061 memset(&buffer, 0, sizeof(buffer)); 01062 buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01063 buffer.memory = V4L2_MEMORY_MMAP; 01064 buffer.index = i; 01065 01066 if (v4l2_ioctl(_dev, VIDIOC_QBUF, &buffer)) 01067 { 01068 close(); 01069 throw Exception("V4L2Cam: Enqueuing buffer failed"); 01070 } 01071 } 01072 01073 /* start streaming */ 01074 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01075 if (v4l2_ioctl(_dev, VIDIOC_STREAMON, &type)) 01076 { 01077 close(); 01078 throw Exception("V4L2Cam: Starting stream failed"); 01079 } 01080 break; 01081 } 01082 01083 case UPTR: 01084 /* not supported yet */ 01085 break; 01086 } 01087 01088 //LibLogger::log_debug("V4L2Cam", "start() complete"); 01089 _started = true; 01090 } 01091 01092 void 01093 V4L2Camera::stop() 01094 { 01095 //LibLogger::log_debug("V4L2Cam", "stop()"); 01096 01097 if (!_started) return; 01098 01099 switch (_read_method) 01100 { 01101 case READ: 01102 /* nothing to do here */ 01103 break; 01104 01105 case MMAP: /* fall through */ 01106 case UPTR: 01107 { 01108 /* stop streaming */ 01109 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01110 if (v4l2_ioctl(_dev, VIDIOC_STREAMOFF, &type)) 01111 { 01112 close(); 01113 throw Exception("V4L2Cam: Stopping stream failed"); 01114 } 01115 break; 01116 } 01117 } 01118 01119 _current_buffer = -1; 01120 _started = false; 01121 } 01122 01123 bool 01124 V4L2Camera::ready() 01125 { 01126 //LibLogger::log_debug("V4L2Cam", "ready()"); 01127 01128 return _started; 01129 } 01130 01131 void 01132 V4L2Camera::flush() 01133 { 01134 //LibLogger::log_debug("V4L2Cam", "flush()"); 01135 /* not needed */ 01136 } 01137 01138 void 01139 V4L2Camera::capture() 01140 { 01141 //LibLogger::log_debug("V4L2Cam", "capture()"); 01142 01143 if (!_started) return; 01144 01145 switch (_read_method) 01146 { 01147 case READ: 01148 { 01149 _current_buffer = 0; 01150 //LibLogger::log_debug("V4L2Cam", "calling read()"); 01151 if (v4l2_read(_dev, _frame_buffers[_current_buffer].buffer, _frame_buffers[_current_buffer].size) == -1) 01152 { 01153 //TODO: errno handling 01154 LibLogger::log_warn("V4L2Cam", "read() failed with code %d: %s", errno, strerror(errno)); 01155 } 01156 //LibLogger::log_debug("V4L2Cam", "returned from read()"); 01157 01158 //No timestamping support here - just take current system time 01159 if (_capture_time) 01160 { 01161 _capture_time->stamp(); 01162 } 01163 else 01164 { 01165 _capture_time = new fawkes::Time(); 01166 } 01167 01168 break; 01169 } 01170 01171 case MMAP: 01172 { 01173 /* dequeue buffer */ 01174 v4l2_buffer buffer; 01175 memset(&buffer, 0, sizeof(buffer)); 01176 buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01177 buffer.memory = V4L2_MEMORY_MMAP; 01178 01179 if (v4l2_ioctl(_dev, VIDIOC_DQBUF, &buffer)) 01180 { 01181 //TODO: errno handling -> EAGAIN, ...? 01182 close(); 01183 throw Exception("V4L2Cam: Dequeuing buffer failed"); 01184 } 01185 01186 _current_buffer = buffer.index; 01187 01188 if (_capture_time) 01189 { 01190 _capture_time->set_time(&buffer.timestamp); 01191 } 01192 else 01193 { 01194 _capture_time = new fawkes::Time(&buffer.timestamp); 01195 } 01196 break; 01197 } 01198 01199 case UPTR: 01200 /* not supported yet */ 01201 break; 01202 } 01203 } 01204 01205 unsigned char * 01206 V4L2Camera::buffer() 01207 { 01208 //LibLogger::log_debug("V4L2Cam", "buffer()"); 01209 01210 return (_current_buffer == -1 ? NULL : _frame_buffers[_current_buffer].buffer); 01211 } 01212 01213 unsigned int 01214 V4L2Camera::buffer_size() 01215 { 01216 //LibLogger::log_debug("V4L2Cam", "buffer_size()"); 01217 01218 return (_opened && (_current_buffer != -1) ? _frame_buffers[_current_buffer].size : 0); 01219 } 01220 01221 void 01222 V4L2Camera::dispose_buffer() 01223 { 01224 //LibLogger::log_debug("V4L2Cam", "dispose_buffer()"); 01225 01226 if (!_opened) return; 01227 01228 switch (_read_method) 01229 { 01230 case READ: 01231 /* nothing to do here */ 01232 break; 01233 01234 case MMAP: 01235 { 01236 /* enqueue next buffer */ 01237 v4l2_buffer buffer; 01238 memset(&buffer, 0, sizeof(buffer)); 01239 buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01240 buffer.memory = V4L2_MEMORY_MMAP; 01241 buffer.index = _current_buffer; 01242 01243 //TODO: Test if the next buffer is also the latest buffer (VIDIOC_QUERYBUF) 01244 if (v4l2_ioctl(_dev, VIDIOC_QBUF, &buffer)) 01245 { 01246 close(); 01247 throw Exception("V4L2Cam: Enqueuing buffer failed"); 01248 } 01249 break; 01250 } 01251 01252 case UPTR: 01253 /* not supported yet */ 01254 break; 01255 } 01256 01257 _current_buffer = -1; 01258 } 01259 01260 unsigned int 01261 V4L2Camera::pixel_width() 01262 { 01263 //LibLogger::log_debug("V4L2Cam", "pixel_width()"); 01264 01265 return _width; 01266 } 01267 01268 unsigned int 01269 V4L2Camera::pixel_height() 01270 { 01271 //LibLogger::log_debug("V4L2Cam", "pixel_height()"); 01272 01273 return _height; 01274 } 01275 01276 colorspace_t 01277 V4L2Camera::colorspace() 01278 { 01279 //LibLogger::log_debug("V4L2Cam", "colorspace()"); 01280 01281 if (!_opened) 01282 return CS_UNKNOWN; 01283 else 01284 return _colorspace; 01285 } 01286 01287 fawkes::Time * 01288 V4L2Camera::capture_time() 01289 { 01290 return _capture_time; 01291 } 01292 01293 void 01294 V4L2Camera::set_image_number(unsigned int n) 01295 { 01296 //LibLogger::log_debug("V4L2Cam", "set_image_number(%d)", n); 01297 01298 /* not needed */ 01299 } 01300 01301 01302 /* --- CameraControls --- */ 01303 01304 bool 01305 V4L2Camera::auto_gain() 01306 { 01307 return get_one_control("AGC", V4L2_CID_AUTOGAIN); 01308 } 01309 01310 void 01311 V4L2Camera::set_auto_gain(bool enabled) 01312 { 01313 LibLogger::log_debug("V4L2Cam", (enabled ? "enabling AGC" : "disabling AGC")); 01314 set_one_control("AGC", V4L2_CID_AUTOGAIN, (enabled ? 1 : 0)); 01315 } 01316 01317 bool 01318 V4L2Camera::auto_white_balance() 01319 { 01320 return get_one_control("AWB", V4L2_CID_AUTO_WHITE_BALANCE); 01321 } 01322 01323 void 01324 V4L2Camera::set_auto_white_balance(bool enabled) 01325 { 01326 LibLogger::log_debug("V4L2Cam", (enabled ? "enabling AWB" : "disabling AWB")); 01327 set_one_control("AWB", V4L2_CID_AUTO_WHITE_BALANCE, (enabled ? 1 : 0)); 01328 } 01329 01330 bool 01331 V4L2Camera::auto_exposure() 01332 { 01333 throw NotImplementedException("No such method in the V4L2 standard"); 01334 } 01335 01336 void 01337 V4L2Camera::set_auto_exposure(bool enabled) 01338 { 01339 throw NotImplementedException("No such method in the V4L2 standard"); 01340 } 01341 01342 int 01343 V4L2Camera::red_balance() 01344 { 01345 return get_one_control("red balance", V4L2_CID_RED_BALANCE); 01346 } 01347 01348 void 01349 V4L2Camera::set_red_balance(int red_balance) 01350 { 01351 LibLogger::log_debug("V4L2Cam", "Setting red balance to %d", red_balance); 01352 set_one_control("red balance", V4L2_CID_RED_BALANCE, red_balance); 01353 } 01354 01355 int 01356 V4L2Camera::blue_balance() 01357 { 01358 return get_one_control("blue balance", V4L2_CID_BLUE_BALANCE); 01359 } 01360 01361 void 01362 V4L2Camera::set_blue_balance(int blue_balance) 01363 { 01364 LibLogger::log_debug("V4L2Cam", "Setting blue balance to %d", blue_balance); 01365 set_one_control("blue balance", V4L2_CID_BLUE_BALANCE, blue_balance); 01366 } 01367 01368 int 01369 V4L2Camera::u_balance() 01370 { 01371 throw NotImplementedException("No such method in the V4L2 standard"); 01372 } 01373 01374 void 01375 V4L2Camera::set_u_balance(int u_balance) 01376 { 01377 throw NotImplementedException("No such method in the V4L2 standard"); 01378 } 01379 01380 int 01381 V4L2Camera::v_balance() 01382 { 01383 throw NotImplementedException("No such method in the V4L2 standard"); 01384 } 01385 01386 void 01387 V4L2Camera::set_v_balance(int v_balance) 01388 { 01389 throw NotImplementedException("No such method in the V4L2 standard"); 01390 } 01391 01392 unsigned int 01393 V4L2Camera::brightness() 01394 { 01395 return get_one_control("brightness", V4L2_CID_BRIGHTNESS); 01396 } 01397 01398 void 01399 V4L2Camera::set_brightness(unsigned int brightness) 01400 { 01401 LibLogger::log_debug("V4L2Cam", "Setting brighness to %d", brightness); 01402 set_one_control("brightness", V4L2_CID_BRIGHTNESS, brightness); 01403 } 01404 01405 unsigned int 01406 V4L2Camera::contrast() 01407 { 01408 return get_one_control("contrast", V4L2_CID_CONTRAST); 01409 } 01410 01411 void 01412 V4L2Camera::set_contrast(unsigned int contrast) 01413 { 01414 LibLogger::log_debug("V4L2Cam", "Setting contrast to %d", contrast); 01415 set_one_control("contrast", V4L2_CID_CONTRAST, contrast); 01416 } 01417 01418 unsigned int 01419 V4L2Camera::saturation() 01420 { 01421 return get_one_control("saturation", V4L2_CID_SATURATION); 01422 } 01423 01424 void 01425 V4L2Camera::set_saturation(unsigned int saturation) 01426 { 01427 LibLogger::log_debug("V4L2Cam", "Setting saturation to %d", saturation); 01428 set_one_control("saturation", V4L2_CID_SATURATION, saturation); 01429 } 01430 01431 int 01432 V4L2Camera::hue() 01433 { 01434 return get_one_control("hue", V4L2_CID_HUE); 01435 } 01436 01437 void 01438 V4L2Camera::set_hue(int hue) 01439 { 01440 LibLogger::log_debug("V4L2Cam", "Setting hue to %d", hue); 01441 set_one_control("hue", V4L2_CID_HUE, hue); 01442 } 01443 01444 unsigned int 01445 V4L2Camera::exposure() 01446 { 01447 return get_one_control("exposure", V4L2_CID_EXPOSURE); 01448 } 01449 01450 void 01451 V4L2Camera::set_exposure(unsigned int exposure) 01452 { 01453 LibLogger::log_debug("V4L2Cam", "Setting exposure to %d", exposure); 01454 set_one_control("exposure", V4L2_CID_EXPOSURE, exposure); 01455 } 01456 01457 unsigned int 01458 V4L2Camera::gain() 01459 { 01460 return get_one_control("gain", V4L2_CID_GAIN); 01461 } 01462 01463 void 01464 V4L2Camera::set_gain(unsigned int gain) 01465 { 01466 LibLogger::log_debug("V4L2Cam", "Setting gain to %u", gain); 01467 set_one_control("gain", V4L2_CID_GAIN, gain); 01468 } 01469 01470 01471 const char * 01472 V4L2Camera::format() 01473 { 01474 return _format; 01475 } 01476 01477 void 01478 V4L2Camera::set_format(const char *format) 01479 { 01480 strncpy(_format, format, 4); 01481 _format[4] = '\0'; 01482 select_format(); 01483 } 01484 01485 unsigned int 01486 V4L2Camera::width() 01487 { 01488 return pixel_width(); 01489 } 01490 01491 unsigned int 01492 V4L2Camera::height() 01493 { 01494 return pixel_height(); 01495 } 01496 01497 void 01498 V4L2Camera::set_size(unsigned int width, 01499 unsigned int height) 01500 { 01501 _width = width; 01502 _height = height; 01503 select_format(); 01504 } 01505 01506 bool 01507 V4L2Camera::horiz_mirror() 01508 { 01509 return (get_one_control("hflip", V4L2_CID_HFLIP) != 0); 01510 } 01511 01512 bool 01513 V4L2Camera::vert_mirror() 01514 { 01515 return (get_one_control("vflip", V4L2_CID_VFLIP) != 0); 01516 } 01517 01518 void 01519 V4L2Camera::set_horiz_mirror(bool enabled) 01520 { 01521 LibLogger::log_debug("V4L2Cam", (enabled ? "enabling horizontal flip" : "disabling horizontal flip")); 01522 set_one_control("hflip", V4L2_CID_HFLIP, (enabled ? 1 : 0)); 01523 } 01524 01525 void 01526 V4L2Camera::set_vert_mirror(bool enabled) 01527 { 01528 LibLogger::log_debug("V4L2Cam", (enabled ? "enabling vertical flip" : "disabling vertical flip")); 01529 set_one_control("vflip", V4L2_CID_VFLIP, (enabled ? 1 : 0)); 01530 } 01531 01532 /** Get the number of frames per second that have been requested from the camera. 01533 * A return value of 0 means that fps haven't been set yet through the camera. 01534 * @return the currently requested fps or 0 if not set yet 01535 */ 01536 unsigned int 01537 V4L2Camera::fps() 01538 { 01539 return _fps; 01540 } 01541 01542 void 01543 V4L2Camera::set_fps(unsigned int fps) 01544 { 01545 _fps = fps; 01546 set_fps(); 01547 } 01548 01549 unsigned int 01550 V4L2Camera::lens_x_corr() 01551 { 01552 return get_one_control("lens x", V4L2_CID_HCENTER/*_DEPRECATED*/); 01553 } 01554 01555 unsigned int 01556 V4L2Camera::lens_y_corr() 01557 { 01558 return get_one_control("lens y", V4L2_CID_VCENTER/*_DEPRECATED*/); 01559 } 01560 01561 void 01562 V4L2Camera::set_lens_x_corr(unsigned int x_corr) 01563 { 01564 LibLogger::log_debug("V4L2Cam", "Setting horizontal lens correction to %d", x_corr); 01565 set_one_control("lens x", V4L2_CID_HCENTER/*_DEPRECATED*/, x_corr); 01566 } 01567 01568 void 01569 V4L2Camera::set_lens_y_corr(unsigned int y_corr) 01570 { 01571 LibLogger::log_debug("V4L2Cam", "Setting vertical lens correction to %d", y_corr); 01572 set_one_control("lens x", V4L2_CID_VCENTER/*_DEPRECATED*/, y_corr); 01573 } 01574 01575 01576 void 01577 V4L2Camera::print_info() 01578 { 01579 /* General capabilities */ 01580 cout << 01581 "==========================================================================" 01582 << endl << _device_name << " (" << _data->caps.card << ") - " << _data->caps.bus_info 01583 << endl << "Driver: " << _data->caps.driver << " (ver " << 01584 ((_data->caps.version >> 16) & 0xFF) << "." << 01585 ((_data->caps.version >> 8) & 0xFF) << "." << 01586 (_data->caps.version & 0xFF) << ")" << endl << 01587 "--------------------------------------------------------------------------" 01588 << endl; 01589 01590 /* General capabilities */ 01591 cout << "Capabilities:" << endl; 01592 if (_data->caps.capabilities & V4L2_CAP_VIDEO_CAPTURE) 01593 cout << " + Video capture interface supported" << endl; 01594 if (_data->caps.capabilities & V4L2_CAP_VIDEO_OUTPUT) 01595 cout << " + Video output interface supported" << endl; 01596 if (_data->caps.capabilities & V4L2_CAP_VIDEO_OVERLAY) 01597 cout << " + Video overlay interface supported" << endl; 01598 if (_data->caps.capabilities & V4L2_CAP_VBI_CAPTURE) 01599 cout << " + Raw VBI capture interface supported" << endl; 01600 if (_data->caps.capabilities & V4L2_CAP_VBI_OUTPUT) 01601 cout << " + Raw VBI output interface supported" << endl; 01602 if (_data->caps.capabilities & V4L2_CAP_SLICED_VBI_CAPTURE) 01603 cout << " + Sliced VBI capture interface supported" << endl; 01604 if (_data->caps.capabilities & V4L2_CAP_SLICED_VBI_OUTPUT) 01605 cout << " + Sliced VBI output interface supported" << endl; 01606 if (_data->caps.capabilities & V4L2_CAP_RDS_CAPTURE) 01607 cout << " + RDS_CAPTURE set" << endl; 01608 /* Not included in Nao's version 01609 if (caps.capabilities & V4L2_CAP_VIDEO_OUTPUT_OVERLAY) 01610 cout << " + Video output overlay interface supported" << endl; */ 01611 if (_data->caps.capabilities & V4L2_CAP_TUNER) 01612 cout << " + Has some sort of tuner" << endl; 01613 if (_data->caps.capabilities & V4L2_CAP_AUDIO) 01614 cout << " + Has audio inputs or outputs" << endl; 01615 if (_data->caps.capabilities & V4L2_CAP_RADIO) 01616 cout << " + Has a radio receiver" << endl; 01617 if (_data->caps.capabilities & V4L2_CAP_READWRITE) 01618 cout << " + read() and write() IO supported" << endl; 01619 if (_data->caps.capabilities & V4L2_CAP_ASYNCIO) 01620 cout << " + asynchronous IO supported" << endl; 01621 if (_data->caps.capabilities & V4L2_CAP_STREAMING) 01622 cout << " + streaming IO supported" << endl; 01623 if (_data->caps.capabilities & V4L2_CAP_TIMEPERFRAME) 01624 cout << " + timeperframe field is supported" << endl; 01625 cout << endl; 01626 01627 /* Inputs */ 01628 cout << "Inputs:" << endl; 01629 v4l2_input input; 01630 memset(&input, 0, sizeof(input)); 01631 01632 for (input.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUMINPUT, &input) == 0; input.index++) 01633 { 01634 cout << "Input " << input.index << ": " << input.name << endl; 01635 01636 cout << " |- Type: "; 01637 switch (input.type) 01638 { 01639 case V4L2_INPUT_TYPE_TUNER: 01640 cout << "Tuner"; 01641 break; 01642 01643 case V4L2_INPUT_TYPE_CAMERA: 01644 cout << "Camera"; 01645 break; 01646 01647 default: 01648 cout << "Unknown"; 01649 } 01650 cout << endl; 01651 01652 cout << " |- Supported standards:"; 01653 if (input.std == 0) 01654 { 01655 cout << " Unknown" << endl; 01656 } 01657 else 01658 { 01659 cout << endl; 01660 01661 v4l2_standard standard; 01662 memset (&standard, 0, sizeof(standard)); 01663 standard.index = 0; 01664 01665 for (standard.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUMSTD, &standard) == 0; standard.index++) 01666 { 01667 if (standard.id & input.std) cout << " + " << standard.name << endl; 01668 } 01669 } 01670 } 01671 if (input.index == 0) cout << "None" << endl; 01672 cout << endl; 01673 01674 /* Outputs */ 01675 cout << "Outputs:" << endl; 01676 v4l2_output output; 01677 memset (&output, 0, sizeof(output)); 01678 01679 for (output.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUMOUTPUT, &output) == 0; output.index++) 01680 { 01681 cout << " + Output " << output.index << ": " << output.name << endl; 01682 01683 cout << " |- Type: "; 01684 switch (output.type) 01685 { 01686 case V4L2_OUTPUT_TYPE_MODULATOR: 01687 cout << "TV Modulator"; 01688 break; 01689 01690 case V4L2_OUTPUT_TYPE_ANALOG: 01691 cout << "Analog output"; 01692 break; 01693 01694 default: 01695 cout << "Unknown"; 01696 } 01697 cout << endl; 01698 01699 cout << " |- Supported standards:"; 01700 if (output.std == 0) 01701 { 01702 cout << " Unknown" << endl; 01703 } 01704 else 01705 { 01706 cout << endl; 01707 01708 v4l2_standard standard; 01709 memset (&standard, 0, sizeof (standard)); 01710 standard.index = 0; 01711 01712 for (standard.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUMSTD, &standard) == 0; standard.index++) 01713 { 01714 if (standard.id & output.std) cout << " + " << standard.name << endl; 01715 } 01716 } 01717 } 01718 if (output.index == 0) cout << "None" << endl; 01719 cout << endl; 01720 01721 /* Supported formats */ 01722 cout << "Formats:" << endl; 01723 v4l2_fmtdesc format_desc; 01724 memset(&format_desc, 0, sizeof(format_desc)); 01725 format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01726 01727 char fourcc[5] = " "; 01728 for (format_desc.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUM_FMT, &format_desc) == 0; format_desc.index++) 01729 { 01730 fourcc[0] = static_cast<char>(format_desc.pixelformat & 0xFF); 01731 fourcc[1] = static_cast<char>((format_desc.pixelformat >> 8) & 0xFF); 01732 fourcc[2] = static_cast<char>((format_desc.pixelformat >> 16) & 0xFF); 01733 fourcc[3] = static_cast<char>((format_desc.pixelformat >> 24) & 0xFF); 01734 01735 colorspace_t cs = CS_UNKNOWN; 01736 if (strcmp(fourcc, "RGB3") == 0) cs = RGB; 01737 else if (strcmp(fourcc, "Y41P") == 0) cs = YUV411_PACKED; //different byte ordering 01738 else if (strcmp(fourcc, "411P") == 0) cs = YUV411_PLANAR; 01739 else if (strcmp(fourcc, "YUYV") == 0) cs = YUY2; 01740 else if (strcmp(fourcc, "BGR3") == 0) cs = BGR; 01741 else if (strcmp(fourcc, "UYVY") == 0) cs = YUV422_PACKED; 01742 else if (strcmp(fourcc, "422P") == 0) cs = YUV422_PLANAR; 01743 else if (strcmp(fourcc, "GREY") == 0) cs = GRAY8; 01744 else if (strcmp(fourcc, "RGB4") == 0) cs = RGB_WITH_ALPHA; 01745 else if (strcmp(fourcc, "BGR4") == 0) cs = BGR_WITH_ALPHA; 01746 else if (strcmp(fourcc, "BA81") == 0) cs = BAYER_MOSAIC_BGGR; 01747 else if (strcmp(fourcc, "Y16 ") == 0) cs = MONO16; 01748 01749 cout << " + Format " << format_desc.index << ": " << fourcc << 01750 " (" << format_desc.description << ")"; 01751 if (format_desc.flags & V4L2_FMT_FLAG_COMPRESSED) cout << " [Compressed]"; 01752 cout << endl << " |- Colorspace: " << colorspace_to_string(cs) << endl; 01753 } 01754 cout << endl; 01755 01756 /* Current Format */ 01757 v4l2_format format; 01758 memset(&format, 0, sizeof(format)); 01759 format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 01760 if (v4l2_ioctl(_dev, VIDIOC_G_FMT, &format)) throw Exception("V4L2Cam: Format query failed"); 01761 fourcc[0] = static_cast<char>(format.fmt.pix.pixelformat & 0xFF); 01762 fourcc[1] = static_cast<char>((format.fmt.pix.pixelformat >> 8) & 0xFF); 01763 fourcc[2] = static_cast<char>((format.fmt.pix.pixelformat >> 16) & 0xFF); 01764 fourcc[3] = static_cast<char>((format.fmt.pix.pixelformat >> 24) & 0xFF); 01765 01766 cout << " Current Format:" << endl << 01767 " " << format.fmt.pix.width << "x" << format.fmt.pix.height << 01768 " (" << fourcc << ")" << endl << 01769 " " << format.fmt.pix.bytesperline << " bytes per line" << endl << 01770 " Total size: " << format.fmt.pix.sizeimage << endl; 01771 01772 /* Supported Controls */ 01773 cout << "Controls:" << endl; 01774 v4l2_queryctrl queryctrl; 01775 v4l2_querymenu querymenu; 01776 01777 memset(&queryctrl, 0, sizeof(queryctrl)); 01778 01779 for (queryctrl.id = V4L2_CID_BASE; queryctrl.id < V4L2_CID_LASTP1; 01780 queryctrl.id++) 01781 { 01782 if (v4l2_ioctl(_dev, VIDIOC_QUERYCTRL, &queryctrl)) 01783 { 01784 if (errno == EINVAL) continue; 01785 01786 cout << "Control query failed" << endl; 01787 return; 01788 } 01789 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) continue; 01790 01791 cout << " + " << queryctrl.name << " [" << 01792 (queryctrl.id - V4L2_CID_BASE) << "] ("; 01793 switch (queryctrl.type) 01794 { 01795 case V4L2_CTRL_TYPE_INTEGER: 01796 cout << "int [" << queryctrl.minimum << "-" << queryctrl.maximum << 01797 " /" << queryctrl.step << " def " << queryctrl.default_value << 01798 "]"; 01799 break; 01800 01801 case V4L2_CTRL_TYPE_MENU: 01802 cout << "menu [def " << queryctrl.default_value << "]"; 01803 break; 01804 01805 case V4L2_CTRL_TYPE_BOOLEAN: 01806 cout << "bool [def " << queryctrl.default_value << "]"; 01807 break; 01808 01809 case V4L2_CTRL_TYPE_BUTTON: 01810 cout << "button"; 01811 break; 01812 01813 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,17) 01814 case V4L2_CTRL_TYPE_INTEGER64: 01815 cout << "int64"; 01816 break; 01817 01818 case V4L2_CTRL_TYPE_CTRL_CLASS: 01819 cout << "ctrl_class"; 01820 break; 01821 #endif 01822 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32) 01823 case V4L2_CTRL_TYPE_STRING: 01824 cout << "string"; 01825 break; 01826 #endif 01827 } 01828 cout << ")" << endl; 01829 01830 if (queryctrl.type == V4L2_CTRL_TYPE_MENU) 01831 { 01832 cout << " |- Menu items:" << endl; 01833 01834 memset(&querymenu, 0, sizeof(querymenu)); 01835 querymenu.id = queryctrl.id; 01836 01837 for (querymenu.index = queryctrl.minimum; 01838 querymenu.index <= static_cast<unsigned long int>(queryctrl.maximum); 01839 querymenu.index++) 01840 { 01841 if (v4l2_ioctl(_dev, VIDIOC_QUERYMENU, &querymenu)) 01842 { 01843 cout << "Getting menu items failed" << endl; 01844 return; 01845 } 01846 cout << " | + " << querymenu.name << endl; 01847 } 01848 } 01849 } 01850 if (queryctrl.id == V4L2_CID_BASE) cout << "None" << endl; 01851 cout << endl; 01852 01853 /* Supported Private Controls */ 01854 cout << "Private Controls:" << endl; 01855 for (queryctrl.id = V4L2_CID_PRIVATE_BASE; ; queryctrl.id++) 01856 { 01857 if (v4l2_ioctl(_dev, VIDIOC_QUERYCTRL, &queryctrl)) 01858 { 01859 if (errno == EINVAL) break; 01860 01861 cout << "Private Control query failed" << endl; 01862 return; 01863 } 01864 01865 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) continue; 01866 01867 cout << " + " << queryctrl.name << " [" << 01868 (queryctrl.id - V4L2_CID_PRIVATE_BASE) << "] ("; 01869 switch (queryctrl.type) 01870 { 01871 case V4L2_CTRL_TYPE_INTEGER: 01872 cout << "int [" << queryctrl.minimum << "-" << queryctrl.maximum << 01873 " /" << queryctrl.step << " def " << queryctrl.default_value << 01874 "]"; 01875 break; 01876 01877 case V4L2_CTRL_TYPE_MENU: 01878 cout << "menu [def " << queryctrl.default_value << "]"; 01879 break; 01880 01881 case V4L2_CTRL_TYPE_BOOLEAN: 01882 cout << "bool [def " << queryctrl.default_value << "]"; 01883 break; 01884 01885 case V4L2_CTRL_TYPE_BUTTON: 01886 cout << "button"; 01887 break; 01888 01889 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,17) 01890 case V4L2_CTRL_TYPE_INTEGER64: 01891 cout << "int64"; 01892 break; 01893 01894 case V4L2_CTRL_TYPE_CTRL_CLASS: 01895 cout << "ctrl_class"; 01896 break; 01897 #endif 01898 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32) 01899 case V4L2_CTRL_TYPE_STRING: 01900 cout << "string"; 01901 break; 01902 #endif 01903 } 01904 cout << ")" << endl; 01905 01906 if (queryctrl.type == V4L2_CTRL_TYPE_MENU) 01907 { 01908 cout << " |- Menu items:" << endl; 01909 01910 memset(&querymenu, 0, sizeof(querymenu)); 01911 querymenu.id = queryctrl.id; 01912 01913 for (querymenu.index = queryctrl.minimum; 01914 querymenu.index <= static_cast<unsigned long int>(queryctrl.maximum); 01915 querymenu.index++) 01916 { 01917 if (v4l2_ioctl(_dev, VIDIOC_QUERYMENU, &querymenu)) 01918 { 01919 cout << "Getting menu items failed" << endl; 01920 return; 01921 } 01922 cout << " | + " << querymenu.name << endl; 01923 } 01924 } 01925 } 01926 if (queryctrl.id == V4L2_CID_PRIVATE_BASE) cout << "None" << endl; 01927 01928 cout << 01929 "==========================================================================" 01930 << endl; 01931 } 01932 01933 } // end namespace firevision