Fawkes API  Fawkes Development Version
v4l2.cpp
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, &param))
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, &param))
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