Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
v4l2.cpp
1 
2 /***************************************************************************
3  * v4l2.cpp - Video4Linux 2 camera access
4  *
5  * Created: Sat Jul 5 20:40:20 2008
6  * Copyright 2008 Tobias Kellner
7  * 2010 Tim Niemueller
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version. A runtime exception applies to
15  * this software (see LICENSE.GPL_WRE file mentioned below for details).
16  *
17  * This program is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  * GNU Library General Public License for more details.
21  *
22  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
23  */
24 
25 #include <fvcams/v4l2.h>
26 
27 #include <core/exception.h>
28 #include <core/exceptions/software.h>
29 #include <logging/liblogger.h>
30 #include <fvutils/system/camargp.h>
31 
32 #include <fcntl.h>
33 #include <sys/ioctl.h>
34 #include <sys/mman.h>
35 #include <iostream>
36 #include <cstring>
37 #include <cerrno>
38 #include <cstdlib>
39 #include <linux/version.h>
40 
41 using std::cout;
42 using std::endl;
43 using std::string;
44 using fawkes::Exception;
47 using fawkes::LibLogger;
48 
49 #ifdef HAVE_LIBV4L2
50 # include <libv4l2.h>
51 #else
52 # include <unistd.h>
53 # define v4l2_fd_open(fd, flags) (fd)
54 # define v4l2_close ::close
55 # define v4l2_dup dup
56 # define v4l2_ioctl ioctl
57 # define v4l2_read read
58 # define v4l2_mmap mmap
59 # define v4l2_munmap munmap
60 #endif
61 
62 namespace firevision {
63 #if 0 /* just to make Emacs auto-indent happy */
64 }
65 #endif
66 
67 /// @cond INTERNALS
68 class V4L2CameraData
69 {
70  public:
71  v4l2_capability caps; //< Device capabilites
72 };
73 
74 /// @endcond
75 
76 
77 /** @class V4L2Camera <fvcams/v4l2.h>
78  * Video4Linux 2 camera access implementation.
79  *
80  * @todo UPTR method
81  * @todo Standards queries (VIDIOC_ENUMSTD)
82  * @todo v4l2_pix_format.field
83  * @author Tobias Kellner
84  */
85 
86 
87 /** Constructor.
88  * @param device_name device file name (e.g. /dev/video0)
89  */
90 V4L2Camera::V4L2Camera(const char *device_name)
91 {
92  _opened = _started = false;
93  _nao_hacks = _switch_u_v = false;
94  _width = _height = _bytes_per_line = _fps = _buffers_length = 0;
95  _current_buffer = -1;
96  _brightness.set = _contrast.set = _saturation.set = _hue.set =
97  _red_balance.set = _blue_balance.set = _exposure.set = _gain.set =
98  _lens_x.set = _lens_y.set = false;
99  _aec = _awb = _agc = _h_flip = _v_flip = NOT_SET;
100  _read_method = MMAP;
101  memset(_format, 0, 5);
102  _frame_buffers = NULL;
103  _capture_time = NULL;
104  _device_name = strdup(device_name);
105  _data = new V4L2CameraData();
106 }
107 
108 
109 /** Constructor.
110  * Initialize camera with parameters from camera argument parser.
111  * Supported arguments:
112  * *Required:
113  * - device=DEV, device file, for example /dev/video0 (required)
114  * *Optional:
115  * - read_method=METHOD, preferred read method
116  * READ: read()
117  * MMAP: memory mapping
118  * UPTR: user pointer
119  * - format=FOURCC, preferred format
120  * - size=WIDTHxHEIGHT, preferred image size
121  * - switch_u_v=true/false, switch U and V channels
122  * - fps=FPS, frames per second
123  * - aec=true/false, Auto Exposition enabled [warning: only valid on nao]
124  * - awb=true/false, Auto White Balance enabled
125  * - agc=true/false, Auto Gain enabled
126  * - h_flip=true/false, Horizontal mirror
127  * - v_flip=true/false, Vertical mirror
128  * - brightness=BRIGHT, Brightness [0-255] (def. 128)
129  * - contrast=CONTR, Contrast [0-127] (def. 64)
130  * - saturation=SAT, Saturation [0-256] (def. 128)
131  * - hue=HUE, Hue [-180-180] (def. 0)
132  * - red_balance=RB, Red Balance [0-255] (def. 128)
133  * - blue_balance=BB, Blue Balance [0-255] (def. 128)
134  * - exposure=EXP, Exposure [0-65535] (def. 60)
135  * - gain=GAIN, Gain [0-255] (def. 0)
136  * - lens_x=CORR, Lens Correction X [0-255] (def. 0)
137  * - lens_y=CORR, Lens Correction Y [0-255] (def. 0)
138  * @param cap camera argument parser
139  */
140 V4L2Camera::V4L2Camera(const CameraArgumentParser *cap)
141 {
142  _opened = _started = false;
143  _nao_hacks = false;
144  _width = _height = _bytes_per_line = _buffers_length = 0;
145  _current_buffer = -1;
146  _frame_buffers = NULL;
147  _capture_time = NULL;
148  _data = new V4L2CameraData();
149 
150  if (cap->has("device")) _device_name = strdup(cap->get("device").c_str());
151  else throw MissingParameterException("V4L2Cam: Missing device");
152 
153 
154  if (cap->has("read_method"))
155  {
156  string rm = cap->get("read_method");
157  if (rm.compare("READ") == 0) _read_method = READ;
158  else if (rm.compare("MMAP") == 0) _read_method = MMAP;
159  else if (rm.compare("UPTR") == 0) _read_method = UPTR;
160  else throw Exception("V4L2Cam: Invalid read method");
161  }
162  else
163  {
164  _read_method = MMAP;
165  }
166 
167 
168  if (cap->has("format"))
169  {
170  string fmt = cap->get("format");
171  if (fmt.length() != 4) throw Exception("V4L2Cam: Invalid format fourcc");
172  strncpy(_format, fmt.c_str(), 4);
173  _format[4] = '\0';
174  }
175  else
176  {
177  memset(_format, 0, 5);
178  }
179 
180 
181  if (cap->has("size"))
182  {
183  string size = cap->get("size");
184  string::size_type pos;
185  if ((pos = size.find('x')) == string::npos) throw Exception("V4L2Cam: invalid image size string");
186  if ((pos == (size.length() - 1))) throw Exception("V4L2Cam: invalid image size string");
187 
188  unsigned int mult = 1;
189  for (string::size_type i = pos - 1; i != string::npos; --i)
190  {
191  _width += (size.at(i) - '0') * mult;
192  mult *= 10;
193  }
194 
195  mult = 1;
196  for (string::size_type i = size.length() - 1; i > pos; --i)
197  {
198  _height += (size.at(i) - '0') * mult;
199  mult *= 10;
200  }
201  }
202 
203 
204  if (cap->has("switch_u_v"))
205  {
206  _switch_u_v = (cap->get("switch_u_v").compare("true") == 0);
207  }
208  else
209  {
210  _switch_u_v = false;
211  }
212 
213 
214  if (cap->has("fps"))
215  {
216  if ((_fps = atoi(cap->get("fps").c_str())) == 0) throw Exception("V4L2Cam: invalid fps string");
217  }
218  else
219  {
220  _fps = 0;
221  }
222 
223 
224  if (cap->has("aec"))
225  {
226  _aec = (cap->get("aec").compare("true") == 0 ? TRUE : FALSE);
227  }
228  else
229  {
230  _aec = NOT_SET;
231  }
232 
233 
234  if (cap->has("awb"))
235  {
236  _awb = (cap->get("awb").compare("true") == 0 ? TRUE : FALSE);
237  }
238  else
239  {
240  _awb = NOT_SET;
241  }
242 
243 
244  if (cap->has("agc"))
245  {
246  _agc = (cap->get("agc").compare("true") == 0 ? TRUE : FALSE);
247  }
248  else
249  {
250  _agc = NOT_SET;
251  }
252 
253 
254  if (cap->has("h_flip"))
255  {
256  _h_flip = (cap->get("h_flip").compare("true") == 0 ? TRUE : FALSE);
257  }
258  else
259  {
260  _h_flip = NOT_SET;
261  }
262 
263 
264  if (cap->has("v_flip"))
265  {
266  _v_flip = (cap->get("v_flip").compare("true") == 0 ? TRUE : FALSE);
267  }
268  else
269  {
270  _v_flip = NOT_SET;
271  }
272 
273 
274  if (cap->has("brightness"))
275  {
276  _brightness.set = true;
277  _brightness.value = atoi(cap->get("brightness").c_str());
278  }
279  else
280  {
281  _brightness.set = false;
282  }
283 
284 
285  if (cap->has("contrast"))
286  {
287  _contrast.set = true;
288  _contrast.value = atoi(cap->get("contrast").c_str());
289  }
290  else
291  {
292  _contrast.set = false;
293  }
294 
295 
296  if (cap->has("saturation"))
297  {
298  _saturation.set = true;
299  _saturation.value = atoi(cap->get("saturation").c_str());
300  }
301  else
302  {
303  _saturation.set = false;
304  }
305 
306 
307  if (cap->has("hue"))
308  {
309  _hue.set = true;
310  _hue.value = atoi(cap->get("hue").c_str());
311  }
312  else
313  {
314  _hue.set = false;
315  }
316 
317 
318  if (cap->has("red_balance"))
319  {
320  _red_balance.set = true;
321  _red_balance.value = atoi(cap->get("red_balance").c_str());
322  }
323  else
324  {
325  _red_balance.set = false;
326  }
327 
328 
329  if (cap->has("blue_balance"))
330  {
331  _blue_balance.set = true;
332  _blue_balance.value = atoi(cap->get("blue_balance").c_str());
333  }
334  else
335  {
336  _blue_balance.set = false;
337  }
338 
339 
340  if (cap->has("exposure"))
341  {
342  _exposure.set = true;
343  _exposure.value = atoi(cap->get("exposure").c_str());
344  }
345  else
346  {
347  _exposure.set = false;
348  }
349 
350 
351  if (cap->has("gain"))
352  {
353  _gain.set = true;
354  _gain.value = atoi(cap->get("gain").c_str());
355  }
356  else
357  {
358  _gain.set = false;
359  }
360 
361 
362  if (cap->has("lens_x"))
363  {
364  _lens_x.set = true;
365  _lens_x.value = atoi(cap->get("lens_x").c_str());
366  }
367  else
368  {
369  _lens_x.set = false;
370  }
371 
372 
373  if (cap->has("lens_y"))
374  {
375  _lens_y.set = true;
376  _lens_y.value = atoi(cap->get("lens_y").c_str());
377  }
378  else
379  {
380  _lens_y.set = false;
381  }
382 }
383 
384 
385 /** Protected Constructor.
386  * Gets called from V4LCamera, when the device has already been opened
387  * and determined to be a V4L2 device.
388  * @param device_name device file name (e.g. /dev/video0)
389  * @param dev file descriptor of the opened device
390  */
391 V4L2Camera::V4L2Camera(const char *device_name, int dev)
392 {
393  _opened = true;
394  _started = false;
395  _nao_hacks = _switch_u_v = false;
396  _width = _height = _bytes_per_line = _buffers_length = _fps = 0;
397  _current_buffer = -1;
398  _brightness.set = _contrast.set = _saturation.set = _hue.set =
399  _red_balance.set = _blue_balance.set = _exposure.set = _gain.set =
400  _lens_x.set = _lens_y.set = false;
401  _aec = _awb = _agc = _h_flip = _v_flip = NOT_SET;
402  _read_method = UPTR;
403  memset(_format, 0, 5);
404  _frame_buffers = NULL;
405  _capture_time = NULL;
406  _device_name = strdup(device_name);
407  _data = new V4L2CameraData();
408 
409  _dev = dev;
410 
411  // getting capabilities
412  if (v4l2_ioctl(_dev, VIDIOC_QUERYCAP, &_data->caps))
413  {
414  close();
415  throw Exception("V4L2Cam: Could not get capabilities - probably not a v4l2 device");
416  }
417 
418  post_open();
419 }
420 
421 /** Destructor. */
422 V4L2Camera::~V4L2Camera()
423 {
424  if (_started) stop();
425  if (_opened) close();
426 
427  free(_device_name);
428  delete _data;
429 }
430 
431 void
432 V4L2Camera::open()
433 {
434  if (_started) stop();
435  if(_opened) close();
436 
437  _dev = ::open(_device_name, O_RDWR);
438  int libv4l2_fd = v4l2_fd_open(_dev, 0);
439  if (libv4l2_fd != -1) _dev = libv4l2_fd;
440  /* Note the v4l2_xxx functions are designed so that if they get passed an
441  unknown fd, the will behave exactly as their regular xxx counterparts, so
442  if v4l2_fd_open fails, we continue as normal (missing the libv4l2 custom
443  cam format to normal formats conversion). Chances are big we will still
444  fail then though, as normally v4l2_fd_open only fails if the device is not
445  a v4l2 device. */
446  if (_dev < 0) throw Exception("V4L2Cam: Could not open device");
447 
448  _opened = true;
449 
450  // getting capabilities
451  if (v4l2_ioctl(_dev, VIDIOC_QUERYCAP, &_data->caps))
452  {
453  close();
454  throw Exception("V4L2Cam: Could not get capabilities - probably not a v4l2 device");
455  }
456 
457  post_open();
458 }
459 
460 /**
461  * Post-open() operations.
462  * Precondition: _dev (file desc) and _data->caps (capabilities) are set.
463  * @param dev file descriptor of the opened device
464  */
465 void
466 V4L2Camera::post_open()
467 {
468  //LibLogger::log_debug("V4L2Cam", "select_read_method()");
469  select_read_method();
470 
471  //LibLogger::log_debug("V4L2Cam", "select_format()");
472  select_format();
473 
474  if (_fps)
475  {
476  //LibLogger::log_debug("V4L2Cam", "set_fps()");
477  set_fps();
478  }
479 
480  //LibLogger::log_debug("V4L2Cam", "set_controls()");
481  set_controls();
482 
483  //LibLogger::log_debug("V4L2Cam", "create_buffer()");
484  create_buffer();
485 
486  //LibLogger::log_debug("V4L2Cam", "reset_cropping()");
487  reset_cropping();
488 
489 }
490 
491 /**
492  * Find suitable reading method.
493  * The one set in _read_method is preferred.
494  * Postconditions:
495  * - _read_method and _buffers_length are set
496  */
497 void
498 V4L2Camera::select_read_method()
499 {
500  /* try preferred method */
501  if (!(_data->caps.capabilities &
502  (_read_method == READ ? V4L2_CAP_READWRITE : V4L2_CAP_STREAMING)))
503  {
504  /* preferred read method not supported - try next */
505  _read_method = (_read_method == READ ? MMAP : READ);
506  if (!(_data->caps.capabilities &
507  (_read_method == READ ? V4L2_CAP_READWRITE : V4L2_CAP_STREAMING)))
508  {
509  close();
510  throw Exception("V4L2Cam: Neither read() nor streaming IO supported");
511  }
512  }
513 
514  if (_read_method != READ)
515  {
516  v4l2_requestbuffers buf;
517 
518  /* Streaming IO - Try 1st method, and if that fails 2nd */
519  for (int i = 0; i < 2; ++i)
520  {
521  if (_read_method == MMAP)
522  {
523  _buffers_length = MMAP_NUM_BUFFERS;
524  buf.count = _buffers_length;
525  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
526  buf.memory = V4L2_MEMORY_MMAP;
527  }
528  else /* UPTR */
529  {
530  _buffers_length = 0;
531  buf.count = 0;
532  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
533  buf.memory = V4L2_MEMORY_USERPTR;
534  }
535 
536  if (v4l2_ioctl(_dev, VIDIOC_REQBUFS, &buf))
537  {
538  if (errno != EINVAL)
539  {
540  close();
541  throw Exception("V4L2Cam: REQBUFS query failed");
542  }
543 
544  /* Not supported */
545  if (i == 1)
546  {
547  close();
548  throw Exception("V4L2Cam: Neither memory mapped nor user pointer IO supported");
549  }
550 
551  /* try other method */
552  _read_method = (_read_method == MMAP ? UPTR : MMAP);
553  continue;
554  }
555 
556  /* Method supported */
557  if ((_read_method == MMAP) && (buf.count < _buffers_length))
558  {
559  close();
560  throw Exception("V4L2Cam: Not enough memory for the buffers");
561  }
562 
563  break;
564  }
565  }
566  else /* Read IO */
567  {
568  _buffers_length = 1;
569  }
570 
571  switch (_read_method)
572  {
573  case READ:
574  LibLogger::log_debug("V4L2Cam", "Using read() method");
575  break;
576 
577  case MMAP:
578  LibLogger::log_debug("V4L2Cam", "Using memory mapping method");
579  break;
580 
581  case UPTR:
582  LibLogger::log_debug("V4L2Cam", "Using user pointer method");
583  //TODO
584  throw Exception("V4L2Cam: user pointer method not supported yet");
585  break;
586  }
587 }
588 
589 /**
590  * Find suitable image format.
591  * The one set in _format (if any) is preferred.
592  * Postconditions:
593  * - _format is set (and selected)
594  * - _colorspace is set accordingly
595  * - _width, _height, and _bytes_per_line are set
596  */
597 void
598 V4L2Camera::select_format()
599 {
600  bool preferred_found = false;
601  v4l2_fmtdesc format_desc;
602 
603  char fourcc[5] = " ";
604 
605  if (strcmp(_format, ""))
606  {
607  /* Try to select preferred format */
608  memset(&format_desc, 0, sizeof(format_desc));
609  format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
610  for (format_desc.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUM_FMT, &format_desc) == 0; format_desc.index++)
611  {
612  fourcc[0] = static_cast<char>(format_desc.pixelformat & 0xFF);
613  fourcc[1] = static_cast<char>((format_desc.pixelformat >> 8) & 0xFF);
614  fourcc[2] = static_cast<char>((format_desc.pixelformat >> 16) & 0xFF);
615  fourcc[3] = static_cast<char>((format_desc.pixelformat >> 24) & 0xFF);
616 
617  if (strcmp(_format, fourcc) == 0)
618  {
619  preferred_found = true;
620  break;
621  }
622  }
623  }
624 
625  if (!preferred_found)
626  {
627  /* Preferred format not found (or none selected)
628  -> just take first available format */
629  memset(&format_desc, 0, sizeof(format_desc));
630  format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
631  format_desc.index = 0;
632  if (v4l2_ioctl(_dev, VIDIOC_ENUM_FMT, &format_desc))
633  {
634  close();
635  throw Exception("V4L2Cam: No image format found");
636  }
637 
638  fourcc[0] = static_cast<char>(format_desc.pixelformat & 0xFF);
639  fourcc[1] = static_cast<char>((format_desc.pixelformat >> 8) & 0xFF);
640  fourcc[2] = static_cast<char>((format_desc.pixelformat >> 16) & 0xFF);
641  fourcc[3] = static_cast<char>((format_desc.pixelformat >> 24) & 0xFF);
642  }
643 
644  /* Now set this format */
645  v4l2_format format;
646  memset(&format, 0, sizeof(format));
647  format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
648  if (v4l2_ioctl(_dev, VIDIOC_G_FMT, &format))
649  {
650  close();
651  throw Exception("V4L2Cam: Format query failed");
652  }
653 
654  //LibLogger::log_debug("V4L2Cam", "setting %dx%d (%s) - type %d", _width, _height, fourcc, format.type);
655 
656  format.fmt.pix.pixelformat = v4l2_fourcc(fourcc[0], fourcc[1], fourcc[2], fourcc[3]);
657  format.fmt.pix.field = V4L2_FIELD_ANY;
658  if (_width)
659  format.fmt.pix.width = _width;
660  if (_height)
661  format.fmt.pix.height = _height;
662 
663  if (v4l2_ioctl(_dev, VIDIOC_S_FMT, &format))
664  {
665  //throw Exception(errno, "Failed to set video format");
666  //}
667 
668  // Nao workaround (Hack alert)
669  LibLogger::log_warn("V4L2Cam", "Format setting failed (driver sucks) - %d: %s", errno, strerror(errno));
670  LibLogger::log_info("V4L2Cam", "Trying workaround");
671  _nao_hacks = true;
672 
673  v4l2_std_id std;
674  if (v4l2_ioctl(_dev, VIDIOC_G_STD, &std))
675  {
676  close();
677  throw Exception("V4L2Cam: Standard query (workaround) failed");
678  }
679 
680  if ((_width == 320) && (_height == 240))
681  {
682  std = 0x04000000UL; // QVGA
683  }
684  else
685  {
686  std = 0x08000000UL; // VGA
687  _width = 640;
688  _height = 480;
689  }
690  if (v4l2_ioctl(_dev, VIDIOC_S_STD, &std))
691  {
692  close();
693  throw Exception("V4L2Cam: Standard setting (workaround) failed");
694  }
695 
696  format.fmt.pix.width = _width;
697  format.fmt.pix.height = _height;
698  format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
699  format.fmt.pix.field = V4L2_FIELD_ANY;
700 
701  if (v4l2_ioctl(_dev, VIDIOC_S_FMT, &format))
702  {
703  close();
704  throw Exception("V4L2Cam: Format setting (workaround) failed");
705  }
706 
707  if (_switch_u_v) _colorspace = YVY2;
708  }
709 
710  /* ...and store final values */
711  _format[0] = static_cast<char>(format.fmt.pix.pixelformat & 0xFF);
712  _format[1] = static_cast<char>((format.fmt.pix.pixelformat >> 8) & 0xFF);
713  _format[2] = static_cast<char>((format.fmt.pix.pixelformat >> 16) & 0xFF);
714  _format[3] = static_cast<char>((format.fmt.pix.pixelformat >> 24) & 0xFF);
715 
716  if (!_nao_hacks || !_switch_u_v)
717  {
718  if (strcmp(_format, "RGB3") == 0) _colorspace = RGB;
719  else if (strcmp(_format, "Y41P") == 0) _colorspace = YUV411_PACKED; //different byte ordering
720  else if (strcmp(_format, "411P") == 0) _colorspace = YUV411_PLANAR;
721  else if (strcmp(_format, "YUYV") == 0) _colorspace = YUY2;
722  else if (strcmp(_format, "BGR3") == 0) _colorspace = BGR;
723  else if (strcmp(_format, "UYVY") == 0) _colorspace = YUV422_PACKED;
724  else if (strcmp(_format, "422P") == 0) _colorspace = YUV422_PLANAR;
725  else if (strcmp(_format, "GREY") == 0) _colorspace = GRAY8;
726  else if (strcmp(_format, "RGB4") == 0) _colorspace = RGB_WITH_ALPHA;
727  else if (strcmp(_format, "BGR4") == 0) _colorspace = BGR_WITH_ALPHA;
728  else if (strcmp(_format, "BA81") == 0) _colorspace = BAYER_MOSAIC_BGGR;
729  else if (strcmp(_format, "Y16 ") == 0) _colorspace = MONO16;
730  else _colorspace = CS_UNKNOWN;
731  }
732 
733  if (!_nao_hacks)
734  {
735  _width = format.fmt.pix.width;
736  _height = format.fmt.pix.height;
737  }
738 
739  _bytes_per_line = format.fmt.pix.bytesperline;
740 
741  /* Hack for bad drivers */
742  if (_bytes_per_line == 0)
743  {
744  LibLogger::log_warn("V4L2Cam", "bytesperline is 0 (driver sucks)");
745  _bytes_per_line = colorspace_buffer_size(_colorspace, _width, _height) / _height;
746  }
747 
748  LibLogger::log_debug("V4L2Cam", "w%d h%d bpl%d cs%d fmt%s", _width, _height, _bytes_per_line, _colorspace, _format);
749 }
750 
751 /**
752  * Set desired FPS count.
753  */
754 void
755 V4L2Camera::set_fps()
756 {
757  v4l2_streamparm param;
758  param.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
759  if (v4l2_ioctl(_dev, VIDIOC_G_PARM, &param))
760  {
761  close();
762  throw Exception("V4L2Cam: Streaming parameter query failed");
763  }
764 
765  if (!(param.parm.capture.capability & V4L2_CAP_TIMEPERFRAME))
766  {
767  LibLogger::log_warn("V4L2Cam", "FPS change not supported");
768  return;
769  }
770 
771  param.parm.capture.timeperframe.numerator = 1;
772  param.parm.capture.timeperframe.denominator = _fps;
773  if (v4l2_ioctl(_dev, VIDIOC_S_PARM, &param))
774  {
775  close();
776  throw Exception("V4L2Cam: Streaming parameter setting failed");
777  }
778  else
779  {
780  LibLogger::log_debug("V4L2Cam", "FPS set - %d/%d",
781  param.parm.capture.timeperframe.numerator,
782  param.parm.capture.timeperframe.denominator);
783  }
784 }
785 
786 /**
787  * Set Camera controls.
788  */
789 void
790 V4L2Camera::set_controls()
791 {
792  if (_aec != NOT_SET) set_auto_exposure(_aec == TRUE);
793  if (_awb != NOT_SET) set_auto_white_balance(_awb == TRUE);
794  if (_agc != NOT_SET) set_auto_gain(_agc == TRUE);
795 
796  if (_h_flip != NOT_SET) set_horiz_mirror(_h_flip == TRUE);
797  if (_v_flip != NOT_SET) set_vert_mirror(_v_flip == TRUE);
798 
799  if (_brightness.set) set_brightness(_brightness.value);
800  if (_contrast.set) set_contrast(_contrast.value);
801  if (_saturation.set) set_saturation(_saturation.value);
802  if (_hue.set) set_hue(_hue.value);
803  if (_red_balance.set) set_red_balance(_red_balance.value);
804  if (_blue_balance.set) set_blue_balance(_blue_balance.value);
805  if (_exposure.set) set_exposure(_exposure.value);
806  if (_gain.set) set_gain(_gain.value);
807  if (_lens_x.set) set_lens_x_corr(_lens_x.value);
808  if (_lens_y.set) set_lens_y_corr(_lens_y.value);
809 }
810 
811 /**
812  * Set one Camera control value.
813  * @param ctrl name of the value
814  * @param id ID of the value
815  * @param value value to set
816  */
817 void
818 V4L2Camera::set_one_control(const char *ctrl, unsigned int id, int value)
819 {
820  v4l2_queryctrl queryctrl;
821  v4l2_control control;
822 
823  memset(&queryctrl, 0, sizeof(queryctrl));
824  queryctrl.id = id;
825 
826  if (v4l2_ioctl(_dev, VIDIOC_QUERYCTRL, &queryctrl))
827  {
828  if (errno == EINVAL)
829  {
830  LibLogger::log_error("V4L2Cam", "Control %s not supported", ctrl);
831  return;
832  }
833 
834  close();
835  throw Exception("V4L2Cam: %s Control query failed", ctrl);
836  }
837  if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
838  {
839  LibLogger::log_error("V4L2Cam", "Control %s disabled", ctrl);
840  return;
841  }
842 
843  memset(&control, 0, sizeof(control));
844  control.id = id;
845  control.value = value;
846 
847  if (v4l2_ioctl(_dev, VIDIOC_S_CTRL, &control))
848  {
849  close();
850  throw Exception("V4L2Cam: %s Control setting failed", ctrl);
851  }
852 }
853 
854 /**
855  * Get one Camera control value.
856  * @param ctrl name of the value
857  * @param id ID of the value
858  * @return current value
859  */
860 int
861 V4L2Camera::get_one_control(const char *ctrl, unsigned int id)
862 {
863  v4l2_queryctrl queryctrl;
864  v4l2_control control;
865 
866  memset(&queryctrl, 0, sizeof(queryctrl));
867  queryctrl.id = id;
868 
869  if (v4l2_ioctl(_dev, VIDIOC_QUERYCTRL, &queryctrl))
870  {
871  if (errno == EINVAL)
872  {
873  LibLogger::log_error("V4L2Cam", "Control %s not supported", ctrl);
874  return 0;
875  }
876 
877  close();
878  throw Exception("V4L2Cam: %s Control query failed", ctrl);
879  }
880  if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
881  {
882  LibLogger::log_error("V4L2Cam", "Control %s disabled", ctrl);
883  return 0;
884  }
885 
886  memset(&control, 0, sizeof(control));
887  control.id = id;
888 
889  if (v4l2_ioctl(_dev, VIDIOC_G_CTRL, &control))
890  {
891  close();
892  throw Exception("V4L2Cam: %s Control value reading failed", ctrl);
893  }
894 
895  return control.value;
896 }
897 
898 /**
899  * Create a buffer for image transfer.
900  * Preconditions:
901  * - _read_method is set
902  * - _height and _bytes_per_line are set
903  * - _buffers_length is set
904  * Postconditions:
905  * - _frame_buffers is set up
906  */
907 void
908 V4L2Camera::create_buffer()
909 {
910  _frame_buffers = new FrameBuffer[_buffers_length];
911 
912  switch (_read_method)
913  {
914  case READ:
915  {
916  _frame_buffers[0].size = _bytes_per_line * _height;
917  _frame_buffers[0].buffer = static_cast<unsigned char *>(malloc(_frame_buffers[0].size));
918  if (_frame_buffers[0].buffer == NULL)
919  {
920  close();
921  throw Exception("V4L2Cam: Out of memory");
922  }
923  break;
924  }
925 
926  case MMAP:
927  {
928  for (unsigned int i = 0; i < _buffers_length; ++i)
929  {
930  /* Query status of buffer */
931  v4l2_buffer buffer;
932 
933  memset(&buffer, 0, sizeof (buffer));
934  buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
935  buffer.memory = V4L2_MEMORY_MMAP;
936  buffer.index = i;
937 
938  if (v4l2_ioctl(_dev, VIDIOC_QUERYBUF, &buffer))
939  {
940  close();
941  throw Exception("V4L2Cam: Buffer query failed");
942  }
943 
944  _frame_buffers[i].size = buffer.length;
945  _frame_buffers[i].buffer = static_cast<unsigned char *>(
946  v4l2_mmap(NULL, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, _dev, buffer.m.offset)
947  );
948  if (_frame_buffers[i].buffer == MAP_FAILED)
949  {
950  close();
951  throw Exception("V4L2Cam: Memory mapping failed");
952  }
953  }
954 
955  break;
956  }
957 
958  case UPTR:
959  /* not supported yet */
960  break;
961  }
962 }
963 
964 /**
965  * Reset cropping parameters.
966  */
967 void
968 V4L2Camera::reset_cropping()
969 {
970  v4l2_cropcap cropcap;
971  v4l2_crop crop;
972 
973  memset(&cropcap, 0, sizeof(cropcap));
974  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
975 
976  if (v4l2_ioctl(_dev, VIDIOC_CROPCAP, &cropcap))
977  {
978  LibLogger::log_warn("V4L2Cam", "cropcap query failed (driver sucks) - %d: %s", errno, strerror(errno));
979  }
980 
981  memset(&crop, 0, sizeof(crop));
982  crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
983  crop.c = cropcap.defrect;
984 
985  /* Ignore if cropping is not supported (EINVAL). */
986  if (v4l2_ioctl(_dev, VIDIOC_S_CROP, &crop) && errno != EINVAL)
987  {
988  LibLogger::log_warn("V4L2Cam", "cropping query failed (driver sucks) - %d: %s", errno, strerror(errno));
989  }
990 }
991 
992 void
993 V4L2Camera::close()
994 {
995  //LibLogger::log_debug("V4L2Cam", "close()");
996 
997  if (_started) stop();
998 
999  if (_frame_buffers)
1000  {
1001  switch (_read_method)
1002  {
1003  case READ:
1004  {
1005  free(_frame_buffers[0].buffer);
1006  break;
1007  }
1008 
1009  case MMAP:
1010  {
1011  for (unsigned int i = 0; i < _buffers_length; ++i)
1012  {
1013  v4l2_munmap(_frame_buffers[i].buffer, _frame_buffers[i].size);
1014  }
1015  break;
1016  }
1017 
1018  case UPTR:
1019  /* not supported yet */
1020  break;
1021  }
1022  delete[] _frame_buffers;
1023  _frame_buffers = NULL;
1024  _current_buffer = -1;
1025  }
1026 
1027  if (_opened)
1028  {
1029  v4l2_close(_dev);
1030  _opened = false;
1031  _dev = 0;
1032  }
1033 
1034  if (_capture_time)
1035  {
1036  delete _capture_time;
1037  _capture_time = 0;
1038  }
1039 }
1040 
1041 void
1042 V4L2Camera::start()
1043 {
1044  //LibLogger::log_info("V4L2Cam", "start()");
1045 
1046  if (!_opened) throw Exception("VL42Cam: Camera not opened");
1047 
1048  if (_started) stop();
1049 
1050  switch (_read_method)
1051  {
1052  case READ:
1053  /* nothing to do here */
1054  break;
1055 
1056  case MMAP:
1057  {
1058  /* enqueue buffers */
1059  for (unsigned int i = 0; i < _buffers_length; ++i)
1060  {
1061  v4l2_buffer buffer;
1062  memset(&buffer, 0, sizeof(buffer));
1063  buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1064  buffer.memory = V4L2_MEMORY_MMAP;
1065  buffer.index = i;
1066 
1067  if (v4l2_ioctl(_dev, VIDIOC_QBUF, &buffer))
1068  {
1069  close();
1070  throw Exception("V4L2Cam: Enqueuing buffer failed");
1071  }
1072  }
1073 
1074  /* start streaming */
1075  int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1076  if (v4l2_ioctl(_dev, VIDIOC_STREAMON, &type))
1077  {
1078  close();
1079  throw Exception("V4L2Cam: Starting stream failed");
1080  }
1081  break;
1082  }
1083 
1084  case UPTR:
1085  /* not supported yet */
1086  break;
1087  }
1088 
1089  //LibLogger::log_debug("V4L2Cam", "start() complete");
1090  _started = true;
1091 }
1092 
1093 void
1094 V4L2Camera::stop()
1095 {
1096  //LibLogger::log_debug("V4L2Cam", "stop()");
1097 
1098  if (!_started) return;
1099 
1100  switch (_read_method)
1101  {
1102  case READ:
1103  /* nothing to do here */
1104  break;
1105 
1106  case MMAP: /* fall through */
1107  case UPTR:
1108  {
1109  /* stop streaming */
1110  int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1111  if (v4l2_ioctl(_dev, VIDIOC_STREAMOFF, &type))
1112  {
1113  close();
1114  throw Exception("V4L2Cam: Stopping stream failed");
1115  }
1116  break;
1117  }
1118  }
1119 
1120  _current_buffer = -1;
1121  _started = false;
1122 }
1123 
1124 bool
1125 V4L2Camera::ready()
1126 {
1127  //LibLogger::log_debug("V4L2Cam", "ready()");
1128 
1129  return _started;
1130 }
1131 
1132 void
1133 V4L2Camera::flush()
1134 {
1135  //LibLogger::log_debug("V4L2Cam", "flush()");
1136  /* not needed */
1137 }
1138 
1139 void
1140 V4L2Camera::capture()
1141 {
1142  //LibLogger::log_debug("V4L2Cam", "capture()");
1143 
1144  if (!_started) return;
1145 
1146  switch (_read_method)
1147  {
1148  case READ:
1149  {
1150  _current_buffer = 0;
1151  //LibLogger::log_debug("V4L2Cam", "calling read()");
1152  if (v4l2_read(_dev, _frame_buffers[_current_buffer].buffer, _frame_buffers[_current_buffer].size) == -1)
1153  {
1154  //TODO: errno handling
1155  LibLogger::log_warn("V4L2Cam", "read() failed with code %d: %s", errno, strerror(errno));
1156  }
1157  //LibLogger::log_debug("V4L2Cam", "returned from read()");
1158 
1159  //No timestamping support here - just take current system time
1160  if (_capture_time)
1161  {
1162  _capture_time->stamp();
1163  }
1164  else
1165  {
1166  _capture_time = new fawkes::Time();
1167  }
1168 
1169  break;
1170  }
1171 
1172  case MMAP:
1173  {
1174  /* dequeue buffer */
1175  v4l2_buffer buffer;
1176  memset(&buffer, 0, sizeof(buffer));
1177  buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1178  buffer.memory = V4L2_MEMORY_MMAP;
1179 
1180  if (v4l2_ioctl(_dev, VIDIOC_DQBUF, &buffer))
1181  {
1182  //TODO: errno handling -> EAGAIN, ...?
1183  close();
1184  throw Exception("V4L2Cam: Dequeuing buffer failed");
1185  }
1186 
1187  _current_buffer = buffer.index;
1188 
1189  if (_capture_time)
1190  {
1191  _capture_time->set_time(&buffer.timestamp);
1192  }
1193  else
1194  {
1195  _capture_time = new fawkes::Time(&buffer.timestamp);
1196  }
1197  break;
1198  }
1199 
1200  case UPTR:
1201  /* not supported yet */
1202  break;
1203  }
1204 }
1205 
1206 unsigned char *
1207 V4L2Camera::buffer()
1208 {
1209  //LibLogger::log_debug("V4L2Cam", "buffer()");
1210 
1211  return (_current_buffer == -1 ? NULL : _frame_buffers[_current_buffer].buffer);
1212 }
1213 
1214 unsigned int
1215 V4L2Camera::buffer_size()
1216 {
1217  //LibLogger::log_debug("V4L2Cam", "buffer_size()");
1218 
1219  return (_opened && (_current_buffer != -1) ? _frame_buffers[_current_buffer].size : 0);
1220 }
1221 
1222 void
1223 V4L2Camera::dispose_buffer()
1224 {
1225  //LibLogger::log_debug("V4L2Cam", "dispose_buffer()");
1226 
1227  if (!_opened) return;
1228 
1229  switch (_read_method)
1230  {
1231  case READ:
1232  /* nothing to do here */
1233  break;
1234 
1235  case MMAP:
1236  {
1237  /* enqueue next buffer */
1238  v4l2_buffer buffer;
1239  memset(&buffer, 0, sizeof(buffer));
1240  buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1241  buffer.memory = V4L2_MEMORY_MMAP;
1242  buffer.index = _current_buffer;
1243 
1244  //TODO: Test if the next buffer is also the latest buffer (VIDIOC_QUERYBUF)
1245  if (v4l2_ioctl(_dev, VIDIOC_QBUF, &buffer))
1246  {
1247  int errno_save = errno;
1248  close();
1249  throw Exception(errno_save, "V4L2Cam: Enqueuing buffer failed");
1250  }
1251  break;
1252  }
1253 
1254  case UPTR:
1255  /* not supported yet */
1256  break;
1257  }
1258 
1259  _current_buffer = -1;
1260 }
1261 
1262 unsigned int
1263 V4L2Camera::pixel_width()
1264 {
1265  //LibLogger::log_debug("V4L2Cam", "pixel_width()");
1266 
1267  return _width;
1268 }
1269 
1270 unsigned int
1271 V4L2Camera::pixel_height()
1272 {
1273  //LibLogger::log_debug("V4L2Cam", "pixel_height()");
1274 
1275  return _height;
1276 }
1277 
1278 colorspace_t
1279 V4L2Camera::colorspace()
1280 {
1281  //LibLogger::log_debug("V4L2Cam", "colorspace()");
1282 
1283  if (!_opened)
1284  return CS_UNKNOWN;
1285  else
1286  return _colorspace;
1287 }
1288 
1289 fawkes::Time *
1290 V4L2Camera::capture_time()
1291 {
1292  return _capture_time;
1293 }
1294 
1295 void
1296 V4L2Camera::set_image_number(unsigned int n)
1297 {
1298  //LibLogger::log_debug("V4L2Cam", "set_image_number(%d)", n);
1299 
1300  /* not needed */
1301 }
1302 
1303 
1304 /* --- CameraControls --- */
1305 
1306 bool
1307 V4L2Camera::auto_gain()
1308 {
1309  return get_one_control("AGC", V4L2_CID_AUTOGAIN);
1310 }
1311 
1312 void
1313 V4L2Camera::set_auto_gain(bool enabled)
1314 {
1315  LibLogger::log_debug("V4L2Cam", (enabled ? "enabling AGC" : "disabling AGC"));
1316  set_one_control("AGC", V4L2_CID_AUTOGAIN, (enabled ? 1 : 0));
1317 }
1318 
1319 bool
1320 V4L2Camera::auto_white_balance()
1321 {
1322  return get_one_control("AWB", V4L2_CID_AUTO_WHITE_BALANCE);
1323 }
1324 
1325 void
1326 V4L2Camera::set_auto_white_balance(bool enabled)
1327 {
1328  LibLogger::log_debug("V4L2Cam", (enabled ? "enabling AWB" : "disabling AWB"));
1329  set_one_control("AWB", V4L2_CID_AUTO_WHITE_BALANCE, (enabled ? 1 : 0));
1330 }
1331 
1332 bool
1333 V4L2Camera::auto_exposure()
1334 {
1335  throw NotImplementedException("No such method in the V4L2 standard");
1336 }
1337 
1338 void
1339 V4L2Camera::set_auto_exposure(bool enabled)
1340 {
1341  throw NotImplementedException("No such method in the V4L2 standard");
1342 }
1343 
1344 int
1345 V4L2Camera::red_balance()
1346 {
1347  return get_one_control("red balance", V4L2_CID_RED_BALANCE);
1348 }
1349 
1350 void
1351 V4L2Camera::set_red_balance(int red_balance)
1352 {
1353  LibLogger::log_debug("V4L2Cam", "Setting red balance to %d", red_balance);
1354  set_one_control("red balance", V4L2_CID_RED_BALANCE, red_balance);
1355 }
1356 
1357 int
1358 V4L2Camera::blue_balance()
1359 {
1360  return get_one_control("blue balance", V4L2_CID_BLUE_BALANCE);
1361 }
1362 
1363 void
1364 V4L2Camera::set_blue_balance(int blue_balance)
1365 {
1366  LibLogger::log_debug("V4L2Cam", "Setting blue balance to %d", blue_balance);
1367  set_one_control("blue balance", V4L2_CID_BLUE_BALANCE, blue_balance);
1368 }
1369 
1370 int
1371 V4L2Camera::u_balance()
1372 {
1373  throw NotImplementedException("No such method in the V4L2 standard");
1374 }
1375 
1376 void
1377 V4L2Camera::set_u_balance(int u_balance)
1378 {
1379  throw NotImplementedException("No such method in the V4L2 standard");
1380 }
1381 
1382 int
1383 V4L2Camera::v_balance()
1384 {
1385  throw NotImplementedException("No such method in the V4L2 standard");
1386 }
1387 
1388 void
1389 V4L2Camera::set_v_balance(int v_balance)
1390 {
1391  throw NotImplementedException("No such method in the V4L2 standard");
1392 }
1393 
1394 unsigned int
1395 V4L2Camera::brightness()
1396 {
1397  return get_one_control("brightness", V4L2_CID_BRIGHTNESS);
1398 }
1399 
1400 void
1401 V4L2Camera::set_brightness(unsigned int brightness)
1402 {
1403  LibLogger::log_debug("V4L2Cam", "Setting brighness to %d", brightness);
1404  set_one_control("brightness", V4L2_CID_BRIGHTNESS, brightness);
1405 }
1406 
1407 unsigned int
1408 V4L2Camera::contrast()
1409 {
1410  return get_one_control("contrast", V4L2_CID_CONTRAST);
1411 }
1412 
1413 void
1414 V4L2Camera::set_contrast(unsigned int contrast)
1415 {
1416  LibLogger::log_debug("V4L2Cam", "Setting contrast to %d", contrast);
1417  set_one_control("contrast", V4L2_CID_CONTRAST, contrast);
1418 }
1419 
1420 unsigned int
1421 V4L2Camera::saturation()
1422 {
1423  return get_one_control("saturation", V4L2_CID_SATURATION);
1424 }
1425 
1426 void
1427 V4L2Camera::set_saturation(unsigned int saturation)
1428 {
1429  LibLogger::log_debug("V4L2Cam", "Setting saturation to %d", saturation);
1430  set_one_control("saturation", V4L2_CID_SATURATION, saturation);
1431 }
1432 
1433 int
1434 V4L2Camera::hue()
1435 {
1436  return get_one_control("hue", V4L2_CID_HUE);
1437 }
1438 
1439 void
1440 V4L2Camera::set_hue(int hue)
1441 {
1442  LibLogger::log_debug("V4L2Cam", "Setting hue to %d", hue);
1443  set_one_control("hue", V4L2_CID_HUE, hue);
1444 }
1445 
1446 unsigned int
1447 V4L2Camera::exposure()
1448 {
1449  return get_one_control("exposure", V4L2_CID_EXPOSURE);
1450 }
1451 
1452 void
1453 V4L2Camera::set_exposure(unsigned int exposure)
1454 {
1455  LibLogger::log_debug("V4L2Cam", "Setting exposure to %d", exposure);
1456  set_one_control("exposure", V4L2_CID_EXPOSURE, exposure);
1457 }
1458 
1459 unsigned int
1460 V4L2Camera::gain()
1461 {
1462  return get_one_control("gain", V4L2_CID_GAIN);
1463 }
1464 
1465 void
1466 V4L2Camera::set_gain(unsigned int gain)
1467 {
1468  LibLogger::log_debug("V4L2Cam", "Setting gain to %u", gain);
1469  set_one_control("gain", V4L2_CID_GAIN, gain);
1470 }
1471 
1472 
1473 const char *
1474 V4L2Camera::format()
1475 {
1476  return _format;
1477 }
1478 
1479 void
1480 V4L2Camera::set_format(const char *format)
1481 {
1482  strncpy(_format, format, 4);
1483  _format[4] = '\0';
1484  select_format();
1485 }
1486 
1487 unsigned int
1488 V4L2Camera::width()
1489 {
1490  return pixel_width();
1491 }
1492 
1493 unsigned int
1494 V4L2Camera::height()
1495 {
1496  return pixel_height();
1497 }
1498 
1499 void
1500 V4L2Camera::set_size(unsigned int width,
1501  unsigned int height)
1502 {
1503  _width = width;
1504  _height = height;
1505  select_format();
1506 }
1507 
1508 bool
1509 V4L2Camera::horiz_mirror()
1510 {
1511  return (get_one_control("hflip", V4L2_CID_HFLIP) != 0);
1512 }
1513 
1514 bool
1515 V4L2Camera::vert_mirror()
1516 {
1517  return (get_one_control("vflip", V4L2_CID_VFLIP) != 0);
1518 }
1519 
1520 void
1521 V4L2Camera::set_horiz_mirror(bool enabled)
1522 {
1523  LibLogger::log_debug("V4L2Cam", (enabled ? "enabling horizontal flip" : "disabling horizontal flip"));
1524  set_one_control("hflip", V4L2_CID_HFLIP, (enabled ? 1 : 0));
1525 }
1526 
1527 void
1528 V4L2Camera::set_vert_mirror(bool enabled)
1529 {
1530  LibLogger::log_debug("V4L2Cam", (enabled ? "enabling vertical flip" : "disabling vertical flip"));
1531  set_one_control("vflip", V4L2_CID_VFLIP, (enabled ? 1 : 0));
1532 }
1533 
1534 /** Get the number of frames per second that have been requested from the camera.
1535  * A return value of 0 means that fps haven't been set yet through the camera.
1536  * @return the currently requested fps or 0 if not set yet
1537  */
1538 unsigned int
1539 V4L2Camera::fps()
1540 {
1541  return _fps;
1542 }
1543 
1544 void
1545 V4L2Camera::set_fps(unsigned int fps)
1546 {
1547  _fps = fps;
1548  set_fps();
1549 }
1550 
1551 unsigned int
1552 V4L2Camera::lens_x_corr()
1553 {
1554  return get_one_control("lens x", V4L2_CID_PAN_RESET);
1555 }
1556 
1557 unsigned int
1558 V4L2Camera::lens_y_corr()
1559 {
1560  return get_one_control("lens y", V4L2_CID_TILT_RESET);
1561 }
1562 
1563 void
1564 V4L2Camera::set_lens_x_corr(unsigned int x_corr)
1565 {
1566  LibLogger::log_debug("V4L2Cam", "Setting horizontal lens correction to %d", x_corr);
1567  set_one_control("lens x", V4L2_CID_PAN_RESET, x_corr);
1568 }
1569 
1570 void
1571 V4L2Camera::set_lens_y_corr(unsigned int y_corr)
1572 {
1573  LibLogger::log_debug("V4L2Cam", "Setting vertical lens correction to %d", y_corr);
1574  set_one_control("lens x", V4L2_CID_TILT_RESET, y_corr);
1575 }
1576 
1577 
1578 void
1579 V4L2Camera::print_info()
1580 {
1581  /* General capabilities */
1582  cout <<
1583  "=========================================================================="
1584  << endl << _device_name << " (" << _data->caps.card << ") - " << _data->caps.bus_info
1585  << endl << "Driver: " << _data->caps.driver << " (ver " <<
1586  ((_data->caps.version >> 16) & 0xFF) << "." <<
1587  ((_data->caps.version >> 8) & 0xFF) << "." <<
1588  (_data->caps.version & 0xFF) << ")" << endl <<
1589  "--------------------------------------------------------------------------"
1590  << endl;
1591 
1592  /* General capabilities */
1593  cout << "Capabilities:" << endl;
1594  if (_data->caps.capabilities & V4L2_CAP_VIDEO_CAPTURE)
1595  cout << " + Video capture interface supported" << endl;
1596  if (_data->caps.capabilities & V4L2_CAP_VIDEO_OUTPUT)
1597  cout << " + Video output interface supported" << endl;
1598  if (_data->caps.capabilities & V4L2_CAP_VIDEO_OVERLAY)
1599  cout << " + Video overlay interface supported" << endl;
1600  if (_data->caps.capabilities & V4L2_CAP_VBI_CAPTURE)
1601  cout << " + Raw VBI capture interface supported" << endl;
1602  if (_data->caps.capabilities & V4L2_CAP_VBI_OUTPUT)
1603  cout << " + Raw VBI output interface supported" << endl;
1604  if (_data->caps.capabilities & V4L2_CAP_SLICED_VBI_CAPTURE)
1605  cout << " + Sliced VBI capture interface supported" << endl;
1606  if (_data->caps.capabilities & V4L2_CAP_SLICED_VBI_OUTPUT)
1607  cout << " + Sliced VBI output interface supported" << endl;
1608  if (_data->caps.capabilities & V4L2_CAP_RDS_CAPTURE)
1609  cout << " + RDS_CAPTURE set" << endl;
1610  /* Not included in Nao's version
1611  if (caps.capabilities & V4L2_CAP_VIDEO_OUTPUT_OVERLAY)
1612  cout << " + Video output overlay interface supported" << endl; */
1613  if (_data->caps.capabilities & V4L2_CAP_TUNER)
1614  cout << " + Has some sort of tuner" << endl;
1615  if (_data->caps.capabilities & V4L2_CAP_AUDIO)
1616  cout << " + Has audio inputs or outputs" << endl;
1617  if (_data->caps.capabilities & V4L2_CAP_RADIO)
1618  cout << " + Has a radio receiver" << endl;
1619  if (_data->caps.capabilities & V4L2_CAP_READWRITE)
1620  cout << " + read() and write() IO supported" << endl;
1621  if (_data->caps.capabilities & V4L2_CAP_ASYNCIO)
1622  cout << " + asynchronous IO supported" << endl;
1623  if (_data->caps.capabilities & V4L2_CAP_STREAMING)
1624  cout << " + streaming IO supported" << endl;
1625  if (_data->caps.capabilities & V4L2_CAP_TIMEPERFRAME)
1626  cout << " + timeperframe field is supported" << endl;
1627  cout << endl;
1628 
1629  /* Inputs */
1630  cout << "Inputs:" << endl;
1631  v4l2_input input;
1632  memset(&input, 0, sizeof(input));
1633 
1634  for (input.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUMINPUT, &input) == 0; input.index++)
1635  {
1636  cout << "Input " << input.index << ": " << input.name << endl;
1637 
1638  cout << " |- Type: ";
1639  switch (input.type)
1640  {
1641  case V4L2_INPUT_TYPE_TUNER:
1642  cout << "Tuner";
1643  break;
1644 
1645  case V4L2_INPUT_TYPE_CAMERA:
1646  cout << "Camera";
1647  break;
1648 
1649  default:
1650  cout << "Unknown";
1651  }
1652  cout << endl;
1653 
1654  cout << " |- Supported standards:";
1655  if (input.std == 0)
1656  {
1657  cout << " Unknown" << endl;
1658  }
1659  else
1660  {
1661  cout << endl;
1662 
1663  v4l2_standard standard;
1664  memset (&standard, 0, sizeof(standard));
1665  standard.index = 0;
1666 
1667  for (standard.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUMSTD, &standard) == 0; standard.index++)
1668  {
1669  if (standard.id & input.std) cout << " + " << standard.name << endl;
1670  }
1671  }
1672  }
1673  if (input.index == 0) cout << "None" << endl;
1674  cout << endl;
1675 
1676  /* Outputs */
1677  cout << "Outputs:" << endl;
1678  v4l2_output output;
1679  memset (&output, 0, sizeof(output));
1680 
1681  for (output.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUMOUTPUT, &output) == 0; output.index++)
1682  {
1683  cout << " + Output " << output.index << ": " << output.name << endl;
1684 
1685  cout << " |- Type: ";
1686  switch (output.type)
1687  {
1688  case V4L2_OUTPUT_TYPE_MODULATOR:
1689  cout << "TV Modulator";
1690  break;
1691 
1692  case V4L2_OUTPUT_TYPE_ANALOG:
1693  cout << "Analog output";
1694  break;
1695 
1696  default:
1697  cout << "Unknown";
1698  }
1699  cout << endl;
1700 
1701  cout << " |- Supported standards:";
1702  if (output.std == 0)
1703  {
1704  cout << " Unknown" << endl;
1705  }
1706  else
1707  {
1708  cout << endl;
1709 
1710  v4l2_standard standard;
1711  memset (&standard, 0, sizeof (standard));
1712  standard.index = 0;
1713 
1714  for (standard.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUMSTD, &standard) == 0; standard.index++)
1715  {
1716  if (standard.id & output.std) cout << " + " << standard.name << endl;
1717  }
1718  }
1719  }
1720  if (output.index == 0) cout << "None" << endl;
1721  cout << endl;
1722 
1723  /* Supported formats */
1724  cout << "Formats:" << endl;
1725  v4l2_fmtdesc format_desc;
1726  memset(&format_desc, 0, sizeof(format_desc));
1727  format_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1728 
1729  char fourcc[5] = " ";
1730  for (format_desc.index = 0; v4l2_ioctl(_dev, VIDIOC_ENUM_FMT, &format_desc) == 0; format_desc.index++)
1731  {
1732  fourcc[0] = static_cast<char>(format_desc.pixelformat & 0xFF);
1733  fourcc[1] = static_cast<char>((format_desc.pixelformat >> 8) & 0xFF);
1734  fourcc[2] = static_cast<char>((format_desc.pixelformat >> 16) & 0xFF);
1735  fourcc[3] = static_cast<char>((format_desc.pixelformat >> 24) & 0xFF);
1736 
1737  colorspace_t cs = CS_UNKNOWN;
1738  if (strcmp(fourcc, "RGB3") == 0) cs = RGB;
1739  else if (strcmp(fourcc, "Y41P") == 0) cs = YUV411_PACKED; //different byte ordering
1740  else if (strcmp(fourcc, "411P") == 0) cs = YUV411_PLANAR;
1741  else if (strcmp(fourcc, "YUYV") == 0) cs = YUY2;
1742  else if (strcmp(fourcc, "BGR3") == 0) cs = BGR;
1743  else if (strcmp(fourcc, "UYVY") == 0) cs = YUV422_PACKED;
1744  else if (strcmp(fourcc, "422P") == 0) cs = YUV422_PLANAR;
1745  else if (strcmp(fourcc, "GREY") == 0) cs = GRAY8;
1746  else if (strcmp(fourcc, "RGB4") == 0) cs = RGB_WITH_ALPHA;
1747  else if (strcmp(fourcc, "BGR4") == 0) cs = BGR_WITH_ALPHA;
1748  else if (strcmp(fourcc, "BA81") == 0) cs = BAYER_MOSAIC_BGGR;
1749  else if (strcmp(fourcc, "Y16 ") == 0) cs = MONO16;
1750 
1751  cout << " + Format " << format_desc.index << ": " << fourcc <<
1752  " (" << format_desc.description << ")";
1753  if (format_desc.flags & V4L2_FMT_FLAG_COMPRESSED) cout << " [Compressed]";
1754  cout << endl << " |- Colorspace: " << colorspace_to_string(cs) << endl;
1755  }
1756  cout << endl;
1757 
1758  /* Current Format */
1759  v4l2_format format;
1760  memset(&format, 0, sizeof(format));
1761  format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1762  if (v4l2_ioctl(_dev, VIDIOC_G_FMT, &format)) throw Exception("V4L2Cam: Format query failed");
1763  fourcc[0] = static_cast<char>(format.fmt.pix.pixelformat & 0xFF);
1764  fourcc[1] = static_cast<char>((format.fmt.pix.pixelformat >> 8) & 0xFF);
1765  fourcc[2] = static_cast<char>((format.fmt.pix.pixelformat >> 16) & 0xFF);
1766  fourcc[3] = static_cast<char>((format.fmt.pix.pixelformat >> 24) & 0xFF);
1767 
1768  cout << " Current Format:" << endl <<
1769  " " << format.fmt.pix.width << "x" << format.fmt.pix.height <<
1770  " (" << fourcc << ")" << endl <<
1771  " " << format.fmt.pix.bytesperline << " bytes per line" << endl <<
1772  " Total size: " << format.fmt.pix.sizeimage << endl;
1773 
1774  /* Supported Controls */
1775  cout << "Controls:" << endl;
1776  v4l2_queryctrl queryctrl;
1777  v4l2_querymenu querymenu;
1778 
1779  memset(&queryctrl, 0, sizeof(queryctrl));
1780 
1781  for (queryctrl.id = V4L2_CID_BASE; queryctrl.id < V4L2_CID_LASTP1;
1782  queryctrl.id++)
1783  {
1784  if (v4l2_ioctl(_dev, VIDIOC_QUERYCTRL, &queryctrl))
1785  {
1786  if (errno == EINVAL) continue;
1787 
1788  cout << "Control query failed" << endl;
1789  return;
1790  }
1791  if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) continue;
1792 
1793  cout << " + " << queryctrl.name << " [" <<
1794  (queryctrl.id - V4L2_CID_BASE) << "] (";
1795  switch (queryctrl.type)
1796  {
1797  case V4L2_CTRL_TYPE_INTEGER:
1798  cout << "int [" << queryctrl.minimum << "-" << queryctrl.maximum <<
1799  " /" << queryctrl.step << " def " << queryctrl.default_value <<
1800  "]";
1801  break;
1802 
1803  case V4L2_CTRL_TYPE_MENU:
1804  cout << "menu [def " << queryctrl.default_value << "]";
1805  break;
1806 
1807  case V4L2_CTRL_TYPE_BOOLEAN:
1808  cout << "bool [def " << queryctrl.default_value << "]";
1809  break;
1810 
1811  case V4L2_CTRL_TYPE_BUTTON:
1812  cout << "button";
1813  break;
1814 
1815 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,17)
1816  case V4L2_CTRL_TYPE_INTEGER64:
1817  cout << "int64";
1818  break;
1819 
1820  case V4L2_CTRL_TYPE_CTRL_CLASS:
1821  cout << "ctrl_class";
1822  break;
1823 #endif
1824 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
1825  case V4L2_CTRL_TYPE_STRING:
1826  cout << "string";
1827  break;
1828 #endif
1829 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3,1,0) || LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,41)
1830  case V4L2_CTRL_TYPE_BITMASK:
1831  cout << "bitmask";
1832  break;
1833 #endif
1834  }
1835  cout << ")" << endl;
1836 
1837  if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
1838  {
1839  cout << " |- Menu items:" << endl;
1840 
1841  memset(&querymenu, 0, sizeof(querymenu));
1842  querymenu.id = queryctrl.id;
1843 
1844  for (querymenu.index = queryctrl.minimum;
1845  querymenu.index <= static_cast<unsigned long int>(queryctrl.maximum);
1846  querymenu.index++)
1847  {
1848  if (v4l2_ioctl(_dev, VIDIOC_QUERYMENU, &querymenu))
1849  {
1850  cout << "Getting menu items failed" << endl;
1851  return;
1852  }
1853  cout << " | + " << querymenu.name << endl;
1854  }
1855  }
1856  }
1857  if (queryctrl.id == V4L2_CID_BASE) cout << "None" << endl;
1858  cout << endl;
1859 
1860  /* Supported Private Controls */
1861  cout << "Private Controls:" << endl;
1862  for (queryctrl.id = V4L2_CID_PRIVATE_BASE; ; queryctrl.id++)
1863  {
1864  if (v4l2_ioctl(_dev, VIDIOC_QUERYCTRL, &queryctrl))
1865  {
1866  if (errno == EINVAL) break;
1867 
1868  cout << "Private Control query failed" << endl;
1869  return;
1870  }
1871 
1872  if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) continue;
1873 
1874  cout << " + " << queryctrl.name << " [" <<
1875  (queryctrl.id - V4L2_CID_PRIVATE_BASE) << "] (";
1876  switch (queryctrl.type)
1877  {
1878  case V4L2_CTRL_TYPE_INTEGER:
1879  cout << "int [" << queryctrl.minimum << "-" << queryctrl.maximum <<
1880  " /" << queryctrl.step << " def " << queryctrl.default_value <<
1881  "]";
1882  break;
1883 
1884  case V4L2_CTRL_TYPE_MENU:
1885  cout << "menu [def " << queryctrl.default_value << "]";
1886  break;
1887 
1888  case V4L2_CTRL_TYPE_BOOLEAN:
1889  cout << "bool [def " << queryctrl.default_value << "]";
1890  break;
1891 
1892  case V4L2_CTRL_TYPE_BUTTON:
1893  cout << "button";
1894  break;
1895 
1896 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,17)
1897  case V4L2_CTRL_TYPE_INTEGER64:
1898  cout << "int64";
1899  break;
1900 
1901  case V4L2_CTRL_TYPE_CTRL_CLASS:
1902  cout << "ctrl_class";
1903  break;
1904 #endif
1905 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
1906  case V4L2_CTRL_TYPE_STRING:
1907  cout << "string";
1908  break;
1909 #endif
1910 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3,1,0) || LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,41)
1911  case V4L2_CTRL_TYPE_BITMASK:
1912  cout << "bitmask";
1913  break;
1914 #endif
1915  }
1916  cout << ")" << endl;
1917 
1918  if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
1919  {
1920  cout << " |- Menu items:" << endl;
1921 
1922  memset(&querymenu, 0, sizeof(querymenu));
1923  querymenu.id = queryctrl.id;
1924 
1925  for (querymenu.index = queryctrl.minimum;
1926  querymenu.index <= static_cast<unsigned long int>(queryctrl.maximum);
1927  querymenu.index++)
1928  {
1929  if (v4l2_ioctl(_dev, VIDIOC_QUERYMENU, &querymenu))
1930  {
1931  cout << "Getting menu items failed" << endl;
1932  return;
1933  }
1934  cout << " | + " << querymenu.name << endl;
1935  }
1936  }
1937  }
1938  if (queryctrl.id == V4L2_CID_PRIVATE_BASE) cout << "None" << endl;
1939 
1940  cout <<
1941  "=========================================================================="
1942  << endl;
1943 }
1944 
1945 } // end namespace firevision
std::string get(std::string s) const
Get the value of the given parameter.
Definition: camargp.cpp:164
Library logger.
Definition: liblogger.h:38
Called method has not been implemented.
Definition: software.h:107
A class for handling time.
Definition: time.h:91
Camera argument parser.
Definition: camargp.h:38
Base class for exceptions in Fawkes.
Definition: exception.h:36
Expected parameter is missing.
Definition: software.h:76
bool has(std::string s) const
Check if an parameter was given.
Definition: camargp.cpp:152