Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

CameraDriverV4L.cc

Go to the documentation of this file.
00001 #ifdef __linux__
00002 
00003 #include "CameraDriverV4L.h"
00004 #include "Shared/debuget.h"
00005 //#include "Shared/TimeET.h"
00006 #include "Shared/MarkScope.h"
00007 
00008 #include <fcntl.h>
00009 #include <sys/ioctl.h>
00010 
00011 using namespace std; 
00012 
00013 const std::string CameraDriver::autoRegisterCameraDriver = DeviceDriver::getRegistry().registerType<CameraDriver>("Camera");
00014 const size_t CameraDriver::HEADER_SIZE = LoadSave::getSerializedSize<unsigned int>()*4;
00015 
00016 unsigned int CameraDriver::getData(const char *& payload, unsigned int& payloadSize, unsigned int& timestamp, std::string& name) {
00017   //cout << '.' << flush;
00018   payload=NULL;
00019   payloadSize=0;
00020   if(camfd<0)
00021     return frameCount;
00022 
00023   curBuf.resize(getBufferSize());
00024   
00025   // reset non-blocking in case thread was canceled while we were
00026   // were in a blocking call...
00027   if(blocking) {
00028     if( fcntl(camfd,F_SETFL,O_NONBLOCK) != 0) {
00029       perror("ioctl set non-blocking mode");
00030       blocking = fcntl(camfd,F_GETFL) & O_NONBLOCK;
00031     } else
00032       blocking=false;
00033   }
00034 
00035   unsigned int t=get_time();
00036   unsigned int dropped=0;
00037   int nbytes;
00038   size_t lastread;
00039   while(timestamp>t) {
00040     //clear any backlog
00041     while( (nbytes=read(camfd, &curBuf[HEADER_SIZE], curBuf.size()-HEADER_SIZE)) >= 0) {
00042       lastread=nbytes;
00043       ++dropped;
00044       //cout << "clear " << TimeET() << endl;
00045     }
00046     usleep(static_cast<unsigned int>((timestamp-t)*1000/(getTimeScale()>0?getTimeScale():1.f)));
00047     t=get_time();
00048   }
00049   timestamp = t;
00050   // we might've been asleep for a while, so need to double check a few things...
00051   if(camfd<0) // in case we shutdown while asleep!
00052     return frameCount;
00053 
00054   unsigned int width, height, components=3;
00055   {
00056     MarkScope l(lock);
00057     width=getWidth();
00058     height=getHeight();
00059     if(curBuf.size()!=getBufferSize()) { // check in case user changed resolution
00060       curBuf.resize(getBufferSize());
00061       dropped=0;
00062     }
00063   }
00064 
00065   //get most recent image
00066   while( (nbytes=read(camfd, &curBuf[HEADER_SIZE], curBuf.size()-HEADER_SIZE)) >= 0) {
00067     lastread=nbytes;
00068     ++dropped;
00069     //cout << "early " << TimeET() << endl;
00070   }
00071   timestamp = get_time();
00072 
00073   char * buf=reinterpret_cast<char*>(&curBuf[0]);
00074   unsigned int remain=curBuf.size();
00075   LoadSave::encodeIncT(*layer,buf,remain);
00076   LoadSave::encodeIncT(width,buf,remain);
00077   LoadSave::encodeIncT(height,buf,remain);
00078   LoadSave::encodeIncT(components,buf,remain);
00079 
00080   if(remain<img_size) {
00081     // user has changed resolution... skip frame
00082     //cerr << "Not enough space in buffer for image! " << remain << " vs " << img_size << endl;
00083     return frameCount;
00084   }
00085   if(dropped==0 || lastread!=img_size) {
00086     ASSERTRETVAL(static_cast<size_t>(remain)>=img_size, "Read more than remains in buffer!", frameCount);
00087     // disable non-blocking io, we want to wait for the next frame
00088     if( fcntl(camfd,F_SETFL,0) != 0) {
00089       perror("ioctl set blocking mode");
00090       blocking = fcntl(camfd,F_GETFL) & O_NONBLOCK;
00091     } else
00092       blocking=true;
00093     //TimeET bt;
00094     //cout << "block " << bt << ' ';
00095     nbytes = read(camfd, buf, remain);
00096     timestamp = get_time();
00097     //cout << bt.Age() << endl;
00098     if( fcntl(camfd,F_SETFL,O_NONBLOCK) != 0) {
00099       perror("ioctl set non-blocking mode");
00100       blocking = fcntl(camfd,F_GETFL) & O_NONBLOCK;
00101     } else
00102       blocking=false;
00103     if ( nbytes<0 ) {
00104       perror("Error reading from camera");
00105       return frameCount;
00106     }
00107     lastread=nbytes;
00108     ++dropped;
00109   }
00110   
00111   MarkScope l(lock);
00112   // check if we had a short read or if the desired image size changed while we were blocked
00113   if ( lastread!=img_size // short read
00114        || (resolution!=0 && img_size!=width*height*6) // size changed (downsample)
00115        || (resolution==0 && img_size!=width*height*3/2) // size changed (upsample)
00116      )
00117   {
00118     // short read is relatively normal -- always get this on the first frame for
00119     // some reason, and also might have just changed resolution setting and hasn't
00120     // taken effect yet.
00121     //cerr << "CameraDriver got " << nbytes << " bytes from camera, expected " << img_size << endl;
00122     return frameCount;
00123   }
00124 
00125   if(resolution==0)
00126     interleave_yuv_up();
00127   else
00128     interleave_yuv_down();
00129   
00130   if ( autobright )
00131     auto_bright();
00132   
00133   curBuf.swap(lastBuf);
00134   payload = reinterpret_cast<char*>(&lastBuf[0]);
00135   payloadSize = lastBuf.size();
00136   name = nextName();
00137   return frameCount+=dropped;
00138 }
00139 
00140 
00141 void CameraDriver::setDataSourceThread(LoadDataThread* th) {
00142   DataSource::setDataSourceThread(th);
00143   if(th != NULL) {
00144     open_cam();
00145     path.addPrimitiveListener(this);
00146     resolution.addPrimitiveListener(this);
00147   } else {
00148     resolution.removePrimitiveListener(this);
00149     path.removePrimitiveListener(this);
00150     close_cam();
00151   }
00152 }
00153 
00154 void CameraDriver::plistValueChanged(const plist::PrimitiveBase& pl) {
00155   if(&pl == &path) {
00156     MarkScope l(lock);
00157     open_cam();
00158   } else if(&pl == &resolution) {
00159     MarkScope l(lock);
00160     set_resolution();
00161   }
00162 }
00163 
00164 void CameraDriver::close_cam() {
00165   if(camfd >= 0) {
00166     ::close(camfd);
00167     camfd=-1;
00168   }
00169 }
00170 
00171 void CameraDriver::open_cam() {
00172   close_cam();
00173   if(path.size() == 0)
00174     return;
00175   
00176   camfd = ::open(path.c_str(), O_RDWR|O_NONBLOCK, 0);
00177   if ( camfd < 0 ) {
00178     perror("Error on open()");
00179     cerr << "Could not open camera device '" << path << "'" << endl;
00180     return;
00181   }
00182   blocking=false;
00183   
00184   set_resolution();
00185 
00186   if ( ioctl(camfd, VIDIOCGPICT, &vid_pic) == -1 )
00187     perror ("ioctl (VIDIOCGPICT) brightness");
00188   vid_pic.brightness=128*256; // initialize to middle brightness
00189   if ( ioctl(camfd, VIDIOCSPICT, &vid_pic) == -1 )
00190     perror ("ioctl (VIDIOCSPICT) brightness");
00191 
00192   // try to switch to YUV mode so we don't have to do color conversion
00193   // this is only trying for 4-2-0 planar mode...
00194   isYUVMode=false;
00195   ioctl(camfd, VIDIOCGPICT, &vid_pic);
00196   vid_pic.depth=16;
00197   vid_pic.palette=VIDEO_PALETTE_YUV420P;
00198   if ( ioctl(camfd, VIDIOCSPICT, &vid_pic) == -1 )
00199     perror ("ioctl (VIDIOCSPICT) palette");
00200   else {
00201     img_size = vid_win.width * vid_win.height + vid_win.width*vid_win.height/2;
00202     isYUVMode=true;
00203   }
00204   
00205   /*{
00206     struct v4l2_control ctrl;
00207     ctrl.id=V4L2_CID_AUTO_WHITE_BALANCE;
00208     ctrl.value=0;
00209     if ( ioctl(camera->fd,VIDIOC_S_CTRL,&ctrl) == -1 )
00210       perror ("ioctl (VIDIOC_S_CTRL) disable auto white balance");
00211   } */ 
00212 }
00213 
00214 void CameraDriver::get_cam_info() {
00215   ioctl(camfd, VIDIOCGCAP, &vid_caps);
00216   ioctl(camfd, VIDIOCGWIN, &vid_win);
00217   ioctl(camfd, VIDIOCGPICT, &vid_pic);
00218 }
00219 
00220 void CameraDriver::set_cam_info() {
00221   if ( ioctl(camfd, VIDIOCSPICT, &vid_pic) == -1 ) {
00222     perror ("ioctl (VIDIOCSPICT)");
00223     cout << "refreshing settings..." << endl;
00224     if ( ioctl(camfd, VIDIOCGPICT, &vid_pic) == -1 )
00225       perror ("ioctl (VIDIOCGPICT)");
00226   }
00227   if ( ioctl(camfd, VIDIOCSWIN, &vid_win) == -1 ) {
00228     perror ("ioctl (VIDIOCSWIN)");
00229     cout << "refreshing settings..." << endl;
00230     if ( ioctl(camfd, VIDIOCGWIN, &vid_win) == -1 )
00231       perror ("ioctl (VIDIOCGWIN)");
00232   }
00233 }
00234 
00235 void CameraDriver::set_resolution() {
00236   get_cam_info();
00237   int width = vid_caps.maxwidth >> (resolution==0 ? 0 : resolution-1);
00238   int height = vid_caps.maxheight >> (resolution==0 ? 0 : resolution-1);
00239   if ( width < vid_caps.minwidth || height < vid_caps.minheight ) {
00240     cout << "Warning: requested image " << width<<'x'<<height<<" smaller than camera's minimum size, trying ";
00241     width = vid_caps.minwidth;
00242     height = vid_caps.minheight;
00243     cout << width<<'x'<<height << endl;
00244   }
00245 
00246   vid_win.width = width;
00247   vid_win.height = height;
00248   set_cam_info();
00249   width = vid_win.width; // restore in case of error or adjustment of requested values
00250   height = vid_win.height; // (camera might only support certain explicit resolutions)
00251   
00252   if(isYUVMode) {
00253     img_size = width * height + width * height / 2;
00254   } else {
00255     img_size = width * height * 3;
00256   }
00257     
00258   cout << "Camera image size is " << getWidth() << 'x' << getHeight() << endl;
00259 }
00260 
00261 /*! this is run @em after interleaving, so we go through every 3rd byte of the
00262  *  entire image to sum the y channel */
00263 void CameraDriver::auto_bright() {
00264   size_t total = 0;
00265   unsigned char *pix = &curBuf[HEADER_SIZE];
00266   const size_t skip = getWidth() * getHeight() / 719; // actually, only take a subsample
00267   const size_t npix = getWidth() * getHeight() / skip; // we want a regular sample stride, so will actually be a bit less
00268   const unsigned char *endp = pix + npix*skip*3;
00269   for (; pix!=endp; pix+=skip*3) // *3 because of interleaving
00270     total += *pix;
00271   int average = total / npix;
00272   if((average <= 120 || average >= 136)) {
00273     int bright = vid_pic.brightness + (128 - average)*256/4;
00274     const typeof(vid_pic.brightness) maxBright = numeric_limits<typeof(vid_pic.brightness)>::max();
00275     const typeof(vid_pic.brightness) minBright = numeric_limits<typeof(vid_pic.brightness)>::min();
00276     if(bright>maxBright)
00277       vid_pic.brightness = maxBright;
00278     else if(bright<minBright)
00279       vid_pic.brightness = minBright;
00280     else
00281       vid_pic.brightness = bright;
00282     //cout << "Autobrightness " << average << " " << vid_pic.brightness << endl;
00283     set_cam_info();
00284   }
00285 }
00286 
00287 void CameraDriver::interleave_yuv_down() {
00288   unsigned char * buffer = &curBuf[HEADER_SIZE];
00289   int width = vid_win.width;
00290   int height = vid_win.height;
00291   
00292   //first downsample y channel
00293   char tmp[width/2];
00294   int x,y=0;
00295   for(x=0; x<width; x+=2) {
00296     short t=buffer[x+y*width];
00297     t+=buffer[x+y*width+1];
00298     t+=buffer[x+y*width+width];
00299     t+=buffer[x+y*width+width+1];
00300     tmp[x/2] = t/4;
00301   }
00302   for(x=0; x<width/2; x++) {
00303     buffer[x*3] = tmp[x];
00304   }  
00305   for(y=2; y<height; y+=2) {
00306     for(x=0; x<width; x+=2) {
00307       short t=buffer[x+y*width];
00308       t+=buffer[x+y*width+1];
00309       t+=buffer[x+y*width+width];
00310       t+=buffer[x+y*width+width+1];
00311       buffer[y*width/4*3+x/2*3] = t/4;
00312     }
00313   }
00314   // now fill in color components
00315   unsigned char * c = &buffer[width*height];
00316   buffer = &curBuf[HEADER_SIZE]+1;
00317   unsigned char * endp = buffer + width*height/4*3;
00318   while(buffer!=endp) { // u/Cr channel
00319     *buffer = *c++;
00320     buffer+=3;
00321   }
00322   buffer = &curBuf[HEADER_SIZE]+2;
00323   endp = buffer + width*height/4*3;
00324   while(buffer!=endp) { // v/Cb channel
00325     *buffer = *c++;
00326     buffer+=3;
00327   }
00328 }
00329 
00330 void CameraDriver::interleave_yuv_up() {
00331   unsigned char * buffer = &curBuf[HEADER_SIZE];
00332   size_t width = vid_win.width;
00333   size_t height = vid_win.height;
00334   tmpBuf.resize(getBufferSize());
00335   memcpy(&tmpBuf[0],&curBuf[0],HEADER_SIZE);
00336   unsigned char * out = &tmpBuf[HEADER_SIZE];
00337 
00338   //first copy over y channel
00339   unsigned char * i = buffer;
00340   unsigned char * o = out;
00341   unsigned char * e = i + width*height;
00342   while(i!=e) {
00343     *o=*i++;
00344     o+=3;
00345   }
00346   
00347   //now u channel
00348   o = out+1;
00349   buffer+=width*height;
00350   for(size_t y=0; y<height/2; ++y) {
00351     i = buffer+width/2*y;
00352     e = i + width/2;
00353     while(i!=e) {
00354       *o=*(o+3)=*(o+width*3)=*(o+width*3+3)=*i++;
00355       o+=6;
00356     }
00357     o+=width*3;
00358   }
00359   
00360   //and now v channel
00361   o = out+2;
00362   buffer+=width*height/4;
00363   for(size_t y=0; y<height/2; ++y) {
00364     i = buffer+width/2*y;
00365     e = i + width/2;
00366     while(i!=e) {
00367       *o=*(o+3)=*(o+width*3)=*(o+width*3+3)=*i++;
00368       o+=6;
00369     }
00370     o+=width*3;
00371   }
00372   
00373   curBuf.swap(tmpBuf);
00374 }
00375  
00376 
00377 /*! @file
00378  * @brief 
00379  * @author Ethan Tira-Thompson (ejt) (Creator)
00380  *
00381  * $Author: ejt $
00382  * $Name: tekkotsu-4_0 $
00383  * $Revision: 1.2 $
00384  * $State: Exp $
00385  * $Date: 2007/08/31 06:16:07 $
00386  */
00387 
00388 #endif

Tekkotsu Hardware Abstraction Layer 4.0
Generated Thu Nov 22 01:00:53 2007 by Doxygen 1.5.4