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