source: flair-src/trunk/lib/FlairSensorActuator/src/V4LCamera.cpp@ 349

Last change on this file since 349 was 349, checked in by Sanahuja Guillaume, 4 years ago

modifs v4l2
File size: 17.1 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5// created: 2014/07/17
6// filename: V4LCamera.cpp
7//
8// author: Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: base class for V4l camera
14//
15//
16/*********************************************************************/
17
18#include "V4LCamera.h"
19#include <GroupBox.h>
20#include <DoubleSpinBox.h>
21#include <CheckBox.h>
22#include <Label.h>
23#include <Image.h>
24#include <FrameworkManager.h>
25#include <fcntl.h>
26#include <linux/videodev2.h>
27#include <sys/ioctl.h>
28#include <unistd.h>
29#include <cstring>
30#include <sys/mman.h>
31#include <VisionFilter.h>
32
33using std::string;
34using namespace flair::core;
35using namespace flair::gui;
36
37namespace flair {
38namespace sensor {
39
40V4LCamera::V4LCamera(string name,uint8_t camera_index, uint16_t width, uint16_t height,
41 Image::Type::Format format, uint8_t priority)
42 : Thread(getFrameworkManager(), name, priority),
43 Camera(name, width, height, format) {
44
45 string deviceName="/dev/video"+std::to_string(camera_index);
46 device = open(deviceName.c_str(), O_RDWR | O_NONBLOCK);
47 if (device == -1) {
48 Thread::Err("Cannot open %s\n",deviceName.c_str());
49 } else {
50 Printf("V4LCamera %s, opened %s\n",name.c_str(),deviceName.c_str());
51 }
52
53 if(format == Image::Type::Format::UYVY) {
54 if(init(width,height,V4L2_PIX_FMT_UYVY) == -1) {
55 Thread::Err("initialisation failed\n");
56 }
57 } else if (format == Image::Type::Format::YUYV) {
58 if(init(width,height,V4L2_PIX_FMT_YUYV) == -1) {
59 Thread::Err("initialisation failed\n");
60 }
61 } else {
62 Thread::Err("format not supported\n");
63 }
64
65
66 allocBuffers();
67
68 for (int i=0;i < nbBuffers;i++) {
69 struct v4l2_buffer buf;
70 memset(&buf, 0, sizeof (v4l2_buffer));
71
72 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
73 buf.memory = V4L2_MEMORY_MMAP;
74 buf.index = (unsigned long)i;
75
76 if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
77 Thread::Err("VIDIOC_QBUF error\n");
78 }
79 }
80
81 /* enable the streaming */
82 v4l2_buf_type type;
83 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
84 if (-1 == xioctl (device, VIDIOC_STREAMON,&type)) {
85 Thread::Err("VIDIOC_STREAMON error\n");
86 }
87
88 // ground station
89 gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
90 exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,1, 0.1);
91 bright = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
92 contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,1, 0.1);
93 hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
94 sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:", 0, 1, 0.1);
95 sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,0.1);
96 autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:");
97 autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:");
98 awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:");
99 fps = new Label(GetGroupBox()->NewRow(), "fps");
100
101 hasProblems=false;
102}
103
104V4LCamera::~V4LCamera() {
105 for (int i = 0; i < nbBuffers; i++) {
106 //FreeFunction((char*)buffers[i].start);
107 }
108 SafeStop();
109 Join();
110 close(device);
111}
112
113int V4LCamera::init(int width, int height,unsigned long colorspace) {
114 struct v4l2_capability cap;
115 memset(&cap, 0, sizeof (v4l2_capability));
116
117 if(-1 == xioctl(device, VIDIOC_QUERYCAP, &cap)) {
118 return -1;
119 }
120
121 if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
122 Thread::Err("device is unable to capture video memory.\n");
123 return -1;
124 }
125
126 struct v4l2_format form;
127 memset(&form, 0, sizeof (v4l2_format));
128 form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
129
130 /* read the current setting */
131 if(-1 == xioctl(device, VIDIOC_G_FMT, &form)) {
132 Thread::Err("Could not obtain specifics of capture window.\n");
133 return -1;
134 }
135
136 /* set the values we want to change */
137 form.fmt.pix.width = width;
138 form.fmt.pix.height = height;
139 form.fmt.win.chromakey = 0;
140 form.fmt.win.field = V4L2_FIELD_ANY;
141 form.fmt.win.clips = 0;
142 form.fmt.win.clipcount = 0;
143 form.fmt.pix.field = V4L2_FIELD_ANY;
144 form.fmt.pix.pixelformat = colorspace;
145
146 /* ask the device to change the size*/
147 if(-1 == xioctl (device, VIDIOC_S_FMT, &form)) {
148 Thread::Err("Could not set specifics of capture window.\n");
149 return -1;
150 }
151
152 return 0;
153}
154
155/*
156void V4LCamera::Run(void) {
157 Time cam_time, new_time, fpsNow, fpsPrev;
158 int fpsCounter = 0;
159
160 // init image old
161 GrabFrame();
162 cam_time = GetTime();
163 fpsPrev = cam_time;
164
165 while (!ToBeStopped()) {
166 //check for ps3eye deconnection in hds uav
167 if(hasProblems==false) {
168 struct v4l2_format form;
169 form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
170 xioctl(device, VIDIOC_G_FMT,&form);
171 if(xioctl (device, VIDIOC_G_FMT,&form)<0) {
172 Thread::Warn("camera disconnected\n");
173 hasProblems=true;
174 }
175 }
176
177 // fps counter
178 fpsCounter++;
179 if (GetTime() > (fpsPrev + 5 * (Time)1000000000)) {
180 // every 5 secondes
181 fpsNow = GetTime();
182 fps->SetText("fps: %.1f",
183 fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
184 fpsCounter = 0;
185 fpsPrev = fpsNow;
186 }
187
188 // cam properties
189 if (gain->ValueChanged() == true && autogain->Value() == false)
190 SetGain(gain->Value());
191 if (exposure->ValueChanged() == true && autoexposure->Value() == false)
192 SetExposure(exposure->Value());
193 if (bright->ValueChanged() == true)
194 SetBrightness(bright->Value());
195 if (sat->ValueChanged() == true)
196 SetSaturation(sat->Value());
197 if (contrast->ValueChanged() == true)
198 SetContrast(contrast->Value());
199 if (hue->ValueChanged() == true)
200 SetHue(hue->Value());
201 if (sharpness->ValueChanged() == true)
202 SetProperty(V4L2_CID_SHARPNESS, sharpness->Value());
203 if (autogain->ValueChanged() == true) {
204 if (autogain->Value() == true) {
205 gain->setEnabled(false);
206 } else {
207 gain->setEnabled(true);
208 SetGain(gain->Value());
209 }
210 SetAutoGain(autogain->Value());
211 }
212 if (autoexposure->ValueChanged() == true) {
213 if (autoexposure->Value() == true) {
214 exposure->setEnabled(false);
215 } else {
216 exposure->setEnabled(true);
217 SetExposure(exposure->Value());
218 }
219 SetAutoExposure(autoexposure->Value());
220 }
221 if (awb->ValueChanged() == true)
222 SetProperty(V4L2_CID_AUTO_WHITE_BALANCE, awb->Value());
223
224 // get picture
225 GrabFrame();
226 new_time = GetTime();
227
228 //check for ps3eye deconnection in hds uav
229 if(new_time-cam_time>100*1000*1000) {
230 Thread::Warn("delta trop grand\n");
231 hasProblems=true;
232 }
233
234 output->GetMutex();
235 output->buffer=(char*)buffers[bufferIndex].start;
236 output->ReleaseMutex();
237
238 output->SetDataTime(cam_time);
239 ProcessUpdate(output);
240
241 cam_time = new_time;
242 }
243
244 close(device);
245}
246*/
247int V4LCamera::allocBuffers() {
248 struct v4l2_requestbuffers req;
249 memset(&req, 0, sizeof (v4l2_requestbuffers));
250 nbBuffers=DEFAULT_V4L_BUFFERS;
251
252 try_again:
253
254 req.count = nbBuffers;
255 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
256 req.memory = V4L2_MEMORY_MMAP;
257
258 if(-1 == xioctl(device, VIDIOC_REQBUFS, &req)) {
259 if (EINVAL == errno) {
260 Thread::Err("camera does not support memory mapping\n");
261 } else {
262 Thread::Err("VIDIOC_REQBUFS failed\n");
263 }
264 return -1;
265 }
266
267 if(req.count < nbBuffers) {
268 if (nbBuffers == 1) {
269 Thread::Err("Insufficient buffer memory\n");
270 return -1;
271 } else {
272 nbBuffers--;
273 Thread::Warn("Insufficient buffer memory -- decreaseing buffers to %i\n",nbBuffers);
274 goto try_again;
275 }
276 }
277
278 for(int i=0; i<req.count; i++) {
279 struct v4l2_buffer buf;
280 memset(&buf, 0, sizeof (v4l2_buffer));
281
282 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
283 buf.memory = V4L2_MEMORY_MMAP;
284 buf.index = i;
285
286 if(-1 == xioctl(device, VIDIOC_QUERYBUF, &buf)) {
287 Thread::Err("VIDIOC_QUERYBUF error\n");
288 return -1;
289 }
290
291 if(output->GetDataType().GetSize()!=buf.length) {
292 Thread::Err("buf size is not as exepcted %i/%i\n",buf.length,output->GetDataType().GetSize());
293 return -1;
294 }
295
296 buffers[i]=mmap(NULL,buf.length,PROT_READ | PROT_WRITE,MAP_SHARED,device, buf.m.offset);
297
298 if(MAP_FAILED == buffers[i]) {
299 Thread::Err("mmap error\n");
300 return -1;
301 }
302 }
303
304 //allocate output data
305 imageData = AllocFunction(output->GetDataType().GetSize());
306 Printf("cmem allocated %i at %x\n",output->GetDataType().GetSize(),imageData);
307
308 return 1;
309};
310
311int V4LCamera::AllocBuffers(void) {
312 struct v4l2_requestbuffers req;
313 memset(&req, 0, sizeof (v4l2_requestbuffers));
314 nbBuffers=DEFAULT_V4L_BUFFERS;
315
316 req.count = nbBuffers;
317 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
318 req.memory = V4L2_MEMORY_USERPTR;
319
320 if (xioctl (device, VIDIOC_REQBUFS, &req)==-1) {
321 if (errno==EINVAL) {
322 Thread::Err("VIDIOC_REQBUFS user memory not supported\n");
323 } else {
324 Thread::Err ("VIDIOC_REQBUFS xioctl\n");
325 }
326 return -1;
327 }
328
329 for (int i=0; i<nbBuffers; i++) {
330 buffers[i] =AllocFunction(output->GetDataType().GetSize());
331 }
332
333 return 1;
334};
335
336int V4LCamera::cvGrabFrame(void) {
337 unsigned int count;
338
339 count = 1;
340
341 while (count-- > 0) {
342 for (;;) {
343 fd_set fds;
344 struct timeval tv;
345 int r;
346
347 FD_ZERO (&fds);
348 FD_SET (device, &fds);
349
350 /* Timeout. */
351 tv.tv_sec = 2;
352 tv.tv_usec = 0;
353
354 r = select (device+1, &fds, NULL, NULL, &tv);
355
356 if (-1 == r) {
357 if (EINTR == errno)
358 continue;
359
360 perror ("select");
361 }
362
363 if (0 == r) {
364 fprintf (stderr, "select timeout\n");
365
366 /* end the infinite loop */
367 break;
368 }
369
370 if (read_frame_v4l2 ())
371 break;
372 }
373 }
374 return(1);
375}
376
377int V4LCamera::read_frame_v4l2(void) {
378 struct v4l2_buffer buf;
379 memset(&buf, 0, sizeof (v4l2_buffer));
380
381 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
382 buf.memory = V4L2_MEMORY_MMAP;
383
384 if (-1 == xioctl (device, VIDIOC_DQBUF, &buf)) {
385 switch (errno) {
386 case EAGAIN:
387 return 0;
388
389 case EIO:
390 /* Could ignore EIO, see spec. */
391
392 /* fall through */
393
394 default:
395 /* display the error and stop processing */
396 perror ("VIDIOC_DQBUF");
397 return 1;
398 }
399 }
400
401 if(buf.index >= nbBuffers) {
402 Thread::Err("buf.index >= capture->req.count\n");
403 }
404
405 bufferIndex = buf.index;
406
407 if (-1 == xioctl (device, VIDIOC_QBUF, &buf))
408 perror ("VIDIOC_QBUF");
409
410 return 1;
411}
412
413void V4LCamera::Run(void) {
414 Time cam_time, new_time, fpsNow, fpsPrev;
415 //IplImage *img; // raw image
416 int fpsCounter = 0;
417
418 // init image old
419 if (!cvGrabFrame()) {
420 Printf("Could not grab a frame\n");
421 }
422 cam_time = GetTime();
423 fpsPrev = cam_time;
424
425 while (!ToBeStopped()) {
426 // fps counter
427 fpsCounter++;
428 if (fpsCounter == 100) {
429 fpsNow = GetTime();
430 fps->SetText("fps: %.1f",
431 fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
432 fpsCounter = 0;
433 fpsPrev = fpsNow;
434 }
435
436 // cam properties
437 if (gain->ValueChanged() == true && autogain->Value() == false)
438 SetGain(gain->Value());
439 if (exposure->ValueChanged() == true && autoexposure->Value() == false)
440 SetExposure(exposure->Value());
441 if (bright->ValueChanged() == true)
442 SetBrightness(bright->Value());
443 if (sat->ValueChanged() == true)
444 SetSaturation(sat->Value());
445 if (contrast->ValueChanged() == true)
446 SetContrast(contrast->Value());
447 if (hue->ValueChanged() == true)
448 SetHue(hue->Value());
449 //if (sharpness->ValueChanged() == true)
450 // cvSetCaptureProperty(capture, CV_CAP_PROP_SHARPNESS, sharpness->Value());
451 if (autogain->ValueChanged() == true) {
452 if (autogain->Value() == true) {
453 gain->setEnabled(false);
454 } else {
455 gain->setEnabled(true);
456 SetGain(gain->Value());
457 }
458 SetAutoGain(autogain->Value());
459 }
460 if (autoexposure->ValueChanged() == true) {
461 if (autoexposure->Value() == true) {
462 exposure->setEnabled(false);
463 } else {
464 exposure->setEnabled(true);
465 SetExposure(exposure->Value());
466 }
467 SetAutoExposure(autoexposure->Value());
468 }
469 //if (awb->ValueChanged() == true)
470 // cvSetCaptureProperty(capture, CV_CAP_PROP_AWB, awb->Value());
471
472 // cam pictures
473 cvRetrieveRawFrame();
474 if (!cvGrabFrame()) {
475 Printf("Could not grab a frame\n");
476 }
477 new_time = GetTime();
478
479 //check for ps3eye deconnection in hds uav
480 if(new_time-cam_time>100*1000*1000) {
481 Thread::Warn("delta trop grand\n");
482 hasProblems=true;
483 }
484
485 output->GetMutex();
486
487 output->buffer=imageData;
488 output->ReleaseMutex();
489
490 output->SetDataTime(cam_time);
491 ProcessUpdate(output);
492 cam_time = new_time;
493 }
494}
495
496void V4LCamera::cvRetrieveRawFrame(void) {
497 memcpy(imageData,(char *)buffers[bufferIndex],output->GetDataType().GetSize());
498}
499
500int V4LCamera::QueueBuffer(int index) {
501 struct v4l2_buffer buf;
502 if(index>=0 && index<nbBuffers) {
503 memset(&buf, 0, sizeof (v4l2_buffer));
504 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
505 buf.memory = V4L2_MEMORY_USERPTR;
506 buf.index = (unsigned long)index;
507 buf.m.userptr=(unsigned long)(buffers[index]);
508 buf.length=output->GetDataType().GetSize();
509
510 int ret=xioctl (device, VIDIOC_QBUF, &buf);
511 if (ret==-1) {
512 Thread::Err("VIDIOC_QBUF xioctl %s\n",strerror(-ret));
513 return -1;
514 }
515 }
516 return 0;
517}
518
519int V4LCamera::GrabFrame(void) {
520 //queue previous buffer
521 if(QueueBuffer(bufferIndex)<0) return -1;
522
523 fd_set fds;
524 struct timeval tv;
525 FD_ZERO (&fds);
526 FD_SET (device, &fds);
527
528 tv.tv_sec = 0;
529 tv.tv_usec = 100000;
530
531 int r = select (device+1, &fds, NULL, NULL, &tv);
532
533 if (-1 == r) {
534 char errorMsg[256];
535 Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
536 return -1;
537 }
538
539 if (0 == r) {
540 Thread::Err("select timeout\n");
541 return -1;
542 }
543
544 struct v4l2_buffer buf;
545 memset(&buf, 0, sizeof (v4l2_buffer));
546 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
547 buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
548
549 //get last captured image
550 int prevDQbuf=-1;
551 for(int i=0;i<4;i++) {
552 if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
553 if (errno==EAGAIN) {
554 break;
555 } else {
556 Thread::Err("VIDIOC_DQBUF xioctl\n");
557 return -1;
558 }
559 } else {
560 if(prevDQbuf!=-1) {
561 QueueBuffer(prevDQbuf);
562 }
563 for (int i=0; i<nbBuffers; i++) {
564 if((void*)(buf.m.userptr)==buffers[i]) {
565 prevDQbuf=i;
566 bufferIndex=i;
567 break;
568 }
569 }
570 }
571 }
572
573 return 1;
574}
575
576bool V4LCamera::HasProblems(void) {
577 return hasProblems;
578}
579
580void V4LCamera::SetAutoGain(bool value) {
581 SetProperty(V4L2_CID_AUTOGAIN, value);
582}
583
584void V4LCamera::SetAutoExposure(bool value) {
585 Thread::Warn("not implemented\n");
586}
587
588void V4LCamera::SetGain(float value) {
589 SetProperty(V4L2_CID_GAIN, value);
590}
591
592void V4LCamera::SetExposure(float value) {
593 SetProperty(V4L2_CID_EXPOSURE, value);
594}
595
596void V4LCamera::SetBrightness(float value) {
597 SetProperty(V4L2_CID_BRIGHTNESS, value);
598}
599
600void V4LCamera::SetSaturation(float value) {
601 SetProperty(V4L2_CID_SATURATION, value);
602}
603
604void V4LCamera::SetHue(float value) {
605 SetProperty(V4L2_CID_HUE, value);
606}
607
608void V4LCamera::SetContrast(float value) {
609 SetProperty(V4L2_CID_CONTRAST, value);
610}
611
612float V4LCamera::GetProperty(int property) {
613 //get min and max value
614 struct v4l2_queryctrl queryctrl;
615 queryctrl.id = property;
616 if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) return -1;
617 int min = queryctrl.minimum;
618 int max = queryctrl.maximum;
619
620 //set value
621 struct v4l2_control control;
622 memset (&control, 0, sizeof (v4l2_control));
623 control.id = property;
624 if(xioctl (device,VIDIOC_G_CTRL, &control)==-1) return -1;
625
626 return ((float)control.value - min + 1) / (max - min);
627}
628
629void V4LCamera::SetProperty(int property,float value) {
630 //get min and max value
631 struct v4l2_queryctrl queryctrl;
632 queryctrl.id = property;
633 if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) {
634 Thread::Warn("prop %x, VIDIOC_QUERYCTRL failed\n",property);
635 }
636 int min = queryctrl.minimum;
637 int max = queryctrl.maximum;
638
639 //set value
640 struct v4l2_control control;
641 memset (&control, 0, sizeof (v4l2_control));
642 control.id = property;
643 control.value = (int)(value * (max - min) + min);
644 if(xioctl (device,VIDIOC_S_CTRL, &control)==-1) {
645 Thread::Warn("prop %x, VIDIOC_S_CTRL failed\n",property);
646 }
647}
648
649int V4LCamera::xioctl( int fd, int request, void *arg) {
650 int r;
651
652 do r = ioctl (fd, request, arg);
653 while (-1 == r && EINTR == errno);
654
655 return r;
656}
657
658} // end namespace sensor
659} // end namespace flair
Note: See TracBrowser for help on using the repository browser.