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