uvc_cam.cpp
Go to the documentation of this file.
00001 /* This file is from the uvc_cam package by Morgan Quigley */
00002 #include <cstring>
00003 #include <string>
00004 #include <cstdio>
00005 #include <stdexcept>
00006 #include <dirent.h>
00007 #include <sys/stat.h>
00008 #include <sys/ioctl.h>
00009 #include <sys/mman.h>
00010 #include <fcntl.h>
00011 #include <errno.h>
00012 #include "uvc_cam/uvc_cam.h"
00013 
00014 using std::string;
00015 using namespace uvc_cam;
00016 
00017 Cam::Cam(const char *_device, mode_t _mode, int _width, int _height, int _fps)
00018 : mode(_mode), device(_device),
00019   motion_threshold_luminance(100), motion_threshold_count(-1),
00020   width(_width), height(_height), fps(_fps), rgb_frame(NULL)
00021 {
00022   printf("opening %s\n", _device);
00023   if ((fd = open(_device, O_RDWR)) == -1)
00024     throw std::runtime_error("couldn't open " + device);
00025   memset(&fmt, 0, sizeof(v4l2_format));
00026   memset(&cap, 0, sizeof(v4l2_capability));
00027   if (ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0)
00028     throw std::runtime_error("couldn't query " + device);
00029   if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
00030     throw std::runtime_error(device + " does not support capture");
00031   if (!(cap.capabilities & V4L2_CAP_STREAMING))
00032     throw std::runtime_error(device + " does not support streaming");
00033   // enumerate formats
00034   v4l2_fmtdesc f;
00035   memset(&f, 0, sizeof(f));
00036   f.index = 0;
00037   f.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00038   int ret;
00039   while ((ret = ioctl(fd, VIDIOC_ENUM_FMT, &f)) == 0)
00040   {
00041     printf("pixfmt %d = '%4s' desc = '%s'\n",
00042            f.index++, (char *)&f.pixelformat, f.description);
00043     // enumerate frame sizes
00044     v4l2_frmsizeenum fsize;
00045     fsize.index = 0;
00046     fsize.pixel_format = f.pixelformat;
00047     while ((ret = ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &fsize)) == 0)
00048     {
00049       fsize.index++;
00050       if (fsize.type == V4L2_FRMSIZE_TYPE_DISCRETE)
00051       {
00052         printf("  discrete: %ux%u:   ",
00053                fsize.discrete.width, fsize.discrete.height);
00054         // enumerate frame rates
00055         v4l2_frmivalenum fival;
00056         fival.index = 0;
00057         fival.pixel_format = f.pixelformat;
00058         fival.width = fsize.discrete.width;
00059         fival.height = fsize.discrete.height;
00060         while ((ret = ioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &fival)) == 0)
00061         {
00062           fival.index++;
00063           if (fival.type == V4L2_FRMIVAL_TYPE_DISCRETE)
00064           {
00065             printf("%u/%u ",
00066                    fival.discrete.numerator, fival.discrete.denominator);
00067           }
00068           else
00069             printf("I only handle discrete frame intervals...\n");
00070         }
00071         printf("\n");
00072       }
00073       else if (fsize.type == V4L2_FRMSIZE_TYPE_CONTINUOUS)
00074       {
00075         printf("  continuous: %ux%u to %ux%u\n",
00076                fsize.stepwise.min_width, fsize.stepwise.min_height,
00077                fsize.stepwise.max_width, fsize.stepwise.max_height);
00078       }
00079       else if (fsize.type == V4L2_FRMSIZE_TYPE_STEPWISE)
00080       {
00081         printf("  stepwise: %ux%u to %ux%u step %ux%u\n",
00082                fsize.stepwise.min_width,  fsize.stepwise.min_height,
00083                fsize.stepwise.max_width,  fsize.stepwise.max_height,
00084                fsize.stepwise.step_width, fsize.stepwise.step_height);
00085       }
00086       else
00087       {
00088         printf("  fsize.type not supported: %d\n", fsize.type);
00089       }
00090     }
00091   }
00092   if (errno != EINVAL)
00093     throw std::runtime_error("error enumerating frame formats");
00094   fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00095   fmt.fmt.pix.width = width;
00096   fmt.fmt.pix.height = height;
00097   if (mode == MODE_RGB || mode == MODE_YUYV) // we'll convert later
00098     fmt.fmt.pix.pixelformat = 'Y' | ('U' << 8) | ('Y' << 16) | ('V' << 24);
00099   else
00100     fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
00101   fmt.fmt.pix.field = V4L2_FIELD_ANY;
00102   if ((ret = ioctl(fd, VIDIOC_S_FMT, &fmt)) < 0)
00103     throw std::runtime_error("couldn't set format");
00104   if (fmt.fmt.pix.width != width || fmt.fmt.pix.height != height)
00105     throw std::runtime_error("pixel format unavailable");
00106   streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00107   streamparm.parm.capture.timeperframe.numerator = 1;
00108   streamparm.parm.capture.timeperframe.denominator = fps;
00109   if ((ret = ioctl(fd, VIDIOC_S_PARM, &streamparm)) < 0)
00110     throw std::runtime_error("unable to set framerate");
00111   v4l2_queryctrl queryctrl;
00112   memset(&queryctrl, 0, sizeof(queryctrl));
00113   uint32_t i = V4L2_CID_BASE;
00114   while (i != V4L2_CID_LAST_EXTCTR)
00115   {
00116     queryctrl.id = i;
00117     if ((ret = ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl)) == 0 &&
00118         !(queryctrl.flags & V4L2_CTRL_FLAG_DISABLED))
00119     {
00120       const char *ctrl_type = NULL;
00121       if (queryctrl.type == V4L2_CTRL_TYPE_INTEGER)
00122         ctrl_type = "int";
00123       else if (queryctrl.type == V4L2_CTRL_TYPE_BOOLEAN)
00124         ctrl_type = "bool";
00125       else if (queryctrl.type == V4L2_CTRL_TYPE_BUTTON)
00126         ctrl_type = "button";
00127       else if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
00128         ctrl_type = "menu";
00129       printf("  %s (%s, %d, id = %x): %d to %d (%d)\n",
00130              ctrl_type,
00131              queryctrl.name, queryctrl.flags, queryctrl.id,
00132              queryctrl.minimum, queryctrl.maximum, queryctrl.step);
00133       if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
00134       {
00135         v4l2_querymenu querymenu;
00136         memset(&querymenu, 0, sizeof(querymenu));
00137         querymenu.id = queryctrl.id;
00138         querymenu.index = 0;
00139         while (ioctl(fd, VIDIOC_QUERYMENU, &querymenu) == 0)
00140         {
00141           printf("    %d: %s\n", querymenu.index, querymenu.name);
00142           querymenu.index++;
00143         }
00144       }
00145 
00146     }
00147     else if (errno != EINVAL)
00148       throw std::runtime_error("couldn't query control");
00149     i++;
00150     if (i == V4L2_CID_LAST_NEW)
00151       i = V4L2_CID_CAMERA_CLASS_BASE_NEW;
00152     else if (i == V4L2_CID_CAMERA_CLASS_LAST)
00153       i = V4L2_CID_PRIVATE_BASE_OLD;
00154     else if (i == V4L2_CID_PRIVATE_LAST)
00155       i = V4L2_CID_BASE_EXTCTR;
00156   }
00157 
00158   try
00159   {
00160     // the commented labels correspond to the controls in guvcview and uvcdynctrl
00161 
00162     //set_control(V4L2_CID_EXPOSURE_AUTO_NEW, 2);
00163     set_control(10094851, 1); // Exposure, Auto Priority
00164     set_control(10094849, 1); // Exposure, Auto
00165     //set_control(168062321, 0); //Disable video processing
00166     //set_control(0x9a9010, 100);
00167     //set_control(V4L2_CID_EXPOSURE_ABSOLUTE_NEW, 300);
00168     //set_control(V4L2_CID_BRIGHTNESS, 140);
00169     //set_control(V4L2_CID_CONTRAST, 40);
00170     //set_control(V4L2_CID_WHITE_BALANCE_TEMP_AUTO_OLD, 0);
00171     set_control(9963776, 128); //Brightness
00172     set_control(9963777, 32); //Contrast
00173     set_control(9963788, 1); // White Balance Temperature, Auto
00174     set_control(9963802, 5984); // White Balance Temperature
00175     set_control(9963800, 2);  // power line frequency to 60 hz
00176     set_control(9963795, 200); // Gain
00177     set_control(9963803, 224); // Sharpness
00178     set_control(9963804, 1); //Backlight Compensation
00179     set_control(10094850, 250); // Exposure (Absolute)
00180     set_control(168062212, 16); //Focus (absolute)
00181     set_control(168062213, 3); //LED1 Mode
00182     set_control(168062214, 0); //LED1 Frequency
00183     set_control(9963778, 32); // Saturation
00184   }
00185   catch (std::runtime_error &ex)
00186   {
00187     printf("ERROR: could not set some settings.  \n %s \n", ex.what());
00188   }
00189 
00190 /*
00191   v4l2_jpegcompression v4l2_jpeg;
00192   if (ioctl(fd, VIDIOC_G_JPEGCOMP, &v4l2_jpeg) < 0)
00193     throw std::runtime_error("no jpeg compression iface exposed");
00194   printf("jpeg quality: %d\n", v4l2_jpeg.quality);
00195 */
00196 
00197   memset(&rb, 0, sizeof(rb));
00198   rb.count = NUM_BUFFER;
00199   rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00200   rb.memory = V4L2_MEMORY_MMAP;
00201   if (ioctl(fd, VIDIOC_REQBUFS, &rb) < 0)
00202     throw std::runtime_error("unable to allocate buffers");
00203   for (unsigned i = 0; i < NUM_BUFFER; i++)
00204   {
00205     memset(&buf, 0, sizeof(buf));
00206     buf.index = i;
00207     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00208     buf.flags = V4L2_BUF_FLAG_TIMECODE;
00209     buf.timecode = timecode;
00210     buf.timestamp.tv_sec = 0;
00211     buf.timestamp.tv_usec = 0;
00212     buf.memory = V4L2_MEMORY_MMAP;
00213     if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0)
00214       throw std::runtime_error("unable to query buffer");
00215     if (buf.length <= 0)
00216       throw std::runtime_error("buffer length is bogus");
00217     mem[i] = mmap(0, buf.length, PROT_READ, MAP_SHARED, fd, buf.m.offset);
00218     //printf("buf length = %d at %x\n", buf.length, mem[i]);
00219     if (mem[i] == MAP_FAILED)
00220       throw std::runtime_error("couldn't map buffer");
00221   }
00222   buf_length = buf.length;
00223   for (unsigned i = 0; i < NUM_BUFFER; i++)
00224   {
00225     memset(&buf, 0, sizeof(buf));
00226     buf.index = i;
00227     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00228     buf.flags = V4L2_BUF_FLAG_TIMECODE;
00229     buf.timecode = timecode;
00230     buf.timestamp.tv_sec = 0;
00231     buf.timestamp.tv_usec = 0;
00232     buf.memory = V4L2_MEMORY_MMAP;
00233     if (ioctl(fd, VIDIOC_QBUF, &buf) < 0)
00234       throw std::runtime_error("unable to queue buffer");
00235   }
00236   int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00237   if (ioctl(fd, VIDIOC_STREAMON, &type) < 0)
00238     throw std::runtime_error("unable to start capture");
00239   rgb_frame = new unsigned char[width * height * 3];
00240   last_yuv_frame = new unsigned char[width * height * 2];
00241 }
00242 
00243 Cam::~Cam()
00244 {
00245   // stop stream
00246   int type = V4L2_BUF_TYPE_VIDEO_CAPTURE, ret;
00247   if ((ret = ioctl(fd, VIDIOC_STREAMOFF, &type)) < 0)
00248     perror("VIDIOC_STREAMOFF");
00249   for (unsigned i = 0; i < NUM_BUFFER; i++)
00250     if (munmap(mem[i], buf_length) < 0)
00251       perror("failed to unmap buffer");
00252   close(fd);
00253   if (rgb_frame)
00254   {
00255     delete[] rgb_frame;
00256     delete[] last_yuv_frame;
00257   }
00258   last_yuv_frame = rgb_frame = NULL;
00259 }
00260 
00261 void Cam::enumerate()
00262 {
00263   string v4l_path = "/sys/class/video4linux";
00264   DIR *d = opendir(v4l_path.c_str());
00265   if (!d)
00266     throw std::runtime_error("couldn't open " + v4l_path);
00267   struct dirent *ent, *ent2, *ent3;
00268   int fd, ret;
00269   struct v4l2_capability v4l2_cap;
00270   while ((ent = readdir(d)) != NULL)
00271   {
00272     if (strncmp(ent->d_name, "video", 5))
00273       continue; // ignore anything not starting with "video"
00274     string dev_name = string("/dev/") + string(ent->d_name);
00275     printf("enumerating %s ...\n", dev_name.c_str());
00276     if ((fd = open(dev_name.c_str(), O_RDWR)) == -1)
00277       throw std::runtime_error("couldn't open " + dev_name + "  perhaps the " +
00278                                "permissions are not set correctly?");
00279     if ((ret = ioctl(fd, VIDIOC_QUERYCAP, &v4l2_cap)) < 0)
00280       throw std::runtime_error("couldn't query " + dev_name);
00281     printf("name = [%s]\n", v4l2_cap.card);
00282     printf("driver = [%s]\n", v4l2_cap.driver);
00283     printf("location = [%s]\n", v4l2_cap.bus_info);
00284     close(fd);
00285     string v4l_dev_path = v4l_path + string("/") + string(ent->d_name) +
00286                           string("/device");
00287     // my kernel is using /sys/class/video4linux/videoN/device/inputX/id
00288     DIR *d2 = opendir(v4l_dev_path.c_str());
00289     if (!d2)
00290       throw std::runtime_error("couldn't open " + v4l_dev_path);
00291     string input_dir;
00292     while ((ent2 = readdir(d2)) != NULL)
00293     {
00294       if (strncmp(ent2->d_name, "input", 5))
00295         continue; // ignore anything not beginning with "input"
00296 
00297       DIR *input = opendir((v4l_dev_path + string("/") + string(ent2->d_name)).c_str());
00298       bool output_set = false;
00299       while ((ent3 = readdir(input)) != NULL)
00300       {
00301         if (!strncmp(ent3->d_name, "input", 5))
00302         {
00303           input_dir = (string("input/") + string(ent3->d_name )).c_str();
00304           output_set = true;
00305           break;
00306         }
00307       }
00308       if (!output_set)
00309         input_dir = ent2->d_name;
00310       break;
00311     }
00312     closedir(d2);
00313     if (!input_dir.length())
00314       throw std::runtime_error("couldn't find input dir in " + v4l_dev_path);
00315     string vid_fname = v4l_dev_path + string("/") + input_dir +
00316                        string("/id/vendor");
00317     string pid_fname = v4l_dev_path + string("/") + input_dir +
00318                        string("/id/product");
00319     string ver_fname = v4l_dev_path + string("/") + input_dir +
00320                        string("/id/version");
00321     char vid[5], pid[5], ver[5];
00322     FILE *vid_fp = fopen(vid_fname.c_str(), "r");
00323     if (!vid_fp)
00324       throw std::runtime_error("couldn't open " + vid_fname);
00325     if (!fgets(vid, sizeof(vid), vid_fp))
00326       throw std::runtime_error("couldn't read VID from " + vid_fname);
00327     fclose(vid_fp);
00328     vid[4] = 0;
00329     printf("vid = [%s]\n", vid);
00330     FILE *pid_fp = fopen(pid_fname.c_str(), "r");
00331     if (!pid_fp)
00332       throw std::runtime_error("couldn't open " + pid_fname);
00333     if (!fgets(pid, sizeof(pid), pid_fp))
00334       throw std::runtime_error("couldn't read PID from " + pid_fname);
00335     fclose(pid_fp);
00336     printf("pid = [%s]\n", pid);
00337     FILE *ver_fp = fopen(ver_fname.c_str(), "r");
00338     if (!ver_fp)
00339       throw std::runtime_error("couldn't open " + ver_fname);
00340     if (!fgets(ver, sizeof(ver), ver_fp))
00341       throw std::runtime_error("couldn't read version from " + ver_fname);
00342     fclose(ver_fp);
00343     printf("ver = [%s]\n", ver);
00344   }
00345   closedir(d);
00346 }
00347 
00348 // saturate input into [0, 255]
00349 inline unsigned char sat(float f)
00350 {
00351   return (unsigned char)( f >= 255 ? 255 : (f < 0 ? 0 : f));
00352 }
00353 
00354 int Cam::grab(unsigned char **frame, uint32_t &bytes_used)
00355 {
00356   *frame = NULL;
00357   int ret = 0;
00358   fd_set rdset;
00359   timeval timeout;
00360   FD_ZERO(&rdset);
00361   FD_SET(fd, &rdset);
00362   timeout.tv_sec = 1;
00363   timeout.tv_usec = 0;
00364   bytes_used = 0;
00365   ret = select(fd + 1, &rdset, NULL, NULL, &timeout);
00366   if (ret == 0)
00367   {
00368     printf("select timeout in grab\n");
00369     return -1;
00370   }
00371   else if (ret < 0)
00372   {
00373     perror("couldn't grab image");
00374     return -1;
00375   }
00376   if (!FD_ISSET(fd, &rdset))
00377     return -1;
00378   memset(&buf, 0, sizeof(buf));
00379   buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00380   buf.memory = V4L2_MEMORY_MMAP;
00381   if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0)
00382     throw std::runtime_error("couldn't dequeue buffer");
00383   bytes_used = buf.bytesused;
00384   if (mode == MODE_RGB)
00385   {
00386     int num_pixels_different = 0; // just look at the Y channel
00387     unsigned char *pyuv = (unsigned char *)mem[buf.index];
00388     // yuyv is 2 bytes per pixel. step through every pixel pair.
00389     unsigned char *prgb = rgb_frame;
00390     unsigned char *pyuv_last = last_yuv_frame;
00391     for (unsigned i = 0; i < width * height * 2; i += 4)
00392     {
00393       *prgb++ = sat(pyuv[i]+1.402f  *(pyuv[i+3]-128));
00394       *prgb++ = sat(pyuv[i]-0.34414f*(pyuv[i+1]-128)-0.71414f*(pyuv[i+3]-128));
00395       *prgb++ = sat(pyuv[i]+1.772f  *(pyuv[i+1]-128));
00396       *prgb++ = sat(pyuv[i+2]+1.402f*(pyuv[i+3]-128));
00397       *prgb++ = sat(pyuv[i+2]-0.34414f*(pyuv[i+1]-128)-0.71414f*(pyuv[i+3]-128));
00398       *prgb++ = sat(pyuv[i+2]+1.772f*(pyuv[i+1]-128));
00399       if ((int)pyuv[i] - (int)pyuv_last[i] > motion_threshold_luminance ||
00400           (int)pyuv_last[i] - (int)pyuv[i] > motion_threshold_luminance)
00401         num_pixels_different++;
00402       if ((int)pyuv[i+2] - (int)pyuv_last[i+2] > motion_threshold_luminance ||
00403           (int)pyuv_last[i+2] - (int)pyuv[i+2] > motion_threshold_luminance)
00404         num_pixels_different++;
00405 
00406       // this gives bgr images...
00407       /*
00408       *prgb++ = sat(pyuv[i]+1.772f  *(pyuv[i+1]-128));
00409       *prgb++ = sat(pyuv[i]-0.34414f*(pyuv[i+1]-128)-0.71414f*(pyuv[i+3]-128));
00410       *prgb++ = sat(pyuv[i]+1.402f  *(pyuv[i+3]-128));
00411       *prgb++ = sat(pyuv[i+2]+1.772f*(pyuv[i+1]-128));
00412       *prgb++ = sat(pyuv[i+2]-0.34414f*(pyuv[i+1]-128)-0.71414f*(pyuv[i+3]-128));
00413       *prgb++ = sat(pyuv[i+2]+1.402f*(pyuv[i+3]-128));
00414       */
00415     }
00416     memcpy(last_yuv_frame, pyuv, width * height * 2);
00417     if (num_pixels_different > motion_threshold_count) // default: always true
00418       *frame = rgb_frame;
00419     else
00420     {
00421       *frame = NULL; // not enough luminance change
00422       release(buf.index); // let go of this image
00423     }
00424   }
00425   else if (mode == MODE_YUYV)
00426   {
00427     *frame = (uint8_t *)mem[buf.index];
00428   }
00429   else // mode == MODE_JPEG
00430   {
00431     //if (bytes_used > 100)
00432       *frame = (unsigned char *)mem[buf.index];
00433   }
00434   return buf.index;
00435 }
00436 
00437 void Cam::release(unsigned buf_idx)
00438 {
00439   if (buf_idx < NUM_BUFFER)
00440     if (ioctl(fd, VIDIOC_QBUF, &buf) < 0)
00441       throw std::runtime_error("couldn't requeue buffer");
00442 }
00443 
00444 void Cam::set_control(uint32_t id, int val)
00445 {
00446   v4l2_control c;
00447   c.id = id;
00448 
00449   if (ioctl(fd, VIDIOC_G_CTRL, &c) == 0)
00450   {
00451     printf("current value of %d is %d\n", id, c.value);
00452     /*
00453     perror("unable to get control");
00454     throw std::runtime_error("unable to get control");
00455     */
00456   }
00457   c.value = val;
00458   if (ioctl(fd, VIDIOC_S_CTRL, &c) < 0)
00459   {
00460     perror("unable to set control");
00461     throw std::runtime_error("unable to set control");
00462   }
00463 }
00464 
00465 void Cam::set_motion_thresholds(int lum, int count)
00466 {
00467   motion_threshold_luminance = lum;
00468   motion_threshold_count = count;
00469 }
00470 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


uvc_camera
Author(s): Ken Tossell/ktossell@umd.edu
autogenerated on Sat Jul 27 2013 22:46:52