00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <iostream>
00024
00025 #include <sys/ioctl.h>
00026 #include <sys/types.h>
00027 #include <sys/stat.h>
00028 #include <assert.h>
00029 #include <fcntl.h>
00030 #include <unistd.h>
00031 #include <stdio.h>
00032 #include <errno.h>
00033 #include <sys/mman.h>
00034 #include <string.h>
00035 #include <asm/types.h>
00036
00037 #include "ccvt.h"
00038 #include "v4l2_base.h"
00039 #include "../eventloop.h"
00040 #include "../indidevapi.h"
00041
00042 #define ERRMSGSIZ 1024
00043
00044 #define CLEAR(x) memset (&(x), 0, sizeof (x))
00045
00046 using namespace std;
00047
00048 V4L2_Base::V4L2_Base()
00049 {
00050 frameRate=10;
00051 selectCallBackID = -1;
00052 dropFrame = false;
00053
00054 xmax = xmin = 160;
00055 ymax = ymin = 120;
00056
00057 io = IO_METHOD_MMAP;
00058 fd = -1;
00059 buffers = NULL;
00060 n_buffers = 0;
00061
00062 YBuf = NULL;
00063 UBuf = NULL;
00064 VBuf = NULL;
00065 colorBuffer = NULL;
00066 rgb24_buffer = NULL;
00067 callback = NULL;
00068
00069 }
00070
00071 V4L2_Base::~V4L2_Base()
00072 {
00073
00074 delete (YBuf);
00075 delete (UBuf);
00076 delete (VBuf);
00077 delete (colorBuffer);
00078 delete (rgb24_buffer);
00079
00080 }
00081
00082 int V4L2_Base::xioctl(int fd, int request, void *arg)
00083 {
00084 int r;
00085
00086 do r = ioctl (fd, request, arg);
00087 while (-1 == r && EINTR == errno);
00088
00089 return r;
00090 }
00091
00092 int V4L2_Base::errno_exit(const char *s, char *errmsg)
00093 {
00094 fprintf (stderr, "%s error %d, %s\n",
00095 s, errno, strerror (errno));
00096
00097 snprintf(errmsg, ERRMSGSIZ, "%s error %d, %s\n", s, errno, strerror (errno));
00098
00099 return -1;
00100 }
00101
00102 int V4L2_Base::connectCam(const char * devpath, char *errmsg , int pixelFormat , int width , int height )
00103 {
00104 frameRate=10;
00105 selectCallBackID = -1;
00106 dropFrame = false;
00107
00108 if (open_device (devpath, errmsg) < 0)
00109 return -1;
00110
00111 if (init_device(errmsg, pixelFormat, width, height) < 0)
00112 return -1;
00113
00114 cerr << "V4L 2 - All successful, returning\n";
00115 return fd;
00116 }
00117
00118 void V4L2_Base::disconnectCam()
00119 {
00120 char errmsg[ERRMSGSIZ];
00121 delete YBuf;
00122 delete UBuf;
00123 delete VBuf;
00124 YBuf = UBuf = VBuf = NULL;
00125
00126 if (selectCallBackID != -1)
00127 rmCallback(selectCallBackID);
00128
00129 stop_capturing (errmsg);
00130
00131 uninit_device (errmsg);
00132
00133 close_device ();
00134
00135 fprintf(stderr, "Disconnect cam\n");
00136 }
00137
00138 int V4L2_Base::read_frame(char *errmsg)
00139 {
00140 struct v4l2_buffer buf;
00141 unsigned int i;
00142
00143
00144 switch (io) {
00145 case IO_METHOD_READ:
00146 if (-1 == read (fd, buffers[0].start, buffers[0].length)) {
00147 switch (errno) {
00148 case EAGAIN:
00149 return 0;
00150
00151 case EIO:
00152
00153
00154
00155
00156 default:
00157 return errno_exit ("read", errmsg);
00158 }
00159 }
00160
00161
00162
00163 break;
00164
00165 case IO_METHOD_MMAP:
00166 CLEAR (buf);
00167
00168 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00169 buf.memory = V4L2_MEMORY_MMAP;
00170
00171 if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
00172 switch (errno) {
00173 case EAGAIN:
00174 return 0;
00175
00176 case EIO:
00177
00178
00179
00180
00181 default:
00182 return errno_exit ("VIDIOC_DQBUF", errmsg);
00183 }
00184 }
00185
00186 assert (buf.index < n_buffers);
00187
00188 switch (fmt.fmt.pix.pixelformat)
00189 {
00190 case V4L2_PIX_FMT_YUV420:
00191 memcpy(YBuf,((unsigned char *) buffers[buf.index].start), fmt.fmt.pix.width * fmt.fmt.pix.height);
00192 memcpy(UBuf,((unsigned char *) buffers[buf.index].start) + fmt.fmt.pix.width * fmt.fmt.pix.height, (fmt.fmt.pix.width/2) * (fmt.fmt.pix.height/2));
00193 memcpy(VBuf,((unsigned char *) buffers[buf.index].start) + fmt.fmt.pix.width * fmt.fmt.pix.height + (fmt.fmt.pix.width/2) * (fmt.fmt.pix.height/2), (fmt.fmt.pix.width/2) * (fmt.fmt.pix.width/2));
00194 break;
00195
00196 case V4L2_PIX_FMT_YUYV:
00197 ccvt_yuyv_420p( fmt.fmt.pix.width , fmt.fmt.pix.height, buffers[buf.index].start, YBuf, UBuf, VBuf);
00198 break;
00199
00200 case V4L2_PIX_FMT_RGB24:
00201 RGB2YUV(fmt.fmt.pix.width, fmt.fmt.pix.height, buffers[buf.index].start, YBuf, UBuf, VBuf, 0);
00202 break;
00203
00204 case V4L2_PIX_FMT_SBGGR8:
00205 bayer2rgb24(rgb24_buffer, ((unsigned char *) buffers[buf.index].start), fmt.fmt.pix.width, fmt.fmt.pix.height);
00206 break;
00207 }
00208
00209 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
00210 return errno_exit ("VIDIOC_QBUF", errmsg);
00211
00212 if (dropFrame)
00213 {
00214 dropFrame = false;
00215 return 0;
00216 }
00217
00218
00219 if (callback)
00220 (*callback)(uptr);
00221
00222 break;
00223
00224 case IO_METHOD_USERPTR:
00225 CLEAR (buf);
00226
00227 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00228 buf.memory = V4L2_MEMORY_USERPTR;
00229
00230 if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
00231 switch (errno) {
00232 case EAGAIN:
00233 return 0;
00234
00235 case EIO:
00236
00237
00238
00239
00240 default:
00241 errno_exit ("VIDIOC_DQBUF", errmsg);
00242 }
00243 }
00244
00245 for (i = 0; i < n_buffers; ++i)
00246 if (buf.m.userptr == (unsigned long) buffers[i].start
00247 && buf.length == buffers[i].length)
00248 break;
00249
00250 assert (i < n_buffers);
00251
00252
00253
00254 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
00255 errno_exit ("VIDIOC_QBUF", errmsg);
00256
00257 break;
00258 }
00259
00260 return 0;
00261 }
00262
00263 int V4L2_Base::stop_capturing(char *errmsg)
00264 {
00265 enum v4l2_buf_type type;
00266
00267 switch (io) {
00268 case IO_METHOD_READ:
00269
00270 break;
00271
00272 case IO_METHOD_MMAP:
00273 case IO_METHOD_USERPTR:
00274 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00275
00276 IERmCallback(selectCallBackID);
00277 selectCallBackID = -1;
00278
00279 dropFrame = true;
00280
00281 if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type))
00282 return errno_exit ("VIDIOC_STREAMOFF", errmsg);
00283
00284
00285
00286 break;
00287 }
00288
00289 return 0;
00290 }
00291
00292 int V4L2_Base::start_capturing(char * errmsg)
00293 {
00294 unsigned int i;
00295 enum v4l2_buf_type type;
00296
00297 switch (io) {
00298 case IO_METHOD_READ:
00299
00300 break;
00301
00302 case IO_METHOD_MMAP:
00303 for (i = 0; i < n_buffers; ++i) {
00304 struct v4l2_buffer buf;
00305
00306 CLEAR (buf);
00307
00308 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00309 buf.memory = V4L2_MEMORY_MMAP;
00310 buf.index = i;
00311
00312 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
00313 return errno_exit ("VIDIOC_QBUF", errmsg);
00314
00315 }
00316
00317 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00318
00319 if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
00320 return errno_exit ("VIDIOC_STREAMON", errmsg);
00321
00322
00323
00324 selectCallBackID = IEAddCallback(fd, newFrame, this);
00325
00326 break;
00327
00328 case IO_METHOD_USERPTR:
00329 for (i = 0; i < n_buffers; ++i) {
00330 struct v4l2_buffer buf;
00331
00332 CLEAR (buf);
00333
00334 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00335 buf.memory = V4L2_MEMORY_USERPTR;
00336 buf.m.userptr = (unsigned long) buffers[i].start;
00337 buf.length = buffers[i].length;
00338
00339 if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
00340 return errno_exit ("VIDIOC_QBUF", errmsg);
00341 }
00342
00343
00344 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00345
00346 if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
00347 return errno_exit ("VIDIOC_STREAMON", errmsg);
00348
00349 break;
00350 }
00351
00352 return 0;
00353
00354 }
00355
00356 void V4L2_Base::newFrame(int , void *p)
00357 {
00358 char errmsg[ERRMSGSIZ];
00359
00360 ( (V4L2_Base *) (p))->read_frame(errmsg);
00361
00362 }
00363
00364 int V4L2_Base::uninit_device(char *errmsg)
00365 {
00366
00367 switch (io) {
00368 case IO_METHOD_READ:
00369 free (buffers[0].start);
00370 break;
00371
00372 case IO_METHOD_MMAP:
00373 for (unsigned int i = 0; i < n_buffers; ++i)
00374 if (-1 == munmap (buffers[i].start, buffers[i].length))
00375 return errno_exit ("munmap", errmsg);
00376 break;
00377
00378 case IO_METHOD_USERPTR:
00379 for (unsigned int i = 0; i < n_buffers; ++i)
00380 free (buffers[i].start);
00381 break;
00382 }
00383
00384 free (buffers);
00385
00386 return 0;
00387 }
00388
00389 void V4L2_Base::init_read(unsigned int buffer_size)
00390 {
00391 buffers = (buffer *) calloc (1, sizeof (*buffers));
00392
00393 if (!buffers) {
00394 fprintf (stderr, "Out of memory\n");
00395 exit (EXIT_FAILURE);
00396 }
00397
00398 buffers[0].length = buffer_size;
00399 buffers[0].start = malloc (buffer_size);
00400
00401 if (!buffers[0].start) {
00402 fprintf (stderr, "Out of memory\n");
00403 exit (EXIT_FAILURE);
00404 }
00405 }
00406
00407 int V4L2_Base::init_mmap(char *errmsg)
00408 {
00409 struct v4l2_requestbuffers req;
00410
00411 CLEAR (req);
00412
00413 req.count = 4;
00414 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00415 req.memory = V4L2_MEMORY_MMAP;
00416
00417 if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) {
00418 if (EINVAL == errno) {
00419 fprintf (stderr, "%s does not support "
00420 "memory mapping\n", dev_name);
00421 snprintf(errmsg, ERRMSGSIZ, "%s does not support "
00422 "memory mapping\n", dev_name);
00423 return -1;
00424 } else {
00425 return errno_exit ("VIDIOC_REQBUFS", errmsg);
00426 }
00427 }
00428
00429 if (req.count < 2) {
00430 fprintf (stderr, "Insufficient buffer memory on %s\n",
00431 dev_name);
00432 snprintf(errmsg, ERRMSGSIZ, "Insufficient buffer memory on %s\n",
00433 dev_name);
00434 return -1;
00435 }
00436
00437 buffers = (buffer *) calloc (req.count, sizeof (*buffers));
00438
00439 if (!buffers)
00440 {
00441 fprintf (stderr, "buffers. Out of memory\n");
00442 strncpy(errmsg, "buffers. Out of memory\n", ERRMSGSIZ);
00443 return -1;
00444 }
00445
00446 for (n_buffers = 0; n_buffers < req.count; ++n_buffers)
00447 {
00448 struct v4l2_buffer buf;
00449
00450 CLEAR (buf);
00451
00452 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00453 buf.memory = V4L2_MEMORY_MMAP;
00454 buf.index = n_buffers;
00455
00456 if (-1 == xioctl (fd, VIDIOC_QUERYBUF, &buf))
00457 return errno_exit ("VIDIOC_QUERYBUF", errmsg);
00458
00459 buffers[n_buffers].length = buf.length;
00460 buffers[n_buffers].start =
00461 mmap (NULL ,
00462 buf.length,
00463 PROT_READ | PROT_WRITE ,
00464 MAP_SHARED ,
00465 fd, buf.m.offset);
00466
00467 if (MAP_FAILED == buffers[n_buffers].start)
00468 return errno_exit ("mmap", errmsg);
00469 }
00470
00471 return 0;
00472 }
00473
00474 void V4L2_Base::init_userp(unsigned int buffer_size)
00475 {
00476 struct v4l2_requestbuffers req;
00477 char errmsg[ERRMSGSIZ];
00478
00479 CLEAR (req);
00480
00481 req.count = 4;
00482 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00483 req.memory = V4L2_MEMORY_USERPTR;
00484
00485 if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) {
00486 if (EINVAL == errno) {
00487 fprintf (stderr, "%s does not support "
00488 "user pointer i/o\n", dev_name);
00489 exit (EXIT_FAILURE);
00490 } else {
00491 errno_exit ("VIDIOC_REQBUFS", errmsg);
00492 }
00493 }
00494
00495 buffers = (buffer *) calloc (4, sizeof (*buffers));
00496
00497 if (!buffers) {
00498 fprintf (stderr, "Out of memory\n");
00499 exit (EXIT_FAILURE);
00500 }
00501
00502 for (n_buffers = 0; n_buffers < 4; ++n_buffers) {
00503 buffers[n_buffers].length = buffer_size;
00504 buffers[n_buffers].start = malloc (buffer_size);
00505
00506 if (!buffers[n_buffers].start) {
00507 fprintf (stderr, "Out of memory\n");
00508 exit (EXIT_FAILURE);
00509 }
00510 }
00511 }
00512
00513 int V4L2_Base::init_device(char *errmsg, int pixelFormat , int width, int height)
00514 {
00515 unsigned int min;
00516
00517 if (-1 == xioctl (fd, VIDIOC_QUERYCAP, &cap))
00518 {
00519 if (EINVAL == errno) {
00520 fprintf (stderr, "%s is no V4L2 device\n",
00521 dev_name);
00522 snprintf(errmsg, ERRMSGSIZ, "%s is no V4L2 device\n", dev_name);
00523 return -1;
00524 } else {
00525 return errno_exit ("VIDIOC_QUERYCAP", errmsg);
00526 }
00527 }
00528
00529 if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
00530 {
00531 fprintf (stderr, "%s is no video capture device\n",
00532 dev_name);
00533 snprintf(errmsg, ERRMSGSIZ, "%s is no video capture device\n", dev_name);
00534 return -1;
00535 }
00536
00537 switch (io)
00538 {
00539 case IO_METHOD_READ:
00540 if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
00541 fprintf (stderr, "%s does not support read i/o\n",
00542 dev_name);
00543 snprintf(errmsg, ERRMSGSIZ, "%s does not support read i/o\n",
00544 dev_name);
00545 return -1;
00546 }
00547
00548 break;
00549
00550 case IO_METHOD_MMAP:
00551 case IO_METHOD_USERPTR:
00552 if (!(cap.capabilities & V4L2_CAP_STREAMING))
00553 {
00554 fprintf (stderr, "%s does not support streaming i/o\n",
00555 dev_name);
00556 snprintf(errmsg, ERRMSGSIZ, "%s does not support streaming i/o\n",
00557 dev_name);
00558 return -1;
00559 }
00560
00561 break;
00562 }
00563
00564
00565
00566 cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00567
00568 if (-1 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) {
00569
00570 }
00571
00572 crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00573 crop.c = cropcap.defrect;
00574
00575 if (-1 == xioctl (fd, VIDIOC_S_CROP, &crop)) {
00576 switch (errno) {
00577 case EINVAL:
00578
00579 break;
00580 default:
00581
00582 break;
00583 }
00584 }
00585
00586 CLEAR (fmt);
00587
00588 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00589 fmt.fmt.pix.width = width;
00590 fmt.fmt.pix.height = height;
00591 fmt.fmt.pix.pixelformat = pixelFormat;
00592
00593
00594 if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt))
00595 return errno_exit ("VIDIOC_S_FMT", errmsg);
00596
00597
00598
00599
00600 min = fmt.fmt.pix.width * 2;
00601 if (fmt.fmt.pix.bytesperline < min)
00602 fmt.fmt.pix.bytesperline = min;
00603 min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
00604 if (fmt.fmt.pix.sizeimage < min)
00605 fmt.fmt.pix.sizeimage = min;
00606
00607
00608 CLEAR(fmt);
00609
00610 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00611
00612 if (-1 == xioctl (fd, VIDIOC_G_FMT, &fmt))
00613 return errno_exit ("VIDIOC_G_FMT", errmsg);
00614
00615 cerr << "width: " << fmt.fmt.pix.width << " - height: " << fmt.fmt.pix.height << endl;
00616
00617 switch (pixelFormat)
00618 {
00619 case V4L2_PIX_FMT_YUV420:
00620 cerr << "pixel format: V4L2_PIX_FMT_YUV420" << endl;
00621 break;
00622
00623 case V4L2_PIX_FMT_YUYV:
00624 cerr << "pixel format: V4L2_PIX_FMT_YUYV" << endl;
00625 break;
00626
00627 case V4L2_PIX_FMT_RGB24:
00628 cerr << "pixel format: V4L2_PIX_FMT_RGB24" << endl;
00629 break;
00630
00631 case V4L2_PIX_FMT_SBGGR8:
00632 cerr << "pixel format: V4L2_PIX_FMT_SBGGR8" << endl;
00633 break;
00634
00635 }
00636
00637 findMinMax();
00638
00639 allocBuffers();
00640
00641 switch (io)
00642 {
00643 case IO_METHOD_READ:
00644 init_read (fmt.fmt.pix.sizeimage);
00645 break;
00646
00647 case IO_METHOD_MMAP:
00648 return init_mmap(errmsg);
00649 break;
00650
00651 case IO_METHOD_USERPTR:
00652 init_userp (fmt.fmt.pix.sizeimage);
00653 break;
00654 }
00655
00656 return 0;
00657 }
00658
00659 void V4L2_Base::close_device(void)
00660 {
00661 char errmsg[ERRMSGSIZ];
00662
00663 if (-1 == close (fd))
00664 errno_exit ("close", errmsg);
00665
00666 fd = -1;
00667 }
00668
00669 int V4L2_Base::open_device(const char *devpath, char *errmsg)
00670 {
00671 struct stat st;
00672
00673 strncpy(dev_name, devpath, 64);
00674
00675 if (-1 == stat (dev_name, &st)) {
00676 fprintf (stderr, "Cannot identify '%s': %d, %s\n",
00677 dev_name, errno, strerror (errno));
00678 snprintf(errmsg, ERRMSGSIZ, "Cannot identify '%s': %d, %s\n",
00679 dev_name, errno, strerror (errno));
00680 return -1;
00681 }
00682
00683 if (!S_ISCHR (st.st_mode))
00684 {
00685 fprintf (stderr, "%s is no device\n", dev_name);
00686 snprintf(errmsg, ERRMSGSIZ, "%s is no device\n", dev_name);
00687 return -1;
00688 }
00689
00690 fd = open (dev_name, O_RDWR | O_NONBLOCK, 0);
00691
00692 if (-1 == fd)
00693 {
00694 fprintf (stderr, "Cannot open '%s': %d, %s\n",
00695 dev_name, errno, strerror (errno));
00696 snprintf(errmsg, ERRMSGSIZ, "Cannot open '%s': %d, %s\n",
00697 dev_name, errno, strerror (errno));
00698 return -1;
00699 }
00700
00701 return 0;
00702 }
00703
00704
00705
00706 int V4L2_Base::getWidth()
00707 {
00708 return fmt.fmt.pix.width;
00709 }
00710
00711 int V4L2_Base::getHeight()
00712 {
00713 return fmt.fmt.pix.height;
00714 }
00715
00716 void V4L2_Base::setFPS(int fps)
00717 {
00718 frameRate = 15;
00719 }
00720
00721 int V4L2_Base::getFPS()
00722 {
00723 return 15;
00724 }
00725
00726 char * V4L2_Base::getDeviceName()
00727 {
00728 return ((char *) cap.card);
00729 }
00730
00731 void V4L2_Base::allocBuffers()
00732 {
00733 delete (YBuf); YBuf = NULL;
00734 delete (UBuf); UBuf = NULL;
00735 delete (VBuf); VBuf = NULL;
00736 delete (colorBuffer); colorBuffer = NULL;
00737 delete (rgb24_buffer); rgb24_buffer = NULL;
00738
00739 YBuf= new unsigned char[ fmt.fmt.pix.width * fmt.fmt.pix.height];
00740 UBuf= new unsigned char[ fmt.fmt.pix.width * fmt.fmt.pix.height];
00741 VBuf= new unsigned char[ fmt.fmt.pix.width * fmt.fmt.pix.height];
00742 colorBuffer = new unsigned char[fmt.fmt.pix.width * fmt.fmt.pix.height * 4];
00743
00744 if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_SBGGR8)
00745 rgb24_buffer = new unsigned char[fmt.fmt.pix.width * fmt.fmt.pix.height * 3];
00746 }
00747
00748 void V4L2_Base::getMaxMinSize(int & x_max, int & y_max, int & x_min, int & y_min)
00749 {
00750 x_max = xmax; y_max = ymax; x_min = xmin; y_min = ymin;
00751 }
00752
00753 int V4L2_Base::setSize(int x, int y)
00754 {
00755 char errmsg[ERRMSGSIZ];
00756 int oldW, oldH;
00757
00758 oldW = fmt.fmt.pix.width;
00759 oldH = fmt.fmt.pix.height;
00760
00761 fmt.fmt.pix.width = x;
00762 fmt.fmt.pix.height = y;
00763
00764 if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt))
00765 {
00766 fmt.fmt.pix.width = oldW;
00767 fmt.fmt.pix.height = oldH;
00768 return errno_exit ("VIDIOC_S_FMT", errmsg);
00769 }
00770
00771
00772
00773
00774 xioctl (fd, VIDIOC_S_FMT, &fmt);
00775
00776
00777 allocBuffers();
00778
00779 return 0;
00780 }
00781
00782 void V4L2_Base::setContrast(int val)
00783 {
00784
00785
00786 }
00787
00788 int V4L2_Base::getContrast()
00789 {
00790 return 255;
00791 }
00792
00793 void V4L2_Base::setBrightness(int val)
00794 {
00795
00796
00797 }
00798
00799 int V4L2_Base::getBrightness()
00800 {
00801 return 255;
00802 }
00803
00804 void V4L2_Base::setColor(int val)
00805 {
00806
00807
00808 }
00809
00810 int V4L2_Base::getColor()
00811 {
00812 return 255;
00813 }
00814
00815 void V4L2_Base::setHue(int val)
00816 {
00817
00818
00819 }
00820
00821 int V4L2_Base::getHue()
00822 {
00823 return 255;
00824 }
00825
00826 void V4L2_Base::setWhiteness(int val)
00827 {
00828
00829
00830 }
00831
00832 int V4L2_Base::getWhiteness()
00833 {
00834 return 255;
00835 }
00836
00837 void V4L2_Base::setPictureSettings()
00838 {
00839
00840
00841
00842
00843 }
00844
00845 void V4L2_Base::getPictureSettings()
00846 {
00847
00848
00849
00850
00851 }
00852
00853 unsigned char * V4L2_Base::getY()
00854 {
00855 if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_SBGGR8)
00856 RGB2YUV(fmt.fmt.pix.width, fmt.fmt.pix.height, rgb24_buffer, YBuf, UBuf, VBuf, 0);
00857
00858 return YBuf;
00859 }
00860
00861 unsigned char * V4L2_Base::getU()
00862 {
00863 return UBuf;
00864 }
00865
00866 unsigned char * V4L2_Base::getV()
00867 {
00868 return VBuf;
00869 }
00870
00871 unsigned char * V4L2_Base::getColorBuffer()
00872 {
00873
00874
00875 switch (fmt.fmt.pix.pixelformat)
00876 {
00877 case V4L2_PIX_FMT_YUV420:
00878 ccvt_420p_bgr32(fmt.fmt.pix.width, fmt.fmt.pix.height,
00879 buffers[0].start, (void*)colorBuffer);
00880 break;
00881
00882 case V4L2_PIX_FMT_YUYV:
00883 ccvt_yuyv_bgr32(fmt.fmt.pix.width, fmt.fmt.pix.height,
00884 buffers[0].start, (void*)colorBuffer);
00885 break;
00886
00887 case V4L2_PIX_FMT_RGB24:
00888 ccvt_rgb24_bgr32(fmt.fmt.pix.width, fmt.fmt.pix.height,
00889 buffers[0].start, (void*)colorBuffer);
00890 break;
00891
00892 case V4L2_PIX_FMT_SBGGR8:
00893 ccvt_rgb24_bgr32(fmt.fmt.pix.width, fmt.fmt.pix.height,
00894 rgb24_buffer, (void*)colorBuffer);
00895 break;
00896
00897 default:
00898 break;
00899 }
00900
00901 return colorBuffer;
00902 }
00903
00904 void V4L2_Base::registerCallback(WPF *fp, void *ud)
00905 {
00906 callback = fp;
00907 uptr = ud;
00908 }
00909
00910 void V4L2_Base::findMinMax()
00911 {
00912 char errmsg[ERRMSGSIZ];
00913 struct v4l2_format tryfmt;
00914 CLEAR(tryfmt);
00915
00916 xmin = xmax = fmt.fmt.pix.width;
00917 ymin = ymax = fmt.fmt.pix.height;
00918
00919 tryfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00920 tryfmt.fmt.pix.width = 10;
00921 tryfmt.fmt.pix.height = 10;
00922 tryfmt.fmt.pix.pixelformat = fmt.fmt.pix.pixelformat;
00923 tryfmt.fmt.pix.field = fmt.fmt.pix.field;
00924
00925 if (-1 == xioctl (fd, VIDIOC_TRY_FMT, &tryfmt))
00926 {
00927 errno_exit ("VIDIOC_TRY_FMT 1", errmsg);
00928 return;
00929 }
00930
00931 xmin = tryfmt.fmt.pix.width;
00932 ymin = tryfmt.fmt.pix.height;
00933
00934 tryfmt.fmt.pix.width = 1600;
00935 tryfmt.fmt.pix.height = 1200;
00936
00937 if (-1 == xioctl (fd, VIDIOC_TRY_FMT, &tryfmt))
00938 {
00939 errno_exit ("VIDIOC_TRY_FMT 2", errmsg);
00940 return;
00941 }
00942
00943 xmax = tryfmt.fmt.pix.width;
00944 ymax = tryfmt.fmt.pix.height;
00945
00946 cerr << "Min X: " << xmin << " - Max X: " << xmax << " - Min Y: " << ymin << " - Max Y: " << ymax << endl;
00947 }
00948
00949 void V4L2_Base::enumerate_ctrl (void)
00950 {
00951 char errmsg[ERRMSGSIZ];
00952 CLEAR(queryctrl);
00953
00954 for (queryctrl.id = V4L2_CID_BASE; queryctrl.id < V4L2_CID_LASTP1; queryctrl.id++)
00955 {
00956 if (0 == xioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
00957 {
00958 cerr << "Control " << queryctrl.name << endl;
00959
00960 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
00961 continue;
00962
00963 cerr << "Control " << queryctrl.name << endl;
00964
00965 if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
00966 enumerate_menu ();
00967 } else
00968 {
00969 if (errno == EINVAL)
00970 continue;
00971
00972 errno_exit("VIDIOC_QUERYCTRL", errmsg);
00973 return;
00974 }
00975 }
00976
00977 for (queryctrl.id = V4L2_CID_PRIVATE_BASE; ; queryctrl.id++)
00978 {
00979 if (0 == xioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
00980 {
00981 cerr << "Private Control " << queryctrl.name << endl;
00982
00983 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
00984 continue;
00985
00986 if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
00987 enumerate_menu ();
00988 } else {
00989 if (errno == EINVAL)
00990 break;
00991
00992 errno_exit ("VIDIOC_QUERYCTRL", errmsg);
00993 return;
00994 }
00995
00996 }
00997
00998 }
00999
01000 void V4L2_Base::enumerate_menu (void)
01001 {
01002 char errmsg[ERRMSGSIZ];
01003 cerr << " Menu items:" << endl;
01004
01005 CLEAR(querymenu);
01006 querymenu.id = queryctrl.id;
01007
01008 for (querymenu.index = queryctrl.minimum; querymenu.index <= queryctrl.maximum; querymenu.index++)
01009 {
01010 if (0 == xioctl (fd, VIDIOC_QUERYMENU, &querymenu))
01011 {
01012 cerr << " " << querymenu.name << endl;
01013 } else
01014 {
01015 errno_exit("VIDIOC_QUERYMENU", errmsg);
01016 return;
01017 }
01018 }
01019 }
01020
01021 int V4L2_Base::query_ctrl(unsigned int ctrl_id, double & ctrl_min, double & ctrl_max, double & ctrl_step, double & ctrl_value, char *errmsg)
01022 {
01023
01024 struct v4l2_control control;
01025
01026 CLEAR(queryctrl);
01027
01028 queryctrl.id = ctrl_id;
01029
01030 if (-1 == ioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
01031 {
01032 if (errno != EINVAL)
01033 return errno_exit ("VIDIOC_QUERYCTRL", errmsg);
01034
01035 else
01036 {
01037 cerr << "#" << ctrl_id << " is not supported" << endl;
01038 snprintf(errmsg, ERRMSGSIZ, "# %d is not supported", ctrl_id);
01039 return -1;
01040 }
01041 } else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
01042 {
01043 cerr << "#" << ctrl_id << " is disabled" << endl;
01044 snprintf(errmsg, ERRMSGSIZ, "# %d is disabled", ctrl_id);
01045 return -1;
01046 }
01047
01048 ctrl_min = queryctrl.minimum;
01049 ctrl_max = queryctrl.maximum;
01050 ctrl_step = queryctrl.step;
01051 ctrl_value = queryctrl.default_value;
01052
01053
01054 CLEAR(control);
01055 control.id = ctrl_id;
01056
01057 if (0 == xioctl(fd, VIDIOC_G_CTRL, &control))
01058 ctrl_value = control.value;
01059
01060 cerr << queryctrl.name << " -- min: " << ctrl_min << " max: " << ctrl_max << " step: " << ctrl_step << " value: " << ctrl_value << endl;
01061
01062 return 0;
01063
01064 }
01065
01066 int V4L2_Base::queryINTControls(INumberVectorProperty *nvp)
01067 {
01068 struct v4l2_control control;
01069 char errmsg[ERRMSGSIZ];
01070 CLEAR(queryctrl);
01071 INumber *numbers = NULL;
01072 unsigned int *num_ctrls = NULL;
01073 int nnum=0;
01074
01075 for (queryctrl.id = V4L2_CID_BASE; queryctrl.id < V4L2_CID_LASTP1; queryctrl.id++)
01076 {
01077 if (0 == ioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
01078 {
01079 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
01080 {
01081 cerr << queryctrl.name << " is disabled." << endl;
01082 continue;
01083 }
01084
01085 if (queryctrl.type == V4L2_CTRL_TYPE_INTEGER)
01086 {
01087 numbers = (numbers == NULL) ? (INumber *) malloc (sizeof(INumber)) :
01088 (INumber *) realloc (numbers, (nnum+1) * sizeof (INumber));
01089
01090 num_ctrls = (num_ctrls == NULL) ? (unsigned int *) malloc (sizeof (unsigned int)) :
01091 (unsigned int *) realloc (num_ctrls, (nnum+1) * sizeof (unsigned int));
01092
01093 strncpy(numbers[nnum].name, ((char *) queryctrl.name) , MAXINDINAME);
01094 strncpy(numbers[nnum].label, ((char *) queryctrl.name), MAXINDILABEL);
01095 strncpy(numbers[nnum].format, "%0.f", MAXINDIFORMAT);
01096 numbers[nnum].min = queryctrl.minimum;
01097 numbers[nnum].max = queryctrl.maximum;
01098 numbers[nnum].step = queryctrl.step;
01099 numbers[nnum].value = queryctrl.default_value;
01100
01101
01102 CLEAR(control);
01103 control.id = queryctrl.id;
01104 if (0 == xioctl(fd, VIDIOC_G_CTRL, &control))
01105 numbers[nnum].value = control.value;
01106
01107
01108 num_ctrls[nnum] = queryctrl.id;
01109
01110 cerr << queryctrl.name << " -- min: " << queryctrl.minimum << " max: " << queryctrl.maximum << " step: " << queryctrl.step << " value: " << numbers[nnum].value << endl;
01111
01112 nnum++;
01113
01114 }
01115 } else if (errno != EINVAL)
01116 return errno_exit ("VIDIOC_QUERYCTRL", errmsg);
01117
01118 }
01119
01120 for (queryctrl.id = V4L2_CID_PRIVATE_BASE; ; queryctrl.id++)
01121 {
01122 if (0 == ioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
01123 {
01124 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
01125 {
01126 cerr << queryctrl.name << " is disabled." << endl;
01127 continue;
01128 }
01129
01130 if (queryctrl.type == V4L2_CTRL_TYPE_INTEGER)
01131 {
01132 numbers = (numbers == NULL) ? (INumber *) malloc (sizeof(INumber)) :
01133 (INumber *) realloc (numbers, (nnum+1) * sizeof (INumber));
01134
01135 num_ctrls = (num_ctrls == NULL) ? (unsigned int *) malloc (sizeof (unsigned int)) :
01136 (unsigned int *) realloc (num_ctrls, (nnum+1) * sizeof (unsigned int));
01137
01138 strncpy(numbers[nnum].name, ((char *) queryctrl.name) , MAXINDINAME);
01139 strncpy(numbers[nnum].label, ((char *) queryctrl.name), MAXINDILABEL);
01140 strncpy(numbers[nnum].format, "%0.f", MAXINDIFORMAT);
01141 numbers[nnum].min = queryctrl.minimum;
01142 numbers[nnum].max = queryctrl.maximum;
01143 numbers[nnum].step = queryctrl.step;
01144 numbers[nnum].value = queryctrl.default_value;
01145
01146
01147 CLEAR(control);
01148 control.id = queryctrl.id;
01149 if (0 == xioctl(fd, VIDIOC_G_CTRL, &control))
01150 numbers[nnum].value = control.value;
01151
01152
01153 num_ctrls[nnum] = queryctrl.id;
01154
01155 nnum++;
01156
01157 }
01158 }
01159 else break;
01160 }
01161
01162
01163 for (int i=0; i < nnum; i++)
01164 numbers[i].aux0 = &num_ctrls[i];
01165
01166 nvp->np = numbers;
01167 nvp->nnp = nnum;
01168
01169 return nnum;
01170
01171 }
01172
01173 int V4L2_Base::setINTControl(unsigned int ctrl_id, double new_value, char *errmsg)
01174 {
01175 struct v4l2_control control;
01176
01177 CLEAR(control);
01178
01179 cerr << "The id is " << ctrl_id << " new value is " << new_value << endl;
01180
01181 control.id = ctrl_id;
01182 control.value = (int) new_value;
01183
01184 if (-1 == xioctl(fd, VIDIOC_S_CTRL, &control))
01185 return errno_exit ("VIDIOC_S_CTRL", errmsg);
01186
01187 return 0;
01188 }
01189