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

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

V4l modifs
File size: 17.3 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,1024*1024),
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 dQueuedBuffer.index=-1;//mark current buffer not valid for initialisation
66 useMemoryUsrPtr=false;
67 if(useMemoryUsrPtr) {
68 AllocUserBuffers();
69 } else {
70 AllocV4LBuffers();
71 }
72
73 for (int i=0;i < nbBuffers;i++) {
74 QueueBuffer(i);
75 }
76
77 /* enable the streaming */
78 v4l2_buf_type type;
79 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
80 if (-1 == xioctl (device, VIDIOC_STREAMON,&type)) {
81 Thread::Err("VIDIOC_STREAMON error\n");
82 }
83
84 // ground station
85 gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
86 exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,1, 0.1);
87 bright = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
88 contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,1, 0.1);
89 hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
90 sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:", 0, 1, 0.1);
91 sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,0.1);
92 autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:");
93 autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:");
94 awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:");
95 fps = new Label(GetGroupBox()->NewRow(), "fps");
96
97 hasProblems=false;
98}
99
100V4LCamera::~V4LCamera() {
101 if(useMemoryUsrPtr) {
102 for (int i = 0; i < nbBuffers; i++) {
103 FreeFunction((char*)buffers[i]);
104 }
105 } else {
106 FreeFunction(imageData);
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::AllocV4LBuffers() {
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::AllocUserBuffers(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::GrabFrame(void) {
337 //queue previous buffer
338 if(QueueBuffer(bufferIndex)<0) return -1;
339
340 fd_set fds;
341 struct timeval tv;
342 FD_ZERO (&fds);
343 FD_SET (device, &fds);
344
345 tv.tv_sec = 0;
346 tv.tv_usec = 100000;
347
348 int r = select (device+1, &fds, NULL, NULL, &tv);
349
350 if (-1 == r) {
351 char errorMsg[256];
352 Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
353 return -1;
354 }
355
356 if (0 == r) {
357 Thread::Err("select timeout\n");
358 return -1;
359 }
360
361 struct v4l2_buffer buf;
362 memset(&buf, 0, sizeof (v4l2_buffer));
363 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
364 buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
365
366 //get last captured image
367 int prevDQbuf=-1;
368 for(int i=0;i<nbBuffers;i++) {
369 if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
370 if (errno==EAGAIN) {
371 break;
372 } else {
373 Thread::Err("VIDIOC_DQBUF xioctl\n");
374 return -1;
375 }
376 } else {
377 if(prevDQbuf!=-1) {
378 QueueBuffer(prevDQbuf);
379 }
380 for (int i=0; i<nbBuffers; i++) {
381 if((void*)(buf.m.userptr)==buffers[i]) {
382 prevDQbuf=i;
383 bufferIndex=i;
384 break;
385 }
386 }
387 }
388 }
389
390 return 1;
391}
392*/
393int V4LCamera::cvGrabFrame(void) {
394 if(QueueBuffer(&dQueuedBuffer)<0) return -1;
395
396 fd_set fds;
397 struct timeval tv;
398 FD_ZERO (&fds);
399 FD_SET (device, &fds);
400
401 tv.tv_sec = 0;
402 tv.tv_usec = 100000;
403
404 int r=select(device+1, &fds, NULL, NULL, &tv);
405
406 if (r==-1) {
407 char errorMsg[256];
408 Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
409 return -1;
410 }
411
412 if (r==0) {
413 Thread::Err("select timeout\n");
414 return -1;
415 }
416
417 struct v4l2_buffer buf;
418 memset(&buf, 0, sizeof (v4l2_buffer));
419 buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
420 if(useMemoryUsrPtr) {
421 buf.memory=V4L2_MEMORY_USERPTR;
422 } else {
423 buf.memory=V4L2_MEMORY_MMAP;
424 }
425
426 //get last captured image
427 dQueuedBuffer.index=-1;
428 for(int i=0;i<nbBuffers;i++) {
429 if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
430 if (errno==EAGAIN) {//when no new buffer is available for reading (non blocking)
431 //Printf("%s eagain\n",Thread::ObjectName().c_str());
432 break;
433 } else {
434 Thread::Err("VIDIOC_DQBUF xioctl\n");
435 return -1;
436 }
437 } else {
438 //Printf("%s %i dqueued\n",Thread::ObjectName().c_str(),buf.index);
439 //Printf("%i %x %i %i\n",buf.bytesused,buf.flags,buf.field,buf.sequence);
440 if(dQueuedBuffer.index!=-1) {
441 //Printf("%s %i queued\n",Thread::ObjectName().c_str(),dQueuedBuffer.index);
442 QueueBuffer(&dQueuedBuffer);
443 }
444 dQueuedBuffer=buf;
445 }
446 }
447 //Printf("%s %i\n",Thread::ObjectName().c_str(),dQueuedBuffer.index);
448
449 return 1;
450}
451
452void V4LCamera::Run(void) {
453 Time cam_time, new_time, fpsNow, fpsPrev;
454 int fpsCounter = 0;
455
456 cam_time = GetTime();
457 fpsPrev = cam_time;
458
459 while (!ToBeStopped()) {
460 // fps counter
461 fpsCounter++;
462 if (fpsCounter == 100) {
463 fpsNow = GetTime();
464 fps->SetText("fps: %.1f",
465 fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
466 fpsCounter = 0;
467 fpsPrev = fpsNow;
468 }
469
470 // cam properties
471 if (gain->ValueChanged() == true && autogain->Value() == false)
472 SetGain(gain->Value());
473 if (exposure->ValueChanged() == true && autoexposure->Value() == false)
474 SetExposure(exposure->Value());
475 if (bright->ValueChanged() == true)
476 SetBrightness(bright->Value());
477 if (sat->ValueChanged() == true)
478 SetSaturation(sat->Value());
479 if (contrast->ValueChanged() == true)
480 SetContrast(contrast->Value());
481 if (hue->ValueChanged() == true)
482 SetHue(hue->Value());
483 //if (sharpness->ValueChanged() == true)
484 // cvSetCaptureProperty(capture, CV_CAP_PROP_SHARPNESS, sharpness->Value());
485 if (autogain->ValueChanged() == true) {
486 if (autogain->Value() == true) {
487 gain->setEnabled(false);
488 } else {
489 gain->setEnabled(true);
490 SetGain(gain->Value());
491 }
492 SetAutoGain(autogain->Value());
493 }
494 if (autoexposure->ValueChanged() == true) {
495 if (autoexposure->Value() == true) {
496 exposure->setEnabled(false);
497 } else {
498 exposure->setEnabled(true);
499 SetExposure(exposure->Value());
500 }
501 SetAutoExposure(autoexposure->Value());
502 }
503 //if (awb->ValueChanged() == true)
504 // cvSetCaptureProperty(capture, CV_CAP_PROP_AWB, awb->Value());
505
506 // cam pictures
507 if (!cvGrabFrame()) {
508 Printf("Could not grab a frame\n");
509 }
510
511 //copy to CMEM allocated buffer if necessary
512 if(!useMemoryUsrPtr) {
513 memcpy(imageData,(char *)buffers[dQueuedBuffer.index],output->GetDataType().GetSize());
514 }
515
516 //check for ps3eye deconnection in hds uav
517 new_time = GetTime();
518 if(new_time-cam_time>100*1000*1000) {
519 Thread::Warn("delta t trop grand\n");
520 hasProblems=true;
521 }/*
522 if(hasProblems==false) {
523 struct v4l2_format form;
524 form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
525 xioctl(device, VIDIOC_G_FMT,&form);
526 if(xioctl (device, VIDIOC_G_FMT,&form)<0) {
527 Thread::Warn("camera disconnected\n");
528 hasProblems=true;
529 }
530 }*/
531
532 output->GetMutex();
533 output->buffer=imageData;
534 output->ReleaseMutex();
535
536 output->SetDataTime(cam_time);
537 ProcessUpdate(output);
538 cam_time = new_time;
539 }
540}
541
542int V4LCamera::QueueBuffer(struct v4l2_buffer *buf) {
543 if(buf->index>=0 && buf->index<nbBuffers) {
544 int ret=xioctl (device, VIDIOC_QBUF,buf);
545 if (ret==-1) {
546 Thread::Err("VIDIOC_QBUF xioctl %s\n",strerror(-ret));
547 return -1;
548 }
549 }
550 return 0;
551}
552
553int V4LCamera::QueueBuffer(int index) {
554 struct v4l2_buffer buf;
555 memset(&buf, 0, sizeof (v4l2_buffer));
556
557 buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
558 buf.index=(unsigned long)index;
559 if(useMemoryUsrPtr) {
560 buf.memory=V4L2_MEMORY_USERPTR;
561 buf.m.userptr=(unsigned long)(buffers[index]);
562 buf.length=output->GetDataType().GetSize();
563 } else {
564 buf.memory=V4L2_MEMORY_MMAP;
565 }
566 return QueueBuffer(&buf);
567}
568
569bool V4LCamera::HasProblems(void) {
570 return hasProblems;
571}
572
573void V4LCamera::SetAutoGain(bool value) {
574 SetProperty(V4L2_CID_AUTOGAIN, value);
575}
576
577void V4LCamera::SetAutoExposure(bool value) {
578 Thread::Warn("not implemented\n");
579}
580
581void V4LCamera::SetGain(float value) {
582 SetProperty(V4L2_CID_GAIN, value);
583}
584
585void V4LCamera::SetExposure(float value) {
586 SetProperty(V4L2_CID_EXPOSURE, value);
587}
588
589void V4LCamera::SetBrightness(float value) {
590 SetProperty(V4L2_CID_BRIGHTNESS, value);
591}
592
593void V4LCamera::SetSaturation(float value) {
594 SetProperty(V4L2_CID_SATURATION, value);
595}
596
597void V4LCamera::SetHue(float value) {
598 SetProperty(V4L2_CID_HUE, value);
599}
600
601void V4LCamera::SetContrast(float value) {
602 SetProperty(V4L2_CID_CONTRAST, value);
603}
604
605float V4LCamera::GetProperty(int property) {
606 //get min and max value
607 struct v4l2_queryctrl queryctrl;
608 queryctrl.id = property;
609 if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) return -1;
610 int min = queryctrl.minimum;
611 int max = queryctrl.maximum;
612
613 //set value
614 struct v4l2_control control;
615 memset (&control, 0, sizeof (v4l2_control));
616 control.id = property;
617 if(xioctl (device,VIDIOC_G_CTRL, &control)==-1) return -1;
618
619 return ((float)control.value - min + 1) / (max - min);
620}
621
622void V4LCamera::SetProperty(int property,float value) {
623 //get min and max value
624 struct v4l2_queryctrl queryctrl;
625 queryctrl.id = property;
626 if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) {
627 Thread::Warn("prop %x, VIDIOC_QUERYCTRL failed\n",property);
628 }
629 int min = queryctrl.minimum;
630 int max = queryctrl.maximum;
631
632 //set value
633 struct v4l2_control control;
634 memset (&control, 0, sizeof (v4l2_control));
635 control.id = property;
636 control.value = (int)(value * (max - min) + min);
637 if(xioctl (device,VIDIOC_S_CTRL, &control)==-1) {
638 Thread::Warn("prop %x, VIDIOC_S_CTRL failed\n",property);
639 }
640}
641
642int V4LCamera::xioctl( int fd, int request, void *arg) {
643 int r;
644
645 do r = ioctl (fd, request, arg);
646 while (-1 == r && EINTR == errno);
647
648 return r;
649}
650
651} // end namespace sensor
652} // end namespace flair
Note: See TracBrowser for help on using the repository browser.