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

Generated on Tue Feb 22 13:32:15 2011 for Fawkes API by  doxygen 1.4.7