00001 #ifdef __linux__
00002
00003 #include "CameraDriverV4L.h"
00004 #include "Shared/debuget.h"
00005
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
00018 payload=NULL;
00019 payloadSize=0;
00020 if(camfd<0)
00021 return frameCount;
00022
00023 curBuf.resize(getBufferSize());
00024
00025
00026
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
00041 while( (nbytes=read(camfd, &curBuf[HEADER_SIZE], curBuf.size()-HEADER_SIZE)) >= 0) {
00042 lastread=nbytes;
00043 ++dropped;
00044
00045 }
00046 usleep(static_cast<unsigned int>((timestamp-t)*1000/(getTimeScale()>0?getTimeScale():1.f)));
00047 t=get_time();
00048 }
00049 timestamp = t;
00050
00051 if(camfd<0)
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()) {
00060 curBuf.resize(getBufferSize());
00061 dropped=0;
00062 }
00063 }
00064
00065
00066 while( (nbytes=read(camfd, &curBuf[HEADER_SIZE], curBuf.size()-HEADER_SIZE)) >= 0) {
00067 lastread=nbytes;
00068 ++dropped;
00069
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
00082
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
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
00094
00095 nbytes = read(camfd, buf, remain);
00096 timestamp = get_time();
00097
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
00113 if ( lastread!=img_size
00114 || (resolution!=0 && img_size!=width*height*6)
00115 || (resolution==0 && img_size!=width*height*3/2)
00116 )
00117 {
00118
00119
00120
00121
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;
00189 if ( ioctl(camfd, VIDIOCSPICT, &vid_pic) == -1 )
00190 perror ("ioctl (VIDIOCSPICT) brightness");
00191
00192
00193
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
00207
00208
00209
00210
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;
00250 height = vid_win.height;
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
00262
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;
00267 const size_t npix = getWidth() * getHeight() / skip;
00268 const unsigned char *endp = pix + npix*skip*3;
00269 for (; pix!=endp; pix+=skip*3)
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
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
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
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) {
00319 *buffer = *c++;
00320 buffer+=3;
00321 }
00322 buffer = &curBuf[HEADER_SIZE]+2;
00323 endp = buffer + width*height/4*3;
00324 while(buffer!=endp) {
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
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
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
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
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388 #endif