source: BAORadio/libindi/v1.0.1/libs/webcam/v4l2_base.cpp @ 614

Last change on this file since 614 was 490, checked in by campagne, 14 years ago

import libindi (JEC)
File size: 31.1 KB
Line 
1/*
2    Copyright (C) 2005 by Jasem Mutlaq <mutlaqja@ikarustech.com>
3
4    Based on V4L 2 Example
5    http://v4l2spec.bytesex.org/spec-single/v4l2.html#CAPTURE-EXAMPLE
6
7    This library is free software; you can redistribute it and/or
8    modify it under the terms of the GNU Lesser General Public
9    License as published by the Free Software Foundation; either
10    version 2.1 of the License, or (at your option) any later version.
11
12    This library is distributed in the hope that it will be useful,
13    but WITHOUT ANY WARRANTY; without even the implied warranty of
14    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15    Lesser General Public License for more details.
16
17    You should have received a copy of the GNU Lesser General Public
18    License along with this library; if not, write to the Free Software
19    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
20
21*/ 
22
23#include <iostream>
24
25#include <sys/ioctl.h>
26#include <sys/types.h>
27#include <sys/stat.h>
28#include <assert.h>
29#include <fcntl.h>
30#include <unistd.h>
31#include <stdio.h>
32#include <errno.h>
33#include <sys/mman.h>
34#include <string.h>
35#include <asm/types.h>          /* for videodev2.h */
36
37#include "ccvt.h"
38#include "v4l2_base.h"
39#include "eventloop.h"
40#include "indidevapi.h"
41
42#define ERRMSGSIZ       1024
43
44#define CLEAR(x) memset (&(x), 0, sizeof (x))
45
46using namespace std;
47
48V4L2_Base::V4L2_Base()
49{
50   frameRate=10;
51   selectCallBackID = -1;
52   dropFrame = false;
53 
54   xmax = xmin = 160;
55   ymax = ymin = 120;
56
57   io           = IO_METHOD_MMAP;
58   fd           = -1;
59   buffers      = NULL;
60   n_buffers    = 0;
61
62   YBuf         = NULL;
63   UBuf         = NULL;
64   VBuf         = NULL;
65   colorBuffer  = NULL;
66   rgb24_buffer = NULL;
67   callback     = NULL;
68
69}
70
71V4L2_Base::~V4L2_Base()
72{
73
74  delete (YBuf);
75  delete (UBuf);
76  delete (VBuf);
77  delete (colorBuffer);
78  delete (rgb24_buffer);
79
80}
81
82int V4L2_Base::xioctl(int fd, int request, void *arg)
83{
84        int r;
85
86        do r = ioctl (fd, request, arg);
87        while (-1 == r && EINTR == errno);
88
89        return r;
90}
91
92int V4L2_Base::errno_exit(const char *s, char *errmsg)
93{
94        fprintf (stderr, "%s error %d, %s\n",
95                 s, errno, strerror (errno));
96     
97        snprintf(errmsg, ERRMSGSIZ, "%s error %d, %s\n", s, errno, strerror (errno));
98
99        return -1;
100} 
101
102int V4L2_Base::connectCam(const char * devpath, char *errmsg , int pixelFormat , int width , int height )
103{
104   frameRate=10;
105   selectCallBackID = -1;
106   dropFrame = false;
107
108    if (open_device (devpath, errmsg) < 0)
109      return -1;
110
111    if (init_device(errmsg, pixelFormat, width, height) < 0)
112      return -1;
113
114   cerr << "V4L 2 - All successful, returning\n";
115   return fd;
116}
117
118void V4L2_Base::disconnectCam()
119{
120   char errmsg[ERRMSGSIZ];
121   delete YBuf;
122   delete UBuf;
123   delete VBuf;
124   YBuf = UBuf = VBuf = NULL;
125   
126   if (selectCallBackID != -1)
127     rmCallback(selectCallBackID);
128     
129   stop_capturing (errmsg);
130
131   uninit_device (errmsg);
132
133   close_device ();
134     
135   fprintf(stderr, "Disconnect cam\n");
136}
137
138int V4L2_Base::read_frame(char *errmsg)
139{
140        struct v4l2_buffer buf;
141        unsigned int i;
142        //cerr << "in read Frame" << endl;
143
144        switch (io) {
145        case IO_METHOD_READ:
146                if (-1 == read (fd, buffers[0].start, buffers[0].length)) {
147                        switch (errno) {
148                        case EAGAIN:
149                                return 0;
150
151                        case EIO:
152                                /* Could ignore EIO, see spec. */
153
154                                /* fall through */
155
156                        default:
157                                return errno_exit ("read", errmsg);
158                        }
159                }
160
161                //process_image (buffers[0].start);
162
163                break;
164
165        case IO_METHOD_MMAP:
166                CLEAR (buf);
167
168                buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
169                buf.memory = V4L2_MEMORY_MMAP;
170
171                if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
172                        switch (errno) {
173                        case EAGAIN:
174                                return 0;
175
176                        case EIO:
177                                /* Could ignore EIO, see spec. */
178
179                                /* fall through */
180
181                        default:
182                                return errno_exit ("VIDIOC_DQBUF", errmsg);
183                        }
184                }
185
186                assert (buf.index < n_buffers);
187
188                switch (fmt.fmt.pix.pixelformat)
189                {
190                  case V4L2_PIX_FMT_YUV420:
191                    memcpy(YBuf,((unsigned char *) buffers[buf.index].start), fmt.fmt.pix.width * fmt.fmt.pix.height);
192                    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));
193                    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));
194                    break;
195
196                  case V4L2_PIX_FMT_YUYV:
197                     ccvt_yuyv_420p( fmt.fmt.pix.width , fmt.fmt.pix.height, buffers[buf.index].start, YBuf, UBuf, VBuf);
198                     break;
199
200                 case V4L2_PIX_FMT_RGB24:
201                        RGB2YUV(fmt.fmt.pix.width, fmt.fmt.pix.height, buffers[buf.index].start, YBuf, UBuf, VBuf, 0);
202                        break;
203
204                 case V4L2_PIX_FMT_SBGGR8:
205                         bayer2rgb24(rgb24_buffer, ((unsigned char *) buffers[buf.index].start), fmt.fmt.pix.width, fmt.fmt.pix.height);
206                         break;
207                }
208                 
209                /*if (dropFrame)
210                {
211                  dropFrame = false;
212                  return 0;
213                } */
214
215                /* Call provided callback function if any */
216                 if (callback)
217                        (*callback)(uptr);
218
219                if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
220                        return errno_exit ("VIDIOC_QBUF", errmsg);
221
222                break;
223
224        case IO_METHOD_USERPTR:
225                CLEAR (buf);
226
227                buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
228                buf.memory = V4L2_MEMORY_USERPTR;
229
230                if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
231                        switch (errno) {
232                        case EAGAIN:
233                                return 0;
234
235                        case EIO:
236                                /* Could ignore EIO, see spec. */
237
238                                /* fall through */
239
240                        default:
241                                errno_exit ("VIDIOC_DQBUF", errmsg);
242                        }
243                }
244
245                for (i = 0; i < n_buffers; ++i)
246                        if (buf.m.userptr == (unsigned long) buffers[i].start
247                            && buf.length == buffers[i].length)
248                                break;
249
250                assert (i < n_buffers);
251
252                //process_image ((void *) buf.m.userptr);
253
254                if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
255                        errno_exit ("VIDIOC_QBUF", errmsg);
256
257                break;
258        }
259
260        return 0;
261}
262
263int V4L2_Base::stop_capturing(char *errmsg)
264{
265        enum v4l2_buf_type type;
266
267        switch (io) {
268        case IO_METHOD_READ:
269                /* Nothing to do. */
270                break;
271
272        case IO_METHOD_MMAP:
273        case IO_METHOD_USERPTR:
274                type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
275
276                IERmCallback(selectCallBackID);
277                selectCallBackID = -1;
278
279                // N.B. I used this as a hack to solve a problem with capturing a frame
280                // long time ago. I recently tried taking this hack off, and it worked fine!
281                //dropFrame = true;
282
283                if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type))
284                        return errno_exit ("VIDIOC_STREAMOFF", errmsg);
285
286               
287
288                break;
289        }
290
291   return 0;
292}
293
294int V4L2_Base::start_capturing(char * errmsg)
295{
296        unsigned int i;
297        enum v4l2_buf_type type;
298
299        switch (io) {
300        case IO_METHOD_READ:
301                /* Nothing to do. */
302                break;
303
304        case IO_METHOD_MMAP:
305                for (i = 0; i < n_buffers; ++i) {
306                        struct v4l2_buffer buf;
307
308                        CLEAR (buf);
309
310                        buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
311                        buf.memory      = V4L2_MEMORY_MMAP;
312                        buf.index       = i;
313
314                        if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
315                                return errno_exit ("VIDIOC_QBUF", errmsg);
316
317                }
318               
319                type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
320
321                if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
322                        return errno_exit ("VIDIOC_STREAMON", errmsg);
323
324               
325
326                selectCallBackID = IEAddCallback(fd, newFrame, this);
327
328                break;
329
330        case IO_METHOD_USERPTR:
331                for (i = 0; i < n_buffers; ++i) {
332                        struct v4l2_buffer buf;
333
334                        CLEAR (buf);
335
336                        buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
337                        buf.memory      = V4L2_MEMORY_USERPTR;
338                        buf.m.userptr   = (unsigned long) buffers[i].start;
339                        buf.length      = buffers[i].length;
340
341                        if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
342                                return errno_exit ("VIDIOC_QBUF", errmsg);
343                }
344
345
346                type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
347
348                if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
349                        return errno_exit ("VIDIOC_STREAMON", errmsg);
350
351                break;
352        }
353
354  return 0;
355
356}
357
358void V4L2_Base::newFrame(int /*fd*/, void *p)
359{
360  char errmsg[ERRMSGSIZ];
361
362  ( (V4L2_Base *) (p))->read_frame(errmsg);
363
364}
365
366int V4L2_Base::uninit_device(char *errmsg)
367{
368
369        switch (io) {
370        case IO_METHOD_READ:
371                free (buffers[0].start);
372                break;
373
374        case IO_METHOD_MMAP:
375                for (unsigned int i = 0; i < n_buffers; ++i)
376                        if (-1 == munmap (buffers[i].start, buffers[i].length))
377                                return errno_exit ("munmap", errmsg);
378                break;
379
380        case IO_METHOD_USERPTR:
381                for (unsigned int i = 0; i < n_buffers; ++i)
382                        free (buffers[i].start);
383                break;
384        }
385
386        free (buffers);
387
388   return 0;
389}
390
391void V4L2_Base::init_read(unsigned int buffer_size)
392{
393        buffers = (buffer *) calloc (1, sizeof (*buffers));
394
395        if (!buffers) {
396                fprintf (stderr, "Out of memory\n");
397                exit (EXIT_FAILURE);
398        }
399
400        buffers[0].length = buffer_size;
401        buffers[0].start = malloc (buffer_size);
402
403        if (!buffers[0].start) {
404                fprintf (stderr, "Out of memory\n");
405                exit (EXIT_FAILURE);
406        }
407}
408
409int V4L2_Base::init_mmap(char *errmsg)
410{
411        struct v4l2_requestbuffers req;
412
413        CLEAR (req);
414
415        req.count               = 4;
416        //req.count               = 1;
417        req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
418        req.memory              = V4L2_MEMORY_MMAP;
419
420        if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) {
421                if (EINVAL == errno) {
422                        fprintf (stderr, "%s does not support "
423                                 "memory mapping\n", dev_name);
424                        snprintf(errmsg, ERRMSGSIZ, "%s does not support "
425                                 "memory mapping\n", dev_name);
426                        return -1;
427                } else {
428                        return errno_exit ("VIDIOC_REQBUFS", errmsg);
429                }
430        }
431
432        if (req.count < 2) {
433                fprintf (stderr, "Insufficient buffer memory on %s\n",
434                         dev_name);
435                snprintf(errmsg, ERRMSGSIZ, "Insufficient buffer memory on %s\n",
436                         dev_name);
437                return -1;
438        }
439
440        buffers = (buffer *) calloc (req.count, sizeof (*buffers));
441
442        if (!buffers)
443        {
444                fprintf (stderr, "buffers. Out of memory\n");
445                strncpy(errmsg, "buffers. Out of memory\n", ERRMSGSIZ);
446                return -1;
447        }
448
449        for (n_buffers = 0; n_buffers < req.count; ++n_buffers)
450        {
451                struct v4l2_buffer buf;
452
453                CLEAR (buf);
454
455                buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
456                buf.memory      = V4L2_MEMORY_MMAP;
457                buf.index       = n_buffers;
458
459                if (-1 == xioctl (fd, VIDIOC_QUERYBUF, &buf))
460                        return errno_exit ("VIDIOC_QUERYBUF", errmsg);
461
462                buffers[n_buffers].length = buf.length;
463                buffers[n_buffers].start =
464                        mmap (NULL /* start anywhere */,
465                              buf.length,
466                              PROT_READ | PROT_WRITE /* required */,
467                              MAP_SHARED /* recommended */,
468                              fd, buf.m.offset);
469
470                if (MAP_FAILED == buffers[n_buffers].start)
471                        return errno_exit ("mmap", errmsg);
472        }
473
474   return 0;
475}
476
477void V4L2_Base::init_userp(unsigned int buffer_size)
478{
479        struct v4l2_requestbuffers req;
480        char errmsg[ERRMSGSIZ];
481
482        CLEAR (req);
483
484        req.count               = 4;
485        req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
486        req.memory              = V4L2_MEMORY_USERPTR;
487
488        if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) {
489                if (EINVAL == errno) {
490                        fprintf (stderr, "%s does not support "
491                                 "user pointer i/o\n", dev_name);
492                        exit (EXIT_FAILURE);
493                } else {
494                        errno_exit ("VIDIOC_REQBUFS", errmsg);
495                }
496        }
497
498        buffers = (buffer *) calloc (4, sizeof (*buffers));
499
500        if (!buffers) {
501                fprintf (stderr, "Out of memory\n");
502                exit (EXIT_FAILURE);
503        }
504
505        for (n_buffers = 0; n_buffers < 4; ++n_buffers) {
506                buffers[n_buffers].length = buffer_size;
507                buffers[n_buffers].start = malloc (buffer_size);
508
509                if (!buffers[n_buffers].start) {
510                        fprintf (stderr, "Out of memory\n");
511                        exit (EXIT_FAILURE);
512                }
513        }
514}
515
516int V4L2_Base::init_device(char *errmsg, int pixelFormat , int width, int height)
517{
518        unsigned int min;
519
520        if (-1 == xioctl (fd, VIDIOC_QUERYCAP, &cap))
521        {
522                if (EINVAL == errno) {
523                        fprintf (stderr, "%s is no V4L2 device\n",
524                                 dev_name);
525                        snprintf(errmsg, ERRMSGSIZ, "%s is no V4L2 device\n", dev_name);
526                        return -1;
527                } else {
528                        return errno_exit ("VIDIOC_QUERYCAP", errmsg);
529                }
530        }
531
532        if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
533        {
534                fprintf (stderr, "%s is no video capture device\n",
535                         dev_name);
536                snprintf(errmsg, ERRMSGSIZ, "%s is no video capture device\n", dev_name);
537                return -1;
538        }
539
540        switch (io) 
541        {
542        case IO_METHOD_READ:
543                if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
544                        fprintf (stderr, "%s does not support read i/o\n",
545                                 dev_name);
546                        snprintf(errmsg, ERRMSGSIZ, "%s does not support read i/o\n",
547                                 dev_name);
548                        return -1;
549                }
550
551                break;
552
553        case IO_METHOD_MMAP:
554        case IO_METHOD_USERPTR:
555                if (!(cap.capabilities & V4L2_CAP_STREAMING))
556                {
557                        fprintf (stderr, "%s does not support streaming i/o\n",
558                                 dev_name);
559                        snprintf(errmsg, ERRMSGSIZ, "%s does not support streaming i/o\n",
560                                 dev_name);
561                        return -1;
562                }
563
564                break;
565        }
566
567        /* Select video input, video standard and tune here. */
568
569        cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
570
571        if (-1 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) {
572                /* Errors ignored. */
573        }
574
575        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
576        crop.c = cropcap.defrect; /* reset to default */
577
578        if (-1 == xioctl (fd, VIDIOC_S_CROP, &crop)) {
579                switch (errno) {
580                case EINVAL:
581                        /* Cropping not supported. */
582                        break;
583                default:
584                        /* Errors ignored. */
585                        break;
586                }
587        }
588
589        CLEAR (fmt);
590
591       fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
592
593       if (-1 == xioctl (fd, VIDIOC_G_FMT, &fmt))
594            return errno_exit ("VIDIOC_G_FMT", errmsg);
595
596        fmt.fmt.pix.width       = (width == -1)       ? fmt.fmt.pix.width : width; 
597        fmt.fmt.pix.height      = (height == -1)      ? fmt.fmt.pix.height : height;
598        fmt.fmt.pix.pixelformat = (pixelFormat == -1) ? fmt.fmt.pix.pixelformat : pixelFormat;
599        //fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;
600
601        if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt))
602                return errno_exit ("VIDIOC_S_FMT", errmsg);
603
604        /* Note VIDIOC_S_FMT may change width and height. */
605
606        /* Buggy driver paranoia. */
607        min = fmt.fmt.pix.width * 2;
608        if (fmt.fmt.pix.bytesperline < min)
609                fmt.fmt.pix.bytesperline = min;
610        min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
611        if (fmt.fmt.pix.sizeimage < min)
612                fmt.fmt.pix.sizeimage = min;
613
614        /* Let's get the actual size */
615        CLEAR(fmt);
616
617        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
618
619        if (-1 == xioctl (fd, VIDIOC_G_FMT, &fmt))
620                return errno_exit ("VIDIOC_G_FMT", errmsg);
621
622        cerr << "width: " << fmt.fmt.pix.width << " - height: " << fmt.fmt.pix.height << endl;
623
624       switch (pixelFormat)
625       {
626        case V4L2_PIX_FMT_YUV420:
627         cerr << "pixel format: V4L2_PIX_FMT_YUV420" << endl;
628        break;
629
630        case V4L2_PIX_FMT_YUYV:
631          cerr << "pixel format: V4L2_PIX_FMT_YUYV" << endl;
632         break;
633
634        case V4L2_PIX_FMT_RGB24:
635          cerr << "pixel format: V4L2_PIX_FMT_RGB24" << endl;
636         break;
637
638        case V4L2_PIX_FMT_SBGGR8:
639         cerr << "pixel format: V4L2_PIX_FMT_SBGGR8" << endl;
640         break;
641
642       }
643
644        findMinMax();
645
646        allocBuffers();
647
648        switch (io)
649        {
650        case IO_METHOD_READ:
651                init_read (fmt.fmt.pix.sizeimage);
652                break;
653
654        case IO_METHOD_MMAP:
655                return init_mmap(errmsg);
656                break;
657
658        case IO_METHOD_USERPTR:
659                init_userp (fmt.fmt.pix.sizeimage);
660                break;
661        }
662
663  return 0;
664}
665
666void V4L2_Base::close_device(void)
667{
668        char errmsg[ERRMSGSIZ];
669
670        if (-1 == close (fd))
671                errno_exit ("close", errmsg);
672
673        fd = -1;
674}
675
676int V4L2_Base::open_device(const char *devpath, char *errmsg)
677{
678        struct stat st; 
679
680        strncpy(dev_name, devpath, 64);
681
682        if (-1 == stat (dev_name, &st)) {
683                fprintf (stderr, "Cannot identify '%s': %d, %s\n",
684                         dev_name, errno, strerror (errno));
685                snprintf(errmsg, ERRMSGSIZ, "Cannot identify '%s': %d, %s\n",
686                         dev_name, errno, strerror (errno));
687                return -1;
688        }
689
690        if (!S_ISCHR (st.st_mode))
691        {
692                fprintf (stderr, "%s is no device\n", dev_name);
693                snprintf(errmsg, ERRMSGSIZ, "%s is no device\n", dev_name);
694                return -1;
695        }
696
697        fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
698
699        if (-1 == fd)
700        {
701                fprintf (stderr, "Cannot open '%s': %d, %s\n",
702                         dev_name, errno, strerror (errno));
703                snprintf(errmsg, ERRMSGSIZ, "Cannot open '%s': %d, %s\n",
704                         dev_name, errno, strerror (errno));
705                return -1;
706        }
707
708  return 0;
709}
710
711
712
713int V4L2_Base::getWidth()
714{
715  return fmt.fmt.pix.width;
716}
717
718int V4L2_Base::getHeight()
719{
720 return fmt.fmt.pix.height;
721}
722
723void V4L2_Base::setFPS(int fps)
724{
725  frameRate = 15;//fps;
726}
727
728int V4L2_Base::getFPS()
729{
730  return 15;
731}
732
733char * V4L2_Base::getDeviceName()
734{
735  return ((char *) cap.card);
736}
737
738void V4L2_Base::allocBuffers()
739{
740   delete (YBuf); YBuf = NULL;
741   delete (UBuf); UBuf = NULL;
742   delete (VBuf); VBuf = NULL;
743   delete (colorBuffer); colorBuffer = NULL;
744   delete (rgb24_buffer); rgb24_buffer = NULL;
745   
746   YBuf= new unsigned char[ fmt.fmt.pix.width * fmt.fmt.pix.height];
747   UBuf= new unsigned char[ fmt.fmt.pix.width * fmt.fmt.pix.height];
748   VBuf= new unsigned char[ fmt.fmt.pix.width * fmt.fmt.pix.height];
749   colorBuffer = new unsigned char[fmt.fmt.pix.width * fmt.fmt.pix.height * 4];
750
751   if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_SBGGR8)
752     rgb24_buffer = new unsigned char[fmt.fmt.pix.width * fmt.fmt.pix.height * 3];
753}
754
755void V4L2_Base::getMaxMinSize(int & x_max, int & y_max, int & x_min, int & y_min)
756{
757  x_max = xmax; y_max = ymax; x_min = xmin; y_min = ymin;
758}
759
760int V4L2_Base::setSize(int x, int y)
761{
762   char errmsg[ERRMSGSIZ];
763   int oldW, oldH;
764 
765   oldW = fmt.fmt.pix.width;
766   oldH = fmt.fmt.pix.height;
767
768   fmt.fmt.pix.width  = x;
769   fmt.fmt.pix.height = y;
770
771   if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt))
772   {
773        fmt.fmt.pix.width  = oldW;
774        fmt.fmt.pix.height = oldH;
775        return errno_exit ("VIDIOC_S_FMT", errmsg);
776   }
777
778   /* PWC bug? It seems that setting the "wrong" width and height will mess something in the driver.
779      Only 160x120, 320x280, and 640x480 are accepted. If I try to set it for example to 300x200, it wii
780      get set to 320x280, which is fine, but then the video information is messed up for some reason. */
781      xioctl (fd, VIDIOC_S_FMT, &fmt);
782 
783 
784   allocBuffers();
785
786  return 0;
787}
788
789void V4L2_Base::setContrast(int val)
790{
791   /*picture_.contrast=val;
792   updatePictureSettings();*/
793}
794
795int V4L2_Base::getContrast()
796{
797   return 255;//picture_.contrast;
798}
799
800void V4L2_Base::setBrightness(int val)
801{
802   /*picture_.brightness=val;
803   updatePictureSettings();*/
804}
805
806int V4L2_Base::getBrightness()
807{
808   return 255;//picture_.brightness;
809}
810
811void V4L2_Base::setColor(int val)
812{
813   /*picture_.colour=val;
814   updatePictureSettings();*/
815}
816
817int V4L2_Base::getColor()
818{
819   return 255; //picture_.colour;
820}
821
822void V4L2_Base::setHue(int val)
823{
824   /*picture_.hue=val;
825   updatePictureSettings();*/
826}
827
828int V4L2_Base::getHue()
829{
830   return 255;//picture_.hue;
831}
832
833void V4L2_Base::setWhiteness(int val)
834{
835   /*picture_.whiteness=val;
836   updatePictureSettings();*/
837}
838
839int V4L2_Base::getWhiteness() 
840{
841   return 255;//picture_.whiteness;
842}
843
844void V4L2_Base::setPictureSettings()
845{
846   /*if (ioctl(device_, VIDIOCSPICT, &picture_) ) {
847      cerr << "updatePictureSettings" << endl;
848   }
849   ioctl(device_, VIDIOCGPICT, &picture_);*/
850}
851
852void V4L2_Base::getPictureSettings()
853{
854   /*if (ioctl(device_, VIDIOCGPICT, &picture_) )
855   {
856      cerr << "refreshPictureSettings" << endl;
857   }*/
858}
859
860unsigned char * V4L2_Base::getY()
861{
862  if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_SBGGR8)
863      RGB2YUV(fmt.fmt.pix.width, fmt.fmt.pix.height, rgb24_buffer, YBuf, UBuf, VBuf, 0);
864
865  return YBuf;
866}
867
868unsigned char * V4L2_Base::getU()
869{
870 return UBuf;
871}
872
873unsigned char * V4L2_Base::getV()
874{
875 return VBuf;
876}
877
878unsigned char * V4L2_Base::getColorBuffer()
879{
880  //cerr << "in get color buffer " << endl;
881
882  switch (fmt.fmt.pix.pixelformat) 
883  {
884      case V4L2_PIX_FMT_YUV420:
885        ccvt_420p_bgr32(fmt.fmt.pix.width, fmt.fmt.pix.height,
886                      buffers[0].start, (void*)colorBuffer);
887      break;
888
889    case V4L2_PIX_FMT_YUYV:
890         ccvt_yuyv_bgr32(fmt.fmt.pix.width, fmt.fmt.pix.height,
891                      buffers[0].start, (void*)colorBuffer);
892         break;
893
894    case V4L2_PIX_FMT_RGB24:
895         ccvt_rgb24_bgr32(fmt.fmt.pix.width, fmt.fmt.pix.height,
896                      buffers[0].start, (void*)colorBuffer);
897         break;
898
899    case V4L2_PIX_FMT_SBGGR8:
900         ccvt_rgb24_bgr32(fmt.fmt.pix.width, fmt.fmt.pix.height,
901                      rgb24_buffer, (void*)colorBuffer);
902         break;
903
904   default:
905    break;
906  }
907
908  return colorBuffer;
909}
910
911void V4L2_Base::registerCallback(WPF *fp, void *ud)
912{
913  callback = fp;
914  uptr = ud;
915}
916
917void V4L2_Base::findMinMax()
918{
919    char errmsg[ERRMSGSIZ];
920    struct v4l2_format tryfmt;
921    CLEAR(tryfmt);
922
923    xmin = xmax = fmt.fmt.pix.width;
924    ymin = ymax = fmt.fmt.pix.height;
925
926    tryfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
927    tryfmt.fmt.pix.width       = 10;
928    tryfmt.fmt.pix.height      = 10;
929    tryfmt.fmt.pix.pixelformat = fmt.fmt.pix.pixelformat;
930    tryfmt.fmt.pix.field       = fmt.fmt.pix.field;
931
932    if (-1 == xioctl (fd, VIDIOC_TRY_FMT, &tryfmt))
933    {
934        errno_exit ("VIDIOC_TRY_FMT 1", errmsg);
935        return;
936    }
937   
938    xmin = tryfmt.fmt.pix.width;
939    ymin = tryfmt.fmt.pix.height;
940
941    tryfmt.fmt.pix.width       = 1600;
942    tryfmt.fmt.pix.height      = 1200;
943
944    if (-1 == xioctl (fd, VIDIOC_TRY_FMT, &tryfmt))
945    {
946                errno_exit ("VIDIOC_TRY_FMT 2", errmsg);
947                return;
948    }
949
950    xmax = tryfmt.fmt.pix.width;
951    ymax = tryfmt.fmt.pix.height;
952
953    cerr << "Min X: " << xmin << " - Max X: " << xmax << " - Min Y: " << ymin << " - Max Y: " << ymax << endl;
954}
955
956void V4L2_Base::enumerate_ctrl (void)
957{
958  char errmsg[ERRMSGSIZ];
959  CLEAR(queryctrl);
960
961  for (queryctrl.id = V4L2_CID_BASE; queryctrl.id < V4L2_CID_LASTP1; queryctrl.id++)
962  {
963        if (0 == xioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
964        {
965               cerr << "Control " << queryctrl.name << endl;
966
967                if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
968                        continue;
969
970                cerr << "Control " << queryctrl.name << endl;
971
972                if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
973                        enumerate_menu ();
974        } else
975        {
976                if (errno == EINVAL)
977                        continue;
978
979                errno_exit("VIDIOC_QUERYCTRL", errmsg);
980                return;
981        }
982  }
983
984  for (queryctrl.id = V4L2_CID_PRIVATE_BASE;  ;  queryctrl.id++)
985  {
986        if (0 == xioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
987        {
988              cerr << "Private Control " << queryctrl.name << endl;
989
990                if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
991                        continue;
992
993                if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
994                        enumerate_menu ();
995        } else {
996                if (errno == EINVAL)
997                        break;
998
999                errno_exit ("VIDIOC_QUERYCTRL", errmsg);
1000                return;
1001        }
1002
1003  }
1004
1005}
1006
1007void V4L2_Base::enumerate_menu (void)
1008{
1009  char errmsg[ERRMSGSIZ];
1010  cerr << "  Menu items:" << endl;
1011
1012        CLEAR(querymenu);
1013        querymenu.id = queryctrl.id;
1014
1015        for (querymenu.index = queryctrl.minimum;  querymenu.index <= queryctrl.maximum; querymenu.index++)
1016         {
1017                if (0 == xioctl (fd, VIDIOC_QUERYMENU, &querymenu))
1018                {
1019                        cerr << "  " <<  querymenu.name << endl;
1020                } else
1021                {
1022                        errno_exit("VIDIOC_QUERYMENU", errmsg);
1023                        return;
1024                }
1025         }
1026}
1027
1028int  V4L2_Base::query_ctrl(unsigned int ctrl_id, double & ctrl_min, double & ctrl_max, double & ctrl_step, double & ctrl_value, char *errmsg)
1029{ 
1030
1031    struct v4l2_control control;
1032
1033    CLEAR(queryctrl);
1034
1035    queryctrl.id = ctrl_id;
1036
1037   if (-1 == ioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
1038   {
1039        if (errno != EINVAL) 
1040                return errno_exit ("VIDIOC_QUERYCTRL", errmsg);
1041               
1042        else 
1043        {
1044                cerr << "#" << ctrl_id << " is not supported" << endl;
1045                snprintf(errmsg, ERRMSGSIZ, "# %d is not supported", ctrl_id);
1046                return -1;
1047        }
1048   } else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
1049   {
1050        cerr << "#" << ctrl_id << " is disabled" << endl;
1051        snprintf(errmsg, ERRMSGSIZ, "# %d is disabled", ctrl_id);
1052        return -1;
1053   }
1054
1055   ctrl_min   = queryctrl.minimum;
1056   ctrl_max   = queryctrl.maximum;
1057   ctrl_step  = queryctrl.step;
1058   ctrl_value = queryctrl.default_value;
1059
1060   /* Get current value */
1061   CLEAR(control);
1062   control.id = ctrl_id;
1063
1064   if (0 == xioctl(fd, VIDIOC_G_CTRL, &control))
1065      ctrl_value = control.value;
1066
1067    cerr << queryctrl.name << " -- min: " << ctrl_min << " max: " << ctrl_max << " step: " << ctrl_step << " value: " << ctrl_value << endl;
1068
1069    return 0;
1070
1071}
1072
1073int  V4L2_Base::queryINTControls(INumberVectorProperty *nvp)
1074{
1075   struct v4l2_control control;
1076   char errmsg[ERRMSGSIZ];
1077   CLEAR(queryctrl);
1078   INumber *numbers = NULL;
1079   unsigned int *num_ctrls = NULL;
1080   int nnum=0;
1081
1082  for (queryctrl.id = V4L2_CID_BASE; queryctrl.id < V4L2_CID_LASTP1; queryctrl.id++)
1083  {
1084        if (0 == ioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
1085        {
1086                if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
1087                {
1088                    cerr << queryctrl.name << " is disabled." << endl;
1089                    continue;
1090                }
1091
1092                if (queryctrl.type == V4L2_CTRL_TYPE_INTEGER)
1093                {
1094                   numbers = (numbers == NULL) ? (INumber *) malloc (sizeof(INumber)) :
1095                                                 (INumber *) realloc (numbers, (nnum+1) * sizeof (INumber));
1096
1097                   num_ctrls = (num_ctrls == NULL) ? (unsigned int *) malloc  (sizeof (unsigned int)) :
1098                                                     (unsigned int *) realloc (num_ctrls, (nnum+1) * sizeof (unsigned int));
1099
1100                   strncpy(numbers[nnum].name, ((char *) queryctrl.name) , MAXINDINAME);
1101                   strncpy(numbers[nnum].label, ((char *) queryctrl.name), MAXINDILABEL);
1102                   strncpy(numbers[nnum].format, "%0.f", MAXINDIFORMAT);
1103                   numbers[nnum].min    = queryctrl.minimum;
1104                   numbers[nnum].max    = queryctrl.maximum;
1105                   numbers[nnum].step   = queryctrl.step;
1106                   numbers[nnum].value  = queryctrl.default_value;
1107     
1108                   /* Get current value if possible */
1109                   CLEAR(control);
1110                   control.id = queryctrl.id;
1111                   if (0 == xioctl(fd, VIDIOC_G_CTRL, &control))
1112                            numbers[nnum].value = control.value;
1113
1114                   /* Store ID info in INumber. This is the first time ever I make use of aux0!! */
1115                   num_ctrls[nnum] = queryctrl.id;
1116
1117                   cerr << queryctrl.name << " -- min: " << queryctrl.minimum << " max: " << queryctrl.maximum << " step: " << queryctrl.step << " value: " << numbers[nnum].value << endl;
1118
1119                   nnum++;
1120                 
1121                }
1122        } else if (errno != EINVAL) {
1123                if(numbers) free(numbers);
1124                return errno_exit ("VIDIOC_QUERYCTRL", errmsg);
1125        }
1126
1127   }
1128
1129  for (queryctrl.id = V4L2_CID_PRIVATE_BASE;  ;  queryctrl.id++)
1130  {
1131     if (0 == ioctl (fd, VIDIOC_QUERYCTRL, &queryctrl))
1132        {
1133                if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
1134                {
1135                    cerr << queryctrl.name << " is disabled." << endl;
1136                    continue;
1137                }
1138
1139                if (queryctrl.type == V4L2_CTRL_TYPE_INTEGER)
1140                {
1141                   numbers = (numbers == NULL) ? (INumber *) malloc (sizeof(INumber)) :
1142                                                 (INumber *) realloc (numbers, (nnum+1) * sizeof (INumber));
1143
1144                   num_ctrls = (num_ctrls == NULL) ? (unsigned int *) malloc  (sizeof (unsigned int)) :
1145                                                     (unsigned int *) realloc (num_ctrls, (nnum+1) * sizeof (unsigned int));
1146
1147                   strncpy(numbers[nnum].name, ((char *) queryctrl.name) , MAXINDINAME);
1148                   strncpy(numbers[nnum].label, ((char *) queryctrl.name), MAXINDILABEL);
1149                   strncpy(numbers[nnum].format, "%0.f", MAXINDIFORMAT);
1150                   numbers[nnum].min    = queryctrl.minimum;
1151                   numbers[nnum].max    = queryctrl.maximum;
1152                   numbers[nnum].step   = queryctrl.step;
1153                   numbers[nnum].value  = queryctrl.default_value;
1154     
1155                   /* Get current value if possible */
1156                   CLEAR(control);
1157                   control.id = queryctrl.id;
1158                   if (0 == xioctl(fd, VIDIOC_G_CTRL, &control))
1159                            numbers[nnum].value = control.value;
1160
1161                   /* Store ID info in INumber. This is the first time ever I make use of aux0!! */
1162                   num_ctrls[nnum] = queryctrl.id;
1163     
1164                   nnum++;
1165                 
1166                }
1167         }
1168         else break;
1169   } 
1170
1171  /* Store numbers in aux0 */
1172  for (int i=0; i < nnum; i++)
1173      numbers[i].aux0 = &num_ctrls[i];
1174
1175  nvp->np  = numbers;
1176  nvp->nnp = nnum;
1177
1178  return nnum;
1179
1180}
1181
1182int  V4L2_Base::setINTControl(unsigned int ctrl_id, double new_value, char *errmsg)
1183{
1184   struct v4l2_control control;
1185
1186   CLEAR(control);
1187
1188   //cerr << "The id is " << ctrl_id << " new value is " << new_value << endl;
1189
1190   control.id = ctrl_id;
1191   control.value = (int) new_value;
1192
1193   if (-1 == xioctl(fd, VIDIOC_S_CTRL, &control))
1194         return errno_exit ("VIDIOC_S_CTRL", errmsg);
1195
1196   return 0;
1197}
1198
Note: See TracBrowser for help on using the repository browser.