v4l2uvc.c
Go to the documentation of this file.
00001 /*******************************************************************************
00002 #               uvcview: Sdl video Usb Video Class grabber           .         #
00003 #This package work with the Logitech UVC based webcams with the mjpeg feature. #
00004 #All the decoding is in user space with the embedded jpeg decoder              #
00005 #.                                                                             #
00006 #               Copyright (C) 2005 2006 Laurent Pinchart &&  Michel Xhaard     #
00007 #                                                                              #
00008 # This program is free software; you can redistribute it and/or modify         #
00009 # it under the terms of the GNU General Public License as published by         #
00010 # the Free Software Foundation; either version 2 of the License, or            #
00011 # (at your option) any later version.                                          #
00012 #                                                                              #
00013 # This program is distributed in the hope that it will be useful,              #
00014 # but WITHOUT ANY WARRANTY; without even the implied warranty of               #
00015 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the                #
00016 # GNU General Public License for more details.                                 #
00017 #                                                                              #
00018 # You should have received a copy of the GNU General Public License            #
00019 # along with this program; if not, write to the Free Software                  #
00020 # Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA    #
00021 #                                                                              #
00022 *******************************************************************************/
00023 
00024 #include <stdlib.h>
00025 #include <math.h>
00026 #include <float.h>
00027 
00028 #include <libv4l2.h>
00029 
00030 #include "luvcview/v4l2uvc.h"
00031 
00032 #define ARRAY_SIZE(a)           (sizeof(a) / sizeof((a)[0]))
00033 #define FOURCC_FORMAT           "%c%c%c%c"
00034 #define FOURCC_ARGS(c)          (c) & 0xFF, ((c) >> 8) & 0xFF, ((c) >> 16) & 0xFF, ((c) >> 24) & 0xFF
00035         
00036 
00037 static int debug = 0;
00038 
00039 
00040 
00041 static int init_v4l2(struct vdIn *vd);
00042 
00043 static int float_to_fraction_recursive(double f, double p, int *num, int *den)
00044 {
00045         int whole = (int)f;
00046         f = fabs(f - whole);
00047 
00048         if(f > p) {
00049                 int n, d;
00050                 int a = float_to_fraction_recursive(1 / f, p + p / f, &n, &d);
00051                 *num = d;
00052                 *den = d * a + n;
00053         }
00054         else {
00055                 *num = 0;
00056                 *den = 1;
00057         }
00058         return whole;
00059 }
00060 
00061 static void float_to_fraction(float f, int *num, int *den)
00062 {
00063         int whole = float_to_fraction_recursive(f, FLT_EPSILON, num, den);
00064         *num += whole * *den;
00065 }
00066 
00067 int check_videoIn(struct vdIn *vd, char *device)
00068 {
00069         int ret;
00070         if (vd == NULL || device == NULL)
00071                 return -1;
00072         vd->videodevice = (char *) calloc(1, 16 * sizeof(char));
00073         snprintf(vd->videodevice, 12, "%s", device);
00074         printf("Device information:\n");
00075         printf("  Device path:  %s\n", vd->videodevice);
00076         if ((vd->fd = v4l2_open(vd->videodevice, O_RDWR)) == -1) {
00077                 perror("ERROR opening V4L interface");
00078                 exit(1);
00079         }
00080         memset(&vd->cap, 0, sizeof(struct v4l2_capability));
00081         ret = v4l2_ioctl(vd->fd, VIDIOC_QUERYCAP, &vd->cap);
00082         if (ret < 0) {
00083                 printf("Error opening device %s: unable to query device.\n",
00084                                 vd->videodevice);
00085                 goto fatal;
00086         }
00087         if ((vd->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
00088                 printf("Error opening device %s: video capture not supported.\n",
00089                                 vd->videodevice);
00090         }
00091         if (!(vd->cap.capabilities & V4L2_CAP_STREAMING)) {
00092                 printf("%s does not support streaming i/o\n", vd->videodevice);
00093         }
00094         if (!(vd->cap.capabilities & V4L2_CAP_READWRITE)) {
00095                 printf("%s does not support read i/o\n", vd->videodevice);
00096         }
00097         enum_frame_formats(vd->fd, NULL, 0);
00098 fatal:    
00099         v4l2_close(vd->fd);
00100         free(vd->videodevice);
00101         return 0;
00102 }
00103 int
00104 init_videoIn(struct vdIn *vd, char *device, int width, int height, float fps,
00105              int format, int grabmethod, char *avifilename)
00106 {
00107    int ret = -1;
00108     int i;
00109     if (vd == NULL || device == NULL)
00110         return -1;
00111     if (width == 0 || height == 0)
00112         return -1;
00113     if (grabmethod < 0 || grabmethod > 1)
00114         grabmethod = 1;         //mmap by default;
00115     vd->videodevice = NULL;
00116     vd->status = NULL;
00117     vd->pictName = NULL;
00118     vd->videodevice = (char *) calloc(1, 16 * sizeof(char));
00119     vd->status = (char *) calloc(1, 100 * sizeof(char));
00120     vd->pictName = (char *) calloc(1, 80 * sizeof(char));
00121     snprintf(vd->videodevice, 12, "%s", device);
00122         printf("Device information:\n");
00123         printf("  Device path:  %s\n", vd->videodevice);
00124     vd->recordtime = 0;
00125     vd->framecount = 0;
00126     vd->recordstart = 0;
00127     vd->getPict = 0;
00128     vd->signalquit = 1;
00129     vd->width = width;
00130     vd->height = height;
00131     vd->fps = fps;
00132     vd->formatIn = format;
00133     vd->grabmethod = grabmethod;
00134     vd->fileCounter = 0;
00135     vd->rawFrameCapture = 0;
00136     vd->rfsBytesWritten = 0;
00137     vd->rfsFramesWritten = 0;
00138     vd->captureFile = NULL;
00139     vd->bytesWritten = 0;
00140     vd->framesWritten = 0;
00141     if (init_v4l2(vd) < 0) {
00142         printf(" Init v4L2 failed !! exit fatal\n");
00143         goto error;;
00144     }
00145     /* alloc a temp buffer to reconstruct the pict */
00146     vd->framesizeIn = (vd->width * vd->height << 1);
00147     switch (vd->formatIn) {
00148     case V4L2_PIX_FMT_MJPEG:
00149         vd->tmpbuffer =
00150             (unsigned char *) calloc(1, (size_t) vd->framesizeIn);
00151         if (!vd->tmpbuffer)
00152             goto error;
00153         vd->framebuffer =
00154             (unsigned char *) calloc(1,
00155                                      (size_t) vd->width * (vd->height +
00156                                                            8) * 2);
00157         break;
00158     case V4L2_PIX_FMT_YUYV:
00159     case V4L2_PIX_FMT_UYVY:
00160         vd->framebuffer =
00161             (unsigned char *) calloc(1, (size_t) vd->framesizeIn);
00162         break;
00163     default:
00164         printf(" should never arrive exit fatal !!\n");
00165         goto error;
00166         break;
00167     }
00168     if (!vd->framebuffer)
00169         goto error;
00170     return 0;
00171   error:
00172     free(vd->videodevice);
00173     free(vd->status);
00174     free(vd->pictName);
00175     v4l2_close(vd->fd);
00176     return -1;
00177 }
00178 int enum_controls(int vd) //struct vdIn *vd)
00179 {    
00180   struct v4l2_queryctrl queryctrl;
00181   struct v4l2_querymenu querymenu;
00182   struct v4l2_control   control_s;
00183   struct v4l2_input*    getinput;
00184 
00185   //Name of the device
00186   getinput=(struct v4l2_input *) calloc(1, sizeof(struct v4l2_input));
00187   memset(getinput, 0, sizeof(struct v4l2_input));
00188   getinput->index=0;
00189   v4l2_ioctl(vd,VIDIOC_ENUMINPUT , getinput);
00190   printf ("Available controls of device '%s' (Type 1=Integer 2=Boolean 3=Menu 4=Button)\n", getinput->name);
00191 
00192   //subroutine to read menu items of controls with type 3
00193   void enumerate_menu (void) {
00194     printf ("  Menu items:\n");
00195     memset (&querymenu, 0, sizeof (querymenu));
00196     querymenu.id = queryctrl.id;
00197     for (querymenu.index = queryctrl.minimum;
00198          querymenu.index <= queryctrl.maximum;
00199          querymenu.index++) {
00200       if (0 == ioctl (vd, VIDIOC_QUERYMENU, &querymenu)) {
00201         printf ("  index:%d name:%s\n", querymenu.index, querymenu.name);
00202         SDL_Delay(10);
00203       } else {
00204         printf ("error getting control menu");
00205         break;
00206       }
00207     }
00208   }
00209 
00210   //predefined controls
00211   printf ("V4L2_CID_BASE         (predefined controls):\n");
00212   memset (&queryctrl, 0, sizeof (queryctrl));
00213   for (queryctrl.id = V4L2_CID_BASE;
00214        queryctrl.id < V4L2_CID_LASTP1;
00215        queryctrl.id++) {
00216     if (0 == ioctl (vd, VIDIOC_QUERYCTRL, &queryctrl)) {
00217       if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
00218         continue;
00219       control_s.id=queryctrl.id;
00220       v4l2_ioctl(vd, VIDIOC_G_CTRL, &control_s);
00221       SDL_Delay(10);
00222       printf (" index:%-10d name:%-32s type:%d min:%-5d max:%-5d step:%-5d def:%-5d now:%d\n",
00223               queryctrl.id, queryctrl.name, queryctrl.type, queryctrl.minimum,
00224               queryctrl.maximum, queryctrl.step, queryctrl.default_value, control_s.value);
00225       if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
00226         enumerate_menu ();
00227     } else {
00228       if (errno == EINVAL)
00229         continue;
00230       perror ("error getting base controls");
00231       goto fatal_controls;
00232     }
00233   }
00234 
00235   //driver specific controls
00236   printf ("V4L2_CID_PRIVATE_BASE (driver specific controls):\n");
00237   for (queryctrl.id = V4L2_CID_PRIVATE_BASE;;
00238        queryctrl.id++) {
00239     if (0 == ioctl (vd, VIDIOC_QUERYCTRL, &queryctrl)) {
00240       if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
00241         continue;
00242       control_s.id=queryctrl.id;
00243       v4l2_ioctl(vd, VIDIOC_G_CTRL, &control_s);
00244       SDL_Delay(20);
00245       printf (" index:%-10d name:%-32s type:%d min:%-5d max:%-5d step:%-5d def:%-5d now:%d\n",
00246               queryctrl.id, queryctrl.name, queryctrl.type, queryctrl.minimum,
00247               queryctrl.maximum, queryctrl.step, queryctrl.default_value, control_s.value);
00248       if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
00249         enumerate_menu ();
00250     } else {
00251       if (errno == EINVAL)
00252         break;
00253       perror ("error getting private base controls");
00254       goto fatal_controls;
00255       }
00256   }
00257   return 0;
00258  fatal_controls:
00259   return -1;  
00260 }
00261 int save_controls(int vd, const char* filename)
00262 { 
00263   struct v4l2_queryctrl queryctrl;
00264   struct v4l2_control   control_s;
00265   FILE *configfile;
00266   memset (&queryctrl, 0, sizeof (queryctrl));
00267   memset (&control_s, 0, sizeof (control_s));
00268   configfile = fopen(filename, "w");
00269   if ( configfile == NULL) {
00270     perror("saving configfile luvcview.cfg failed");
00271   }
00272   else {
00273     fprintf(configfile, "id         value      # luvcview control settings configuration file\n");
00274     for (queryctrl.id = V4L2_CID_BASE;
00275          queryctrl.id < V4L2_CID_LASTP1;
00276          queryctrl.id++) {
00277       if (0 == ioctl (vd, VIDIOC_QUERYCTRL, &queryctrl)) {
00278         if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
00279           continue;
00280         control_s.id=queryctrl.id;
00281         v4l2_ioctl(vd, VIDIOC_G_CTRL, &control_s);
00282         SDL_Delay(10);
00283         fprintf (configfile, "%-10d %-10d # name:%-32s type:%d min:%-5d max:%-5d step:%-5d def:%d\n",
00284                  queryctrl.id, control_s.value, queryctrl.name, queryctrl.type, queryctrl.minimum,
00285                  queryctrl.maximum, queryctrl.step, queryctrl.default_value);
00286         printf ("%-10d %-10d # name:%-32s type:%d min:%-5d max:%-5d step:%-5d def:%d\n",
00287                 queryctrl.id, control_s.value, queryctrl.name, queryctrl.type, queryctrl.minimum,
00288                 queryctrl.maximum, queryctrl.step, queryctrl.default_value);
00289         SDL_Delay(10);
00290       }
00291     }
00292     for (queryctrl.id = V4L2_CID_PRIVATE_BASE;;
00293          queryctrl.id++) {
00294       if (0 == ioctl (vd, VIDIOC_QUERYCTRL, &queryctrl)) {
00295         if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
00296           continue;
00297         if ((queryctrl.id==134217735) || (queryctrl.id==134217736))
00298           continue;
00299         control_s.id=queryctrl.id;
00300         v4l2_ioctl(vd, VIDIOC_G_CTRL, &control_s);
00301         SDL_Delay(10);
00302         fprintf (configfile, "%-10d %-10d # name:%-32s type:%d min:%-5d max:%-5d step:%-5d def:%d\n",
00303                  queryctrl.id, control_s.value, queryctrl.name, queryctrl.type, queryctrl.minimum,
00304                  queryctrl.maximum, queryctrl.step, queryctrl.default_value);
00305         printf ("%-10d %-10d # name:%-32s type:%d min:%-5d max:%-5d step:%-5d def:%d\n",
00306                 queryctrl.id, control_s.value, queryctrl.name, queryctrl.type, queryctrl.minimum,
00307                 queryctrl.maximum, queryctrl.step, queryctrl.default_value);
00308       } else {
00309         if (errno == EINVAL)
00310           break;
00311       }
00312     }
00313     fflush(configfile);
00314     fclose(configfile);
00315     SDL_Delay(100);
00316   }
00317 }
00318 
00319 
00320 int load_controls(int vd, const char* filename) //struct vdIn *vd)
00321 {
00322   struct v4l2_control   control;
00323   FILE *configfile;
00324   memset (&control, 0, sizeof (control));
00325   configfile = fopen(filename, "r");
00326   if ( configfile == NULL) {
00327     perror("configfile luvcview.cfg open failed");
00328   }
00329   else {
00330     printf("loading controls from luvcview.cfg\n");
00331     char buffer[512]; 
00332     fgets(buffer, sizeof(buffer), configfile);
00333     while (NULL !=fgets(buffer, sizeof(buffer), configfile) )
00334       {
00335         sscanf(buffer, "%i%i", &control.id, &control.value);
00336         if (v4l2_ioctl(vd, VIDIOC_S_CTRL, &control))
00337           printf("ERROR id:%d val:%d\n", control.id, control.value);
00338         else
00339           printf("OK    id:%d val:%d\n", control.id, control.value);
00340         SDL_Delay(20);
00341       }   
00342     fclose(configfile);
00343   }
00344 }
00345 
00346 static int init_v4l2(struct vdIn *vd)
00347 {
00348         int i;
00349         int ret = 0;
00350 
00351         if ((vd->fd = v4l2_open(vd->videodevice, O_RDWR)) == -1) {
00352                 perror("ERROR opening V4L interface");
00353                 exit(1);
00354         }
00355         memset(&vd->cap, 0, sizeof(struct v4l2_capability));
00356         ret = v4l2_ioctl(vd->fd, VIDIOC_QUERYCAP, &vd->cap);
00357         if (ret < 0) {
00358                 printf("Error opening device %s: unable to query device.\n",
00359                                 vd->videodevice);
00360                 goto fatal;
00361         }
00362 
00363         if ((vd->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
00364                 printf("Error opening device %s: video capture not supported.\n",
00365                                 vd->videodevice);
00366                 goto fatal;;
00367         }
00368         if (vd->grabmethod) {
00369                 if (!(vd->cap.capabilities & V4L2_CAP_STREAMING)) {
00370                         printf("%s does not support streaming i/o\n", vd->videodevice);
00371                         goto fatal;
00372                 }
00373         } else {
00374                 if (!(vd->cap.capabilities & V4L2_CAP_READWRITE)) {
00375                         printf("%s does not support read i/o\n", vd->videodevice);
00376                         goto fatal;
00377                 }
00378         }
00379 
00380         printf("Stream settings:\n");
00381 
00382         // Enumerate the supported formats to check whether the requested one
00383         // is available. If not, we try to fall back to YUYV.
00384         unsigned int device_formats[16] = { 0 };        // Assume no device supports more than 16 formats
00385         int requested_format_found = 0, fallback_format = -1;
00386         if(enum_frame_formats(vd->fd, device_formats, ARRAY_SIZE(device_formats))) {
00387                 printf("Unable to enumerate frame formats");
00388                 goto fatal;
00389         }
00390         for(i = 0; i < ARRAY_SIZE(device_formats) && device_formats[i]; i++) {
00391                 if(device_formats[i] == vd->formatIn) {
00392                         requested_format_found = 1;
00393                         break;
00394                 }
00395                 if(device_formats[i] == V4L2_PIX_FMT_MJPEG || device_formats[i] == V4L2_PIX_FMT_YUYV
00396                         || device_formats[i] == V4L2_PIX_FMT_UYVY)
00397 
00398                         fallback_format = i;
00399         }
00400         if(requested_format_found) {
00401                 // The requested format is supported
00402                 printf("  Frame format: "FOURCC_FORMAT"\n", FOURCC_ARGS(vd->formatIn));
00403         }
00404         else if(fallback_format >= 0) {
00405                 // The requested format is not supported but there's a fallback format
00406                 printf("  Frame format: "FOURCC_FORMAT" ("FOURCC_FORMAT
00407                         " is not supported by device)\n",
00408                         FOURCC_ARGS(device_formats[0]), FOURCC_ARGS(vd->formatIn));
00409                 vd->formatIn = device_formats[0];
00410         }
00411         else {
00412                 // The requested format is not supported and no fallback format is available
00413                 printf("ERROR: Requested frame format "FOURCC_FORMAT" is not available "
00414                         "and no fallback format was found.\n", FOURCC_ARGS(vd->formatIn));
00415                 goto fatal;
00416         }
00417 
00418         // Set pixel format and frame size
00419         memset(&vd->fmt, 0, sizeof(struct v4l2_format));
00420         vd->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00421         vd->fmt.fmt.pix.width = vd->width;
00422         vd->fmt.fmt.pix.height = vd->height;
00423         vd->fmt.fmt.pix.pixelformat = vd->formatIn;
00424         vd->fmt.fmt.pix.field = V4L2_FIELD_ANY;
00425         ret = v4l2_ioctl(vd->fd, VIDIOC_S_FMT, &vd->fmt);
00426         if (ret < 0) {
00427                 perror("Unable to set format");
00428                 goto fatal;
00429         }
00430         if ((vd->fmt.fmt.pix.width != vd->width) ||
00431                 (vd->fmt.fmt.pix.height != vd->height)) {
00432                 printf("  Frame size:   %ux%u (requested size %ux%u is not supported by device)\n",
00433                         vd->fmt.fmt.pix.width, vd->fmt.fmt.pix.height, vd->width, vd->height);
00434                 vd->width = vd->fmt.fmt.pix.width;
00435                 vd->height = vd->fmt.fmt.pix.height;
00436                 /* look the format is not part of the deal ??? */
00437                 //vd->formatIn = vd->fmt.fmt.pix.pixelformat;
00438         }
00439         else {
00440                 printf("  Frame size:   %dx%d\n", vd->width, vd->height);
00441         }
00442 
00443         /* set framerate */
00444         struct v4l2_streamparm* setfps;  
00445         setfps=(struct v4l2_streamparm *) calloc(1, sizeof(struct v4l2_streamparm));
00446         memset(setfps, 0, sizeof(struct v4l2_streamparm));
00447         setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00448 
00449         // Convert the frame rate into a fraction for V4L2
00450         int n = 0, d = 0;
00451         float_to_fraction(vd->fps, &n, &d);
00452         setfps->parm.capture.timeperframe.numerator = d;
00453         setfps->parm.capture.timeperframe.denominator = n;
00454 
00455         ret = v4l2_ioctl(vd->fd, VIDIOC_S_PARM, setfps);
00456         if(ret == -1) {
00457                 perror("Unable to set frame rate");
00458                 goto fatal;
00459         }
00460         ret = v4l2_ioctl(vd->fd, VIDIOC_G_PARM, setfps);
00461         if(ret == 0) {
00462                 float confirmed_fps = (float)setfps->parm.capture.timeperframe.denominator / (float)setfps->parm.capture.timeperframe.numerator;
00463                 if (confirmed_fps != (float)n / (float)d) {
00464                         printf("  Frame rate:   %g fps (requested frame rate %g fps is "
00465                                 "not supported by device)\n",
00466                                 confirmed_fps,
00467                                 vd->fps);
00468                         vd->fps = confirmed_fps;
00469                 }
00470                 else {
00471                         printf("  Frame rate:   %g fps\n", vd->fps);
00472                 }
00473         }
00474         else {
00475                 perror("Unable to read out current frame rate");
00476                 goto fatal;
00477         }
00478 
00479         /* request buffers */
00480         memset(&vd->rb, 0, sizeof(struct v4l2_requestbuffers));
00481         vd->rb.count = NB_BUFFER;
00482         vd->rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00483         vd->rb.memory = V4L2_MEMORY_MMAP;
00484 
00485         ret = v4l2_ioctl(vd->fd, VIDIOC_REQBUFS, &vd->rb);
00486         if (ret < 0) {
00487                 perror("Unable to allocate buffers");
00488                 goto fatal;
00489         }
00490         /* map the buffers */
00491         for (i = 0; i < NB_BUFFER; i++) {
00492                 memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
00493                 vd->buf.index = i;
00494                 vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00495                 vd->buf.memory = V4L2_MEMORY_MMAP;
00496                 ret = v4l2_ioctl(vd->fd, VIDIOC_QUERYBUF, &vd->buf);
00497                 if (ret < 0) {
00498                         perror("Unable to query buffer");
00499                         goto fatal;
00500                 }
00501                 if (debug)
00502                         printf("length: %u offset: %u\n", vd->buf.length,
00503                                         vd->buf.m.offset);
00504                 vd->mem[i] = v4l2_mmap(0 /* start anywhere */ ,
00505                                 vd->buf.length, PROT_READ, MAP_SHARED, vd->fd,
00506                                 vd->buf.m.offset);
00507                 if (vd->mem[i] == MAP_FAILED) {
00508                         perror("Unable to map buffer");
00509                         goto fatal;
00510                 }
00511                 if (debug)
00512                         printf("Buffer mapped at address %p.\n", vd->mem[i]);
00513         }
00514         /* Queue the buffers. */
00515         for (i = 0; i < NB_BUFFER; ++i) {
00516                 memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
00517                 vd->buf.index = i;
00518                 vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00519                 vd->buf.memory = V4L2_MEMORY_MMAP;
00520                 ret = v4l2_ioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
00521                 if (ret < 0) {
00522                         perror("Unable to queue buffer");
00523                         goto fatal;;
00524                 }
00525         }
00526         return 0;
00527 fatal:
00528         return -1;
00529 
00530 }
00531 
00532 static int video_enable(struct vdIn *vd)
00533 {
00534     int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00535     int ret;
00536 
00537     ret = v4l2_ioctl(vd->fd, VIDIOC_STREAMON, &type);
00538     if (ret < 0) {
00539         perror("Unable to start capture");
00540         return ret;
00541     }
00542     vd->isstreaming = 1;
00543     return 0;
00544 }
00545 
00546 static int video_disable(struct vdIn *vd)
00547 {
00548     int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00549     int ret;
00550 
00551     ret = v4l2_ioctl(vd->fd, VIDIOC_STREAMOFF, &type);
00552     if (ret < 0) {
00553         perror("Unable to stop capture");
00554         return ret;
00555     }
00556     vd->isstreaming = 0;
00557     return 0;
00558 }
00559 
00560 
00561 int uvcGrab(struct vdIn *vd)
00562 {
00563 #define HEADERFRAME1 0xaf
00564     int ret;
00565 
00566     if (!vd->isstreaming)
00567         if (video_enable(vd))
00568             goto err;
00569     memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
00570     vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00571     vd->buf.memory = V4L2_MEMORY_MMAP;
00572     ret = v4l2_ioctl(vd->fd, VIDIOC_DQBUF, &vd->buf);
00573     if (ret < 0) {
00574         perror("Unable to dequeue buffer");
00575         goto err;
00576     }
00577 
00578         /* Capture a single raw frame */
00579         if (vd->rawFrameCapture && vd->buf.bytesused > 0) {
00580                 FILE *frame = NULL;
00581                 char filename[13];
00582                 int ret;
00583 
00584                 /* Disable frame capturing unless we're in frame stream mode */
00585                 if(vd->rawFrameCapture == 1)
00586                         vd->rawFrameCapture = 0;
00587 
00588                 /* Create a file name and open the file */
00589                 sprintf(filename, "frame%03u.raw", vd->fileCounter++ % 1000);
00590                 frame = fopen(filename, "wb");
00591                 if(frame == NULL) {
00592                         perror("Unable to open file for raw frame capturing");
00593                         goto end_capture;
00594                 }
00595                 
00596                 /* Write the raw data to the file */
00597                 ret = fwrite(vd->mem[vd->buf.index], vd->buf.bytesused, 1, frame);
00598                 if(ret < 1) {
00599                         perror("Unable to write to file");
00600                         goto end_capture;
00601                 }
00602                 printf("Saved raw frame to %s (%u bytes)\n", filename, vd->buf.bytesused);
00603                 if(vd->rawFrameCapture == 2) {
00604                         vd->rfsBytesWritten += vd->buf.bytesused;
00605                         vd->rfsFramesWritten++;
00606                 }
00607 
00608 
00609                 /* Clean up */
00610 end_capture:
00611                 if(frame)
00612                         fclose(frame);
00613         }
00614 
00615    
00616 
00617         /* Capture raw stream data */
00618         if (vd->captureFile && vd->buf.bytesused > 0) {
00619                 int ret;
00620                 ret = fwrite(vd->mem[vd->buf.index], vd->buf.bytesused, 1, vd->captureFile);
00621                 if (ret < 1) {
00622                         perror("Unable to write raw stream to file");
00623                         fprintf(stderr, "Stream capturing terminated.\n");
00624                         fclose(vd->captureFile);
00625                         vd->captureFile = NULL;
00626                         vd->framesWritten = 0;
00627                         vd->bytesWritten = 0;
00628                 } else {
00629                         vd->framesWritten++;
00630                         vd->bytesWritten += vd->buf.bytesused;
00631                         if (debug)
00632                                 printf("Appended raw frame to stream file (%u bytes)\n", vd->buf.bytesused);
00633                 }
00634         }
00635 
00636     switch (vd->formatIn) {
00637     case V4L2_PIX_FMT_MJPEG:
00638         if(vd->buf.bytesused <= HEADERFRAME1) { /* Prevent crash on empty image */
00639 /*          if(debug)*/
00640                 printf("Ignoring empty buffer ...\n");
00641             return 0;
00642         }
00643         memcpy(vd->tmpbuffer, vd->mem[vd->buf.index],vd->buf.bytesused);
00644         if (jpeg_decode(&vd->framebuffer, vd->tmpbuffer, &vd->width,
00645              &vd->height) < 0) {
00646             printf("jpeg decode errors\n");
00647             goto err;
00648         }
00649         if (debug)
00650             printf("bytes in used %d\n", vd->buf.bytesused);
00651         break;
00652     case V4L2_PIX_FMT_YUYV:
00653     case V4L2_PIX_FMT_UYVY:
00654         if (vd->buf.bytesused > vd->framesizeIn)
00655             memcpy(vd->framebuffer, vd->mem[vd->buf.index],
00656                    (size_t) vd->framesizeIn);
00657         else
00658             memcpy(vd->framebuffer, vd->mem[vd->buf.index],
00659                    (size_t) vd->buf.bytesused);
00660         break;
00661     default:
00662         goto err;
00663         break;
00664     }
00665     ret = v4l2_ioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
00666     if (ret < 0) {
00667         perror("Unable to requeue buffer");
00668         goto err;
00669     }
00670 
00671     return 0;
00672   err:
00673     vd->signalquit = 0;
00674     return -1;
00675 }
00676 int close_v4l2(struct vdIn *vd)
00677 {
00678     if (vd->isstreaming)
00679         video_disable(vd);
00680     if (vd->tmpbuffer)
00681         free(vd->tmpbuffer);
00682     vd->tmpbuffer = NULL;
00683     free(vd->framebuffer);
00684     vd->framebuffer = NULL;
00685     free(vd->videodevice);
00686     free(vd->status);
00687     free(vd->pictName);
00688     vd->videodevice = NULL;
00689     vd->status = NULL;
00690     vd->pictName = NULL;
00691 }
00692 
00693 /* return >= 0 ok otherwhise -1 */
00694 static int isv4l2Control(struct vdIn *vd, int control,
00695                          struct v4l2_queryctrl *queryctrl)
00696 {
00697 int err =0;
00698     queryctrl->id = control;
00699     if ((err= v4l2_ioctl(vd->fd, VIDIOC_QUERYCTRL, queryctrl)) < 0) {
00700         perror("ioctl querycontrol error");
00701     } else if (queryctrl->flags & V4L2_CTRL_FLAG_DISABLED) {
00702         printf("control %s disabled\n", (char *) queryctrl->name);
00703     } else if (queryctrl->flags & V4L2_CTRL_TYPE_BOOLEAN) {
00704         return 1;
00705     } else if (queryctrl->type & V4L2_CTRL_TYPE_INTEGER) {
00706         return 0;
00707     } else {
00708         printf("contol %s unsupported\n", (char *) queryctrl->name);
00709     }
00710     return -1;
00711 }
00712 
00713 int v4l2GetControl(struct vdIn *vd, int control)
00714 {
00715     struct v4l2_queryctrl queryctrl;
00716     struct v4l2_control control_s;
00717     int err;
00718     if (isv4l2Control(vd, control, &queryctrl) < 0)
00719         return -1;
00720     control_s.id = control;
00721     if ((err = v4l2_ioctl(vd->fd, VIDIOC_G_CTRL, &control_s)) < 0) {
00722         printf("ioctl get control error\n");
00723         return -1;
00724     }
00725     return control_s.value;
00726 }
00727 
00728 int v4l2SetControl(struct vdIn *vd, int control, int value)
00729 {
00730     struct v4l2_control control_s;
00731     struct v4l2_queryctrl queryctrl;
00732     int min, max, step, val_def;
00733     int err;
00734     if (isv4l2Control(vd, control, &queryctrl) < 0)
00735         return -1;
00736     min = queryctrl.minimum;
00737     max = queryctrl.maximum;
00738     step = queryctrl.step;
00739     val_def = queryctrl.default_value;
00740     if ((value >= min) && (value <= max)) {
00741         control_s.id = control;
00742         control_s.value = value;
00743         if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00744             printf("ioctl set control error\n");
00745             return -1;
00746         }
00747     }
00748     return 0;
00749 }
00750 int v4l2UpControl(struct vdIn *vd, int control)
00751 {
00752     struct v4l2_control control_s;
00753     struct v4l2_queryctrl queryctrl;
00754     int min, max, current, step, val_def;
00755     int err;
00756 
00757     if (isv4l2Control(vd, control, &queryctrl) < 0)
00758         return -1;
00759     min = queryctrl.minimum;
00760     max = queryctrl.maximum;
00761     step = queryctrl.step;
00762     val_def = queryctrl.default_value;
00763     current = v4l2GetControl(vd, control);
00764     current += step;
00765     printf("max %d, min %d, step %d, default %d ,current %d\n",max,min,step,val_def,current);
00766     if (current <= max) {
00767         control_s.id = control;
00768         control_s.value = current;
00769         if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00770             printf("ioctl set control error\n");
00771             return -1;
00772         }            
00773         printf ("Control name:%s set to value:%d\n", queryctrl.name, control_s.value);
00774     } else {
00775       printf ("Control name:%s already has max value:%d\n", queryctrl.name, max); 
00776     }
00777      return control_s.value;
00778 }
00779 int v4l2DownControl(struct vdIn *vd, int control)
00780 {
00781     struct v4l2_control control_s;
00782     struct v4l2_queryctrl queryctrl;
00783     int min, max, current, step, val_def;
00784     int err;
00785     if (isv4l2Control(vd, control, &queryctrl) < 0)
00786         return -1;
00787     min = queryctrl.minimum;
00788     max = queryctrl.maximum;
00789     step = queryctrl.step;
00790     val_def = queryctrl.default_value;
00791     current = v4l2GetControl(vd, control);
00792     current -= step;
00793     printf("max %d, min %d, step %d, default %d ,current %d\n",max,min,step,val_def,current);
00794     if (current >= min) {
00795         control_s.id = control;
00796         control_s.value = current;
00797         if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00798             printf("ioctl set control error\n");
00799             return -1;
00800         }
00801     printf ("Control name:%s set to value:%d\n", queryctrl.name, control_s.value);
00802     }
00803     else {
00804       printf ("Control name:%s already has min value:%d\n", queryctrl.name, min); 
00805     }
00806     return control_s.value;
00807 }
00808 int v4l2ToggleControl(struct vdIn *vd, int control)
00809 {
00810     struct v4l2_control control_s;
00811     struct v4l2_queryctrl queryctrl;
00812     int current;
00813     int err;
00814     if (isv4l2Control(vd, control, &queryctrl) != 1)
00815         return -1;
00816     current = v4l2GetControl(vd, control);
00817     control_s.id = control;
00818     control_s.value = !current;
00819     if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00820         printf("ioctl toggle control error\n");
00821         return -1;
00822     }
00823     return control_s.value;
00824 }
00825 int v4l2ResetControl(struct vdIn *vd, int control)
00826 {
00827     struct v4l2_control control_s;
00828     struct v4l2_queryctrl queryctrl;
00829     int val_def;
00830     int err;
00831     if (isv4l2Control(vd, control, &queryctrl) < 0)
00832         return -1;
00833     val_def = queryctrl.default_value;
00834     control_s.id = control;
00835     control_s.value = val_def;
00836     if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00837         printf("ioctl reset control error\n");
00838         return -1;
00839     }
00840 
00841     return 0;
00842 }
00843 
00844 int v4l2ResetPan(struct vdIn *vd)
00845 {
00846         int control = V4L2_CID_PAN_RESET;
00847         struct v4l2_control control_s;
00848         struct v4l2_queryctrl queryctrl;
00849         int err;
00850 
00851         if (isv4l2Control(vd, control, &queryctrl) < 0)
00852                 return -1;
00853 
00854         control_s.id = control;
00855         control_s.value = 1;
00856         if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00857                 printf("ERROR: Unable to reset pan (error = %d)\n", errno);
00858                 return -1;
00859         }
00860 
00861         return 0;
00862 }
00863 
00864 int v4l2ResetTilt(struct vdIn *vd)
00865 {
00866         int control = V4L2_CID_TILT_RESET;
00867         struct v4l2_control control_s;
00868         struct v4l2_queryctrl queryctrl;
00869         int err;
00870 
00871         if (isv4l2Control(vd, control, &queryctrl) < 0)
00872                 return -1;
00873 
00874         control_s.id = control;
00875         control_s.value = 1;
00876         if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00877                 printf("ERROR: Unable to reset tilt (error = %d)\n", errno);
00878                 return -1;
00879         }
00880 
00881         return 0;
00882 }
00883 
00884 int v4l2ResetPanTilt(struct vdIn *vd)
00885 {
00886         int control = V4L2_CID_PANTILT_RESET;
00887         struct v4l2_control control_s;
00888         struct v4l2_queryctrl queryctrl;
00889         unsigned char val;
00890         int err;
00891 
00892         if (isv4l2Control(vd, control, &queryctrl) < 0)
00893                 return -1;
00894 
00895         control_s.id = control;
00896         control_s.value = 3;
00897         if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00898                 printf("ERROR: Unable to reset pan/tilt (error = %d)\n", errno);
00899                 return -1;
00900         }
00901 
00902         return 0;
00903 }
00904 
00905 int v4L2UpDownPan(struct vdIn *vd, short inc)
00906 {
00907         int control = V4L2_CID_PAN_RELATIVE;
00908         struct v4l2_control control_s;
00909         struct v4l2_queryctrl queryctrl;
00910         int err;
00911 
00912         if (isv4l2Control(vd, control, &queryctrl) < 0)
00913                 return -1;
00914         control_s.id = control;
00915         control_s.value = inc;
00916         if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00917                 printf("ioctl pan updown control error\n");
00918                 return -1;
00919         }
00920         return 0;
00921 }
00922 
00923 int v4L2UpDownTilt(struct vdIn *vd, short inc)
00924 {   int control = V4L2_CID_TILT_RELATIVE;
00925     struct v4l2_control control_s;
00926     struct v4l2_queryctrl queryctrl;
00927     int err;
00928 
00929     if (isv4l2Control(vd, control, &queryctrl) < 0)
00930         return -1;  
00931     control_s.id = control;
00932     control_s.value = inc;
00933     if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00934         printf("ioctl tiltupdown control error\n");
00935         return -1;
00936     }
00937     return 0;
00938 }
00939 
00940 int v4L2UpDownPanTilt(struct vdIn *vd, short inc_p, short inc_t) {
00941   int p_control = V4L2_CID_PAN_RELATIVE;
00942   int t_control = V4L2_CID_TILT_RELATIVE;
00943   struct v4l2_ext_controls control_s_array;
00944   struct v4l2_queryctrl queryctrl;
00945   struct v4l2_ext_control control_s[2];
00946   int err;
00947 
00948   if(isv4l2Control(vd, p_control, &queryctrl) < 0 ||
00949      isv4l2Control(vd, t_control, &queryctrl) < 0)
00950     return -1;
00951   control_s_array.count = 2;
00952   control_s_array.ctrl_class = V4L2_CTRL_CLASS_USER;
00953   control_s_array.reserved[0] = 0;
00954   control_s_array.reserved[1] = 0;
00955   control_s_array.controls = control_s;
00956 
00957   control_s[0].id = p_control;
00958   control_s[0].value = inc_p;
00959   control_s[1].id = t_control;
00960   control_s[1].value = inc_t;
00961 
00962   if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_EXT_CTRLS, &control_s_array)) < 0) {
00963     printf("ioctl pan-tilt updown control error\n");
00964     return -1;
00965   }
00966   return 0;
00967 }
00968 
00969 #if 0
00970 
00971 union pantilt {
00972         struct {
00973                 short pan;
00974                 short tilt;
00975         } s16;
00976         int value;
00977 } __attribute__((packed)) ;
00978         
00979 int v4L2UpDownPan(struct vdIn *vd, short inc)
00980 {   int control = V4L2_CID_PANTILT_RELATIVE;
00981     struct v4l2_control control_s;
00982     struct v4l2_queryctrl queryctrl;
00983     int err;
00984     
00985    union pantilt pan;
00986    
00987        control_s.id = control;
00988      if (isv4l2Control(vd, control, &queryctrl) < 0)
00989         return -1;
00990 
00991   pan.s16.pan = inc;
00992   pan.s16.tilt = 0;
00993  
00994         control_s.value = pan.value ;
00995     if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00996         printf("ioctl pan updown control error\n");
00997         return -1;
00998         }
00999         return 0;
01000 }
01001 
01002 int v4L2UpDownTilt(struct vdIn *vd, short inc)
01003 {   int control = V4L2_CID_PANTILT_RELATIVE;
01004     struct v4l2_control control_s;
01005     struct v4l2_queryctrl queryctrl;
01006     int err;
01007      union pantilt pan;  
01008        control_s.id = control;
01009      if (isv4l2Control(vd, control, &queryctrl) < 0)
01010         return -1;  
01011 
01012     pan.s16.pan= 0;
01013     pan.s16.tilt = inc;
01014   
01015         control_s.value = pan.value;
01016     if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
01017         printf("ioctl tiltupdown control error\n");
01018         return -1;
01019         }
01020         return 0;
01021 }
01022 #endif
01023 
01024 int v4l2SetLightFrequencyFilter(struct vdIn *vd, int flt) 
01025 {   int control = V4L2_CID_POWER_LINE_FREQUENCY;
01026     struct v4l2_control control_s;
01027     struct v4l2_queryctrl queryctrl;
01028     int err;
01029        control_s.id = control;
01030      if (isv4l2Control(vd, control, &queryctrl) < 0)
01031         return -1;  
01032 
01033        control_s.value = flt;
01034 
01035     if ((err = v4l2_ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
01036         printf("ioctl set_light_frequency_filter error\n");
01037         return -1;
01038         }
01039         return 0;
01040 }
01041 int enum_frame_intervals(int dev, __u32 pixfmt, __u32 width, __u32 height)
01042 {
01043         int ret;
01044         struct v4l2_frmivalenum fival;
01045 
01046         memset(&fival, 0, sizeof(fival));
01047         fival.index = 0;
01048         fival.pixel_format = pixfmt;
01049         fival.width = width;
01050         fival.height = height;
01051         printf("\tTime interval between frame: ");
01052         while ((ret = v4l2_ioctl(dev, VIDIOC_ENUM_FRAMEINTERVALS, &fival)) == 0) {
01053                 if (fival.type == V4L2_FRMIVAL_TYPE_DISCRETE) {
01054                                 printf("%u/%u, ",
01055                                                 fival.discrete.numerator, fival.discrete.denominator);
01056                 } else if (fival.type == V4L2_FRMIVAL_TYPE_CONTINUOUS) {
01057                                 printf("{min { %u/%u } .. max { %u/%u } }, ",
01058                                                 fival.stepwise.min.numerator, fival.stepwise.min.numerator,
01059                                                 fival.stepwise.max.denominator, fival.stepwise.max.denominator);
01060                                 break;
01061                 } else if (fival.type == V4L2_FRMIVAL_TYPE_STEPWISE) {
01062                                 printf("{min { %u/%u } .. max { %u/%u } / "
01063                                                 "stepsize { %u/%u } }, ",
01064                                                 fival.stepwise.min.numerator, fival.stepwise.min.denominator,
01065                                                 fival.stepwise.max.numerator, fival.stepwise.max.denominator,
01066                                                 fival.stepwise.step.numerator, fival.stepwise.step.denominator);
01067                                 break;
01068                 }
01069                 fival.index++;
01070         }
01071         printf("\n");
01072         if (ret != 0 && errno != EINVAL) {
01073                 perror("ERROR enumerating frame intervals");
01074                 return errno;
01075         }
01076 
01077         return 0;
01078 }
01079 int enum_frame_sizes(int dev, __u32 pixfmt)
01080 {
01081         int ret;
01082         struct v4l2_frmsizeenum fsize;
01083 
01084         memset(&fsize, 0, sizeof(fsize));
01085         fsize.index = 0;
01086         fsize.pixel_format = pixfmt;
01087         while ((ret = v4l2_ioctl(dev, VIDIOC_ENUM_FRAMESIZES, &fsize)) == 0) {
01088                 if (fsize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
01089                         printf("{ discrete: width = %u, height = %u }\n",
01090                                         fsize.discrete.width, fsize.discrete.height);
01091                         ret = enum_frame_intervals(dev, pixfmt,
01092                                         fsize.discrete.width, fsize.discrete.height);
01093                         if (ret != 0)
01094                                 printf("  Unable to enumerate frame sizes.\n");
01095                 } else if (fsize.type == V4L2_FRMSIZE_TYPE_CONTINUOUS) {
01096                         printf("{ continuous: min { width = %u, height = %u } .. "
01097                                         "max { width = %u, height = %u } }\n",
01098                                         fsize.stepwise.min_width, fsize.stepwise.min_height,
01099                                         fsize.stepwise.max_width, fsize.stepwise.max_height);
01100                         printf("  Refusing to enumerate frame intervals.\n");
01101                         break;
01102                 } else if (fsize.type == V4L2_FRMSIZE_TYPE_STEPWISE) {
01103                         printf("{ stepwise: min { width = %u, height = %u } .. "
01104                                         "max { width = %u, height = %u } / "
01105                                         "stepsize { width = %u, height = %u } }\n",
01106                                         fsize.stepwise.min_width, fsize.stepwise.min_height,
01107                                         fsize.stepwise.max_width, fsize.stepwise.max_height,
01108                                         fsize.stepwise.step_width, fsize.stepwise.step_height);
01109                         printf("  Refusing to enumerate frame intervals.\n");
01110                         break;
01111                 }
01112                 fsize.index++;
01113         }
01114         if (ret != 0 && errno != EINVAL) {
01115                 perror("ERROR enumerating frame sizes");
01116                 return errno;
01117         }
01118 
01119         return 0;
01120 }
01121 
01122 int enum_frame_formats(int dev, unsigned int *supported_formats, unsigned int max_formats)
01123 {
01124         int ret;
01125         struct v4l2_fmtdesc fmt;
01126 
01127         memset(&fmt, 0, sizeof(fmt));
01128         fmt.index = 0;
01129         fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
01130         while ((ret = v4l2_ioctl(dev, VIDIOC_ENUM_FMT, &fmt)) == 0) {
01131                 if(supported_formats == NULL) {
01132                         printf("{ pixelformat = '%c%c%c%c', description = '%s' }\n",
01133                                         fmt.pixelformat & 0xFF, (fmt.pixelformat >> 8) & 0xFF,
01134                                         (fmt.pixelformat >> 16) & 0xFF, (fmt.pixelformat >> 24) & 0xFF,
01135                                         fmt.description);
01136                         ret = enum_frame_sizes(dev, fmt.pixelformat);
01137                         if(ret != 0)
01138                                 printf("  Unable to enumerate frame sizes.\n");
01139                 }
01140                 else if(fmt.index < max_formats) {
01141                         supported_formats[fmt.index] = fmt.pixelformat;
01142                 }
01143 
01144                 fmt.index++;
01145         }
01146         if (errno != EINVAL) {
01147                 perror("ERROR enumerating frame formats");
01148                 return errno;
01149         }
01150 
01151         return 0;
01152 }


v4r_uvc
Author(s):
autogenerated on Wed Aug 26 2015 16:41:35