00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "LIN_V4L2Webcam.hpp"
00024
00025 #include <utils.hpp>
00026 #include <unistd.h>
00027 #include <errno.h>
00028 #include <sys/mman.h>
00029 #include <sys/ioctl.h>
00030 #include <asm/types.h>
00031 #include <linux/kernel.h>
00032 #include <cstring>
00033 #include <sys/stat.h>
00034 #include <fcntl.h>
00035
00036
00037
00038
00039 namespace kn{
00040
00041
00042 void V4L2Webcam::initMMAP(void)
00043 {
00044
00045 int nbbuffers = 4;
00046
00047
00048 memset(&rb, 0, sizeof(struct v4l2_requestbuffers));
00049 rb.count = nbbuffers;
00050 rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00051 rb.memory = V4L2_MEMORY_MMAP;
00052 if(ioctl(filedescriptor, VIDIOC_REQBUFS, &rb)<0){
00053 throw ControllerException("Unable allocate buffers","V4L2Webcam");
00054 }
00055 nbbuffers = rb.count;
00056 buffersmmap = new void*[nbbuffers];
00057
00058
00059 for (int i = 0; i < nbbuffers; i++) {
00060 memset(&buf, 0, sizeof(struct v4l2_buffer));
00061 buf.index = i;
00062 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00063 buf.memory = V4L2_MEMORY_MMAP;
00064 if(ioctl(filedescriptor, VIDIOC_QUERYBUF, &buf) < 0)
00065 throw ControllerException("Unable to query buffer","V4L2Webcam");
00066 buffersmmap[i] = mmap(0 ,buf.length, PROT_READ, MAP_SHARED, filedescriptor,buf.m.offset);
00067 if (buffersmmap[i] == MAP_FAILED)
00068 throw ControllerException("Unable to map buffer","V4L2Webcam");
00069 }
00070
00071
00072 for (int i = 0; i < nbbuffers; ++i) {
00073 memset(&buf, 0, sizeof(struct v4l2_buffer));
00074 buf.index = i;
00075 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00076 buf.memory = V4L2_MEMORY_MMAP;
00077 if(ioctl(filedescriptor, VIDIOC_QBUF, &buf) < 0)
00078 throw ControllerException("Unable to queue buffer","V4L2Webcam");
00079 }
00080
00081 }
00082
00083 void V4L2Webcam::initREAD(void)
00084 {
00085
00086 buffersread = new V4L2_Buffer[1];
00087
00088 buffersread[0].length = fmt.fmt.pix.sizeimage;
00089 buffersread[0].start = new void* [fmt.fmt.pix.sizeimage];
00090
00091 }
00092
00093
00094
00095 void V4L2Webcam::openDevice(const V4L2WebcamParams& params)
00096 {
00097 if(this->isenable)
00098 this->closeDevice();
00099
00100 this->gb = params.grab;
00101 this->camwidth = params.width;
00102 this->camheight = params.height;
00103
00104 buffersmmap = 0;
00105 buffersread = 0;
00106 tmpbuffer = 0;
00107 framebuffer = 0;
00108 outputbuffer = 0;
00109
00110 isgrabbing = false;
00111
00112 this->devicename = params.devicename;
00113
00114 if(this->devicename == ""){
00115 throw ControllerException("Device name is empty","V4L2Webcam");
00116 }
00117
00118 if ((filedescriptor = open((this->devicename).c_str(), O_RDWR)) == -1) {
00119 throw ControllerException("Cannot open device " + this->devicename,"V4L2Webcam");
00120 }
00121
00122 std::string msg;
00123 if(!checkAvailability(msg)){
00124 throw ControllerException(msg,"V4L2Webcam");
00125 }
00126
00127
00128 memset(&fmt, 0, sizeof(struct v4l2_format));
00129 fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00130 fmt.fmt.pix.width = this->camwidth;
00131 fmt.fmt.pix.height = this->camheight;
00132 if(params.fmt == FMT_MJPEG)
00133 fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
00134 else
00135 fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
00136 fmt.fmt.pix.field = V4L2_FIELD_ANY;
00137 if (ioctl(filedescriptor, VIDIOC_S_FMT, &fmt) < 0) {
00138 throw ControllerException("Unable to set format","V4L2Webcam");
00139 }
00140
00141
00142 if ((fmt.fmt.pix.width != params.width) ||
00143 (fmt.fmt.pix.height != params.height)) {
00144 std::cerr << "Sorry, expected size (" << params.width << "," << params.height << ") is not available. Thus, size is " << fmt.fmt.pix.width << " " << fmt.fmt.pix.height << std::endl;
00145 this->camwidth = fmt.fmt.pix.width;
00146 this->camheight = fmt.fmt.pix.height;
00147 }
00148
00149
00150 struct v4l2_streamparm setfps;
00151 memset(&setfps, 0, sizeof(struct v4l2_streamparm));
00152 setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00153 setfps.parm.capture.timeperframe.numerator=1;
00154 setfps.parm.capture.timeperframe.denominator=params.framerate;
00155 if(ioctl(filedescriptor, VIDIOC_S_PARM, &setfps) < 0){
00156 throw ControllerException("Unable to set fps","V4L2Webcam");
00157 }
00158
00159 if(params.grab == MMAP_METHOD)
00160 initMMAP();
00161 else
00162 initREAD();
00163
00164 size_t framesizein = (this->camwidth * this->camheight << 1);
00165 switch (params.fmt) {
00166
00167 case FMT_MJPEG:
00168 if(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG){
00169 tmpbuffer = new unsigned char [framesizein];
00170 framebuffer = new unsigned char [this->camwidth * (this->camheight+8) * 2];
00171 outputbuffer = new unsigned char [this->camwidth * (this->camheight) * 3];
00172 }else{
00173 if(params.force){
00174 std::cerr << "V4L2Webcam warning : Unsupported pixel format but FORCE BRUTE" << std::endl;
00175 tmpbuffer = new unsigned char [framesizein];
00176 framebuffer = new unsigned char [this->camwidth * (this->camheight+8) * 2];
00177 outputbuffer = new unsigned char [this->camwidth * (this->camheight) * 3];
00178 fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
00179 }else{
00180 throw ControllerException("Unsupported pixel format","V4L2Webcam");
00181 }
00182
00183 }
00184 break;
00185 case FMT_YUYV:
00186 if(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV){
00187 framebuffer = new unsigned char [framesizein];
00188 outputbuffer = new unsigned char [this->camwidth * (this->camheight) * 3];
00189 }else{
00190 if(params.force){
00191 std::cerr << "V4L2Webcam warning : Unsupported pixel format but FORCE BRUTE" << std::endl;
00192 framebuffer = new unsigned char [framesizein];
00193 outputbuffer = new unsigned char [this->camwidth * (this->camheight) * 3];
00194 fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
00195 }else{
00196 throw ControllerException("Unsupported pixel format","V4L2Webcam");
00197 }
00198 }
00199 break;
00200
00201 default:
00202 throw ControllerException("Unsupported pixel format","V4L2Webcam");
00203 }
00204 isenable = true;
00205 }
00206
00207
00208 bool V4L2Webcam::checkAvailability(std::string& message)
00209 {
00210
00211 memset(&cap, 0, sizeof(struct v4l2_capability));
00212
00213
00214 int ret = ioctl(filedescriptor, VIDIOC_QUERYCAP, &cap);
00215 if (ret < 0) {
00216 message = std::string("Unable to get webcam capabilities");
00217 return false;
00218 }
00219
00220 if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
00221 message = std::string("Video capture not supported");
00222 return false;
00223 }
00224
00225 if(gb==MMAP_METHOD)
00226 if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
00227 message = std::string("Video streaming method not supported");
00228 return false;
00229 }
00230
00231 if(gb==READ_METHOD)
00232 if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
00233 message = std::string("Video read method not supported");
00234 return false;
00235 }
00236
00237 return true;
00238
00239 }
00240
00241 void V4L2Webcam::closeDevice(void)
00242 {
00243 if(isgrabbing) stop();
00244 isgrabbing = false;
00245
00246 if(framebuffer) delete[] framebuffer;
00247 framebuffer = 0;
00248
00249 if(tmpbuffer) delete[] tmpbuffer;
00250 tmpbuffer = 0;
00251
00252 if(buffersmmap){
00253 for(unsigned int i = 0; i < rb.count; ++i)
00254 munmap(buffersmmap[i], buf.length);
00255 delete[] buffersmmap;
00256 }
00257 buffersmmap = 0;
00258
00259 if(outputbuffer) delete[] outputbuffer;
00260 outputbuffer = 0;
00261
00262 if(buffersread) delete[] buffersread;
00263 buffersread = 0;
00264
00265 isenable = false;
00266
00267 if(filedescriptor>0) close(filedescriptor);
00268 filedescriptor = -1;
00269
00270 }
00271
00272
00273 void V4L2Webcam::start(void)
00274 {
00275 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00276 if(ioctl(filedescriptor, VIDIOC_STREAMON, &type) < 0)
00277 throw ControllerException("Unable to start streaming","start");
00278 isgrabbing = true;
00279
00280 }
00281
00282 void V4L2Webcam::stop(void)
00283 {
00284 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00285 if(ioctl(filedescriptor, VIDIOC_STREAMOFF, &type) < 0)
00286 throw ControllerException("Unable to stop streaming","start");
00287 isgrabbing = false;
00288 }
00289
00290
00291 unsigned char * V4L2Webcam::getImage(void)
00292 {
00293
00294 #define HEADERFRAME1 0xaf
00295
00296 if (!isgrabbing)
00297 start();
00298
00299 memset(&buf, 0, sizeof(struct v4l2_buffer));
00300 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00301 buf.memory = V4L2_MEMORY_MMAP;
00302 if(ioctl(filedescriptor, VIDIOC_DQBUF, &buf) < 0)
00303 throw ControllerException("Unable to dequeue buffer","getImage");
00304
00305
00306 if(gb==MMAP_METHOD){
00307 if(buf.bytesused <= HEADERFRAME1) {
00308
00309 printf("Ignoring empty buffer ...\n");
00310 return 0;
00311 }
00312
00313 if(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG){
00314 memcpy(tmpbuffer, buffersmmap[buf.index],buf.bytesused);
00315 int w = camwidth;
00316 int h = camheight;
00317 if (jpeg_decode(&framebuffer, tmpbuffer, &w, &h) < 0)
00318 return 0;
00319 }
00320 else{
00321 memcpy(framebuffer, buffersmmap[buf.index], (this->camwidth * this->camheight << 1));
00322 }
00323 }else{
00324 read (filedescriptor, buffersread[0].start, buffersread[0].length);
00325 }
00326
00327 if(ioctl(filedescriptor, VIDIOC_QBUF, &buf) < 0)
00328 throw ControllerException("Unable to queue buffer","getImage");
00329
00330
00331 if(gb==MMAP_METHOD){
00332 yuyv_to_rgb24(this->camwidth,this->camheight,framebuffer,outputbuffer);
00333 return outputbuffer;
00334 }else
00335 return (unsigned char *)buffersread[0].start;
00336
00337 }
00338
00339 size_t V4L2Webcam::width(void) const
00340 {
00341 return this->camwidth;
00342 }
00343
00344 size_t V4L2Webcam::height(void) const
00345 {
00346 return this->camheight;
00347 }
00348
00349 bool V4L2Webcam::isControlSupported(const WebcamControl& control)
00350 {
00351 queryctrl.id = control;
00352 if (ioctl(this->filedescriptor, VIDIOC_QUERYCTRL, &queryctrl) < 0) {
00353 std::cerr << "ioctl get control error" << std::endl;
00354 perror("");
00355 return false;
00356 }
00357 if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
00358 std::cerr << "Control " << (char*)queryctrl.name << " is disable" << std::endl;
00359 perror("");
00360 return false;
00361 }
00362 if (queryctrl.flags & V4L2_CTRL_TYPE_BOOLEAN)
00363 return true;
00364
00365 if (queryctrl.type & V4L2_CTRL_TYPE_INTEGER)
00366 return true;
00367
00368 std::cerr << "Control " << (char*)queryctrl.name << " is not supported" << std::endl;
00369 return false;
00370 }
00371
00372 bool V4L2Webcam::isControlSupportedQuery(const WebcamControl& control, struct v4l2_queryctrl* queryctrl)
00373 {
00374 queryctrl->id = control;
00375 if (ioctl(this->filedescriptor, VIDIOC_QUERYCTRL, queryctrl) < 0) {
00376 std::cerr << "ioctl get control error" << std::endl;
00377 perror("");
00378 return false;
00379 }
00380 if (queryctrl->flags & V4L2_CTRL_FLAG_DISABLED) {
00381 std::cerr << "Control " << (char*)queryctrl->name << " is disable" << std::endl;
00382 return false;
00383 }
00384 if (queryctrl->flags & V4L2_CTRL_TYPE_BOOLEAN)
00385 return true;
00386
00387 if (queryctrl->type & V4L2_CTRL_TYPE_INTEGER)
00388 return true;
00389
00390 std::cerr << "Control " << (char*)queryctrl->name << " is not supported" << std::endl;
00391 return false;
00392 }
00393
00394 int V4L2Webcam::getControlValue(const WebcamControl& control)
00395 {
00396 if (!isControlSupported(control))
00397 return -1;
00398 control_s.id = control;
00399 if (ioctl(this->filedescriptor, VIDIOC_G_CTRL, &control_s) < 0) {
00400 std::cerr << "ioctl get control error" << std::endl;
00401 perror(" ");
00402 return -1;
00403 }
00404 return control_s.value;
00405 }
00406
00407 bool V4L2Webcam::resetControlValue(const WebcamControl& control)
00408 {
00409 int val_def;
00410 if (isControlSupportedQuery(control,&queryctrl)){
00411 val_def = queryctrl.default_value;
00412 control_s.id = control;
00413 control_s.value = val_def;
00414 if (ioctl(this->filedescriptor, VIDIOC_S_CTRL, &control_s) < 0) {
00415 std::cerr << "resetControlValue : ioctl set control error" << std::endl;
00416 return false;;
00417 }
00418 return true;
00419 }
00420
00421 return false;
00422 }
00423
00424
00425
00426 int V4L2Webcam::getControlMinValue(const WebcamControl& control)
00427 {
00428 if (isControlSupportedQuery(control,&queryctrl)){
00429 return queryctrl.minimum;
00430 }
00431 return -1;
00432 }
00433
00434 int V4L2Webcam::getControlMaxValue(const WebcamControl& control)
00435 {
00436 if (isControlSupportedQuery(control,&queryctrl)){
00437 return queryctrl.maximum;
00438 }
00439 return -1;
00440 }
00441
00442 int V4L2Webcam::getControlStepValue(const WebcamControl& control)
00443 {
00444 if (isControlSupportedQuery(control,&queryctrl)){
00445 return queryctrl.step;
00446 }
00447 return -1;
00448 }
00449
00450 bool V4L2Webcam::setControlValue(const WebcamControl& control, const int& value)
00451 {
00452 if (isControlSupportedQuery(control,&queryctrl)){
00453 int min, max, step;
00454 min = queryctrl.minimum;
00455 max = queryctrl.maximum;
00456 step = queryctrl.step;
00457
00458 if ((value >= min) && (value <= max) && value%step==0) {
00459 control_s.id = control;
00460 control_s.value = value;
00461 if (ioctl(this->filedescriptor, VIDIOC_S_CTRL, &control_s) < 0) {
00462 std::cerr << "setControlValue : ioctl set control error" << std::endl;
00463 perror("");
00464 return false;
00465 }
00466 return true;
00467 }
00468 }
00469
00470 return false;
00471 }
00472
00473 bool V4L2Webcam::setControlUpValue(const WebcamControl& control)
00474 {
00475 if (isControlSupportedQuery(control,&queryctrl)){
00476 int min, max, step, value;
00477 min = queryctrl.minimum;
00478 max = queryctrl.maximum;
00479 step = queryctrl.step;
00480
00481 control_s.id = control;
00482 if (ioctl(this->filedescriptor, VIDIOC_G_CTRL, &control_s) < 0) {
00483 std::cerr << "ioctl get control error" << std::endl;
00484 return -1;
00485 }
00486 value = control_s.value + step;
00487
00488 if ((value >= min) && (value <= max)) {
00489 control_s.id = control;
00490 control_s.value = value;
00491 if (ioctl(this->filedescriptor, VIDIOC_S_CTRL, &control_s) < 0) {
00492 std::cerr << "setControlValue : ioctl set control error" << std::endl;
00493 return false;
00494 }
00495 return true;
00496 }
00497 }
00498
00499 return false;
00500 }
00501
00502 bool V4L2Webcam::setControlDownValue(const WebcamControl& control)
00503 {
00504 if (isControlSupportedQuery(control,&queryctrl)){
00505 int min, max, step, value;
00506 min = queryctrl.minimum;
00507 max = queryctrl.maximum;
00508 step = queryctrl.step;
00509
00510 control_s.id = control;
00511 if (ioctl(this->filedescriptor, VIDIOC_G_CTRL, &control_s) < 0) {
00512 std::cerr << "ioctl get control error" << std::endl;
00513 return -1;
00514 }
00515 value = control_s.value - step;
00516
00517 if ((value >= min) && (value <= max)) {
00518 control_s.id = control;
00519 control_s.value = value;
00520 if (ioctl(this->filedescriptor, VIDIOC_S_CTRL, &control_s) < 0) {
00521 std::cerr << "setControlValue : ioctl set control error" << std::endl;
00522 return false;
00523 }
00524 return true;
00525 }
00526 }
00527
00528 return false;
00529 }
00530
00531 bool V4L2Webcam::toggleControlValue(const WebcamControl& control, const WebcamToggleControl& value)
00532 {
00533 if (!isControlSupported(control)){
00534 return false;
00535 }
00536 bzero(&control_s,sizeof(struct v4l2_control));
00537 control_s.id = control;
00538 control_s.value = value;
00539 if (ioctl(this->filedescriptor, VIDIOC_S_CTRL, &control_s) < 0) {
00540 std::cerr << "setControlValue : ioctl set control error" << std::endl;
00541 perror(" ");
00542 return false;
00543 }
00544
00545 return true;
00546 }
00547
00548
00549
00550
00551
00552 }
