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
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
00032 if(!cap.capabilities&V4L2_CAP_VIDEO_CAPTURE)
00033 throw "Device does not support video capture";
00034
00035
00036 if(!cap.capabilities&V4L2_CAP_STREAMING)
00037 throw "Device does not support streaming i/o";
00038
00039
00040 if(!cap.capabilities&V4L2_CAP_READWRITE)
00041 throw "Device does not support read/write i/o";
00042
00043
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
00059 v4l2_requestbuffers rb;
00060 memset(&rb,0,sizeof(v4l2_requestbuffers));
00061 rb.count=2;
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
00069 v4l2_buffer buf;
00070
00071 buffer[0]=0;
00072 buffer[1]=0;
00073
00074 for(i=0;i<2;i++)
00075 {
00076
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
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
00214
00215
00216 const int UvcInterface::dht_size=432;
00217
00218
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 };
