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