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