UvcInterface.cc

00001 #include <pthread.h>
00002 #include <fcntl.h>
00003 #include <unistd.h>
00004 #include <sys/ioctl.h>
00005 #include <errno.h>
00006 #include <sys/mman.h>
00007 #include <stdio.h>
00008 #include <poll.h>
00009 
00010 #include "UvcInterface.h"
00011 
00012 int UvcInterface::Open(void)
00013 {
00014         int ret=0;
00015         int i;
00016 
00017         if(fd!=-1) Close();
00018 
00019         try
00020         {
00021                 fd=open(device,O_RDWR|O_NONBLOCK);
00022                 if(fd==-1)
00023                         throw "Error opening device";
00024 
00025                 //retrieve device capabilities
00026             memset(&cap, 0, sizeof(v4l2_capability));
00027             ret=ioctl(fd,VIDIOC_QUERYCAP,&cap);
00028             if(ret==-1)
00029                 throw "Error querying capabilities";
00030 
00031             //check for video capture capability
00032             if(!cap.capabilities&V4L2_CAP_VIDEO_CAPTURE)
00033                 throw "Device does not support video capture";
00034 
00035                 //check for streaming i/o capability (unnecessary?)
00036             if(!cap.capabilities&V4L2_CAP_STREAMING)
00037                 throw "Device does not support streaming i/o";
00038 
00039              //check for read/write capability
00040                 if(!cap.capabilities&V4L2_CAP_READWRITE)
00041                         throw "Device does not support read/write i/o";
00042 
00043                 //set capture format
00044                 memset(&fmt, 0, sizeof(v4l2_format));
00045                 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00046                 fmt.fmt.pix.width = width;
00047                 fmt.fmt.pix.height = height;
00048                 fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
00049                 fmt.fmt.pix.field = V4L2_FIELD_ANY;
00050                 ret=ioctl(fd,VIDIOC_S_FMT,&fmt);
00051                 if(ret==-1)
00052                         printf("Unable to set format. (%d, %d)\r\n",ret,errno);
00053 
00054                 ret=ioctl(fd,VIDIOC_G_FMT,&fmt);
00055                 if(ret==-1)
00056                         printf("Unable to retrieve format. (%d, %d)\r\n",ret,errno);
00057 
00058                 //request memory buffers
00059                 v4l2_requestbuffers rb;
00060                 memset(&rb,0,sizeof(v4l2_requestbuffers));
00061                 rb.count=2; //two buffers; one for the device to fill and one for the driver to read
00062                 rb.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
00063                 rb.memory=V4L2_MEMORY_MMAP;
00064                 ret = ioctl(fd,VIDIOC_REQBUFS,&rb);
00065                 if(ret==-1)
00066                         throw "Unable to allocate buffers";
00067 
00068                 //general-purpose buffer info struct
00069                 v4l2_buffer buf;
00070 
00071                 buffer[0]=0;
00072                 buffer[1]=0;
00073 
00074                 for(i=0;i<2;i++)
00075                 {
00076                         //query a buffer
00077                         memset(&buf,0,sizeof(v4l2_buffer));
00078                         buf.index=i;
00079                         buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
00080                         buf.memory=V4L2_MEMORY_MMAP;
00081                         ret=ioctl(fd,VIDIOC_QUERYBUF,&buf);
00082                         if(ret==-1)
00083                                 throw "Unable to query frame";
00084 
00085                         //retrieve the buffer memory location
00086                         buffer[i]=(unsigned char*)mmap(0,buf.length,PROT_READ,MAP_SHARED,fd,buf.m.offset);
00087                         length[i]=buf.length;
00088 
00089                         if(buffer[i]==MAP_FAILED)
00090                         {
00091                                 buffer[i]=0;
00092                                 throw "Unable to map frame";
00093                         }
00094                 }
00095 
00096                 for(i=0;i<2;i++)
00097                 {
00098                         memset(&buf,0,sizeof(v4l2_buffer));
00099                         buf.index=i;
00100                         buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
00101                         buf.memory=V4L2_MEMORY_MMAP;
00102                         ret=ioctl(fd,VIDIOC_QBUF,&buf);
00103                         if(ret==-1)
00104                                 throw "Unable to queue frame";
00105                 }
00106 
00107                 int type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
00108                 ret=ioctl(fd,VIDIOC_STREAMON,&type);
00109                 if(ret==-1)
00110                         throw "Unable to initiate video stream";
00111 
00112                 int bufLength=length[0];
00113                 if(length[1]>bufLength) bufLength=length[1];
00114                 frame=new unsigned char[bufLength+dht_size];
00115         }
00116         catch(char const *msg)
00117         {
00118                 printf("%s. (%d, %d)\r\n",msg,ret,errno);
00119                 Close();
00120                 return -1;
00121         }
00122         return 0;
00123 }
00124 
00125 int UvcInterface::Close(void)
00126 {
00127         int ret,i;
00128 
00129         if(fd==-1) return 0;
00130 
00131         int type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
00132         ret=ioctl(fd,VIDIOC_STREAMOFF,&type);
00133         if(ret==-1)
00134                 printf("Unable to terminate video stream. (%d, %d)\r\n",ret,errno);
00135 
00136         for(i=0;i<2;i++)
00137         {
00138                 if(buffer[i]!=0)
00139                 {
00140                         ret=munmap(buffer[i],length[i]);
00141                         if(ret==-1)
00142                                 printf("Unable to unmap frame. (%d, %d)\r\n",ret,errno);
00143                         buffer[i]=0;
00144                 }
00145         }
00146 
00147         ret=close(fd);
00148         if(ret==-1)
00149                 printf("Unable to close video device. (%d, %d)\r\n",ret,errno);
00150         fd=-1;
00151 
00152         if(frame) delete [] frame;
00153         frame=0;
00154         frameSize=0;
00155 
00156         return 0;
00157 }
00158 
00159 int UvcInterface::Read()
00160 {
00161         int ret=0;
00162         const int hdr=0xaf;
00163 
00164         try
00165         {
00166                 pollfd pfd;
00167                 pfd.fd=fd;
00168                 pfd.events=POLLIN;
00169                 do
00170                 {
00171                         poll(&pfd,1,100);
00172                         pthread_testcancel();
00173 
00174                 } while(pfd.revents==0);
00175 
00176                 v4l2_buffer buf;
00177             memset(&buf,0,sizeof(v4l2_buffer));
00178             buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
00179             buf.memory=V4L2_MEMORY_MMAP;
00180             ret=ioctl(fd,VIDIOC_DQBUF,&buf);
00181             if(ret==-1)
00182                         throw "Unable to dequeue frame";
00183 
00184                 memcpy(frame,buffer[buf.index],hdr);
00185                 memcpy(frame+hdr,dht_data,dht_size);
00186                 memcpy(frame+hdr+dht_size,buffer[buf.index]+hdr,(buf.bytesused-hdr));
00187                 frameSize=buf.bytesused+dht_size;
00188 
00189                 ret=ioctl(fd,VIDIOC_QBUF,&buf);
00190                 if(ret==-1)
00191                         throw "Unable to requeue frame";
00192         }
00193         catch(char const *msg)
00194         {
00195                 printf("%s. (%d, %d)\r\n",msg,ret,errno);
00196                 return -1;
00197         }
00198         return 0;
00199 }
00200 
00201 int UvcInterface::GetWidth(void) const
00202 {
00203         if(fd==-1) return -1;
00204         return fmt.fmt.pix.width;
00205 }
00206 
00207 int UvcInterface::GetHeight(void) const
00208 {
00209         if(fd==-1) return -1;
00210         return fmt.fmt.pix.height;
00211 }
00212 
00213 // Default Huffman Table (DHT) for the MJPG frames
00214 
00215 // table size
00216 const int UvcInterface::dht_size=432;
00217 
00218 // table data
00219 const unsigned char UvcInterface::dht_data[dht_size] = {
00220     0xff, 0xc4, 0x00, 0x1f, 0x00, 0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01,
00221         0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00222     0x00, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a,
00223     0x0b, 0xff, 0xc4, 0x00, 0xb5, 0x10, 0x00, 0x02,
00224     0x01, 0x03, 0x03, 0x02, 0x04, 0x03, 0x05, 0x05, 0x04, 0x04, 0x00, 0x00,
00225     0x01, 0x7d, 0x01, 0x02, 0x03, 0x00, 0x04, 0x11,
00226     0x05, 0x12, 0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, 0x22, 0x71,
00227     0x14, 0x32, 0x81, 0x91, 0xa1, 0x08, 0x23, 0x42,
00228     0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0, 0x24, 0x33, 0x62, 0x72, 0x82, 0x09,
00229     0x0a, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x25, 0x26,
00230     0x27, 0x28, 0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x43,
00231     0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x53,
00232     0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67,
00233     0x68, 0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77,
00234     0x78, 0x79, 0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a, 0x92,
00235     0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a,
00236     0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4,
00237     0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3,
00238     0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4, 0xd5, 0xd6,
00239     0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2, 0xe3, 0xe4,
00240     0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, 0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6,
00241     0xf7, 0xf8, 0xf9, 0xfa, 0xff, 0xc4, 0x00, 0x1f,
00242     0x01, 0x00, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
00243     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02,
00244     0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0xff, 0xc4, 0x00,
00245     0xb5, 0x11, 0x00, 0x02, 0x01, 0x02, 0x04, 0x04,
00246     0x03, 0x04, 0x07, 0x05, 0x04, 0x04, 0x00, 0x01, 0x02, 0x77, 0x00, 0x01,
00247     0x02, 0x03, 0x11, 0x04, 0x05, 0x21, 0x31, 0x06,
00248     0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13, 0x22, 0x32, 0x81, 0x08, 0x14,
00249     0x42, 0x91, 0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33,
00250     0x52, 0xf0, 0x15, 0x62, 0x72, 0xd1, 0x0a, 0x16, 0x24, 0x34, 0xe1, 0x25,
00251     0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26, 0x27, 0x28,
00252     0x29, 0x2a, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46,
00253     0x47, 0x48, 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56,
00254     0x57, 0x58, 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6a,
00255     0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a,
00256     0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a, 0x92, 0x93, 0x94,
00257     0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3,
00258     0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6,
00259     0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5,
00260     0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8,
00261     0xd9, 0xda, 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7,
00262     0xe8, 0xe9, 0xea, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa
00263 };

Last updated 12 September 2005 21:38:45