source: flair-src/trunk/lib/FlairSensorActuator/src/V4LCamera_impl.cpp@ 401

Last change on this file since 401 was 401, checked in by Sanahuja Guillaume, 2 years ago

resolve bug when not using hdsvisionfilter but using cameras on dm3730

File size: 14.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_impl.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_impl.h"
19#include "V4LCamera.h"
20#include <GroupBox.h>
21#include <DoubleSpinBox.h>
22#include <CheckBox.h>
23#include <Label.h>
24#include <Image.h>
25#include <FrameworkManager.h>
26#include <fcntl.h>
27#include <sys/ioctl.h>
28#include <unistd.h>
29#include <cstring>
30#include <sys/mman.h>
31
32using std::string;
33using namespace flair::core;
34using namespace flair::gui;
35using namespace flair::sensor;
36
37
38V4LCamera_impl::V4LCamera_impl(V4LCamera *self,string name,uint8_t camera_index, uint16_t width, uint16_t height,
39 Image::Type::Format format) {
40 this->self=self;
41
42 string deviceName="/dev/video"+std::to_string(camera_index);
43 device = open(deviceName.c_str(), O_RDWR | O_NONBLOCK);
44 if (device == -1) {
45 ((Thread*)self)->Err("Cannot open %s\n",deviceName.c_str());
46 } else {
47 Printf("V4LCamera %s, opened %s\n",name.c_str(),deviceName.c_str());
48 }
49
50 if(format == Image::Type::Format::UYVY) {
51 if(Init(width,height,V4L2_PIX_FMT_UYVY) == -1) {
52 ((Thread*)self)->Err("initialisation failed\n");
53 }
54 } else if (format == Image::Type::Format::YUYV) {
55 if(Init(width,height,V4L2_PIX_FMT_YUYV) == -1) {
56 ((Thread*)self)->Err("initialisation failed\n");
57 }
58 } else {
59 ((Thread*)self)->Err("format not supported\n");
60 }
61
62 //todo: better handling of detection, only neeeded for omap/dm
63 //also in run, copy from v4l to cmem is not necessary if not using cmem...
64#ifdef ARMV7A
65 useMemoryUsrPtr=true;
66#else
67 useMemoryUsrPtr=false;
68#endif
69 if(useMemoryUsrPtr) {
70 AllocUserBuffers();
71 } else {
72 AllocV4LBuffers();
73 }
74
75 for (int i=0;i < nbBuffers;i++) {
76 QueueBuffer(i);
77 }
78
79 /* enable the streaming */
80 v4l2_buf_type type;
81 type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
82 if (-1 == xioctl (device, VIDIOC_STREAMON,&type)) {
83 ((Thread*)self)->Err("VIDIOC_STREAMON error\n");
84 }
85
86 // ground station
87 gain = new DoubleSpinBox(self->GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
88 exposure = new DoubleSpinBox(self->GetGroupBox()->LastRowLastCol(), "exposure:", 0,1, 0.1);
89 bright = new DoubleSpinBox(self->GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
90 contrast = new DoubleSpinBox(self->GetGroupBox()->LastRowLastCol(), "contrast:", 0,1, 0.1);
91 hue = new DoubleSpinBox(self->GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
92 sharpness = new DoubleSpinBox(self->GetGroupBox()->LastRowLastCol(), "sharpness:", 0, 1, 0.1);
93 sat = new DoubleSpinBox(self->GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,0.1);
94 autogain = new CheckBox(self->GetGroupBox()->NewRow(), "autogain:");
95 autoexposure = new CheckBox(self->GetGroupBox()->LastRowLastCol(), "autoexposure:");
96 awb = new CheckBox(self->GetGroupBox()->LastRowLastCol(), "awb:");
97 fps = new Label(self->GetGroupBox()->NewRow(), "fps");
98
99 hasProblems=false;
100}
101
102V4LCamera_impl::~V4LCamera_impl() {
103 if(useMemoryUsrPtr) {
104 for (int i = 0; i < nbBuffers; i++) {
105 Image::FreeFunction((char*)buffers[i]);
106 }
107 } else {
108 Image::FreeFunction(imageData);
109 }
110 close(device);
111}
112
113int V4LCamera_impl::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*)self)->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*)self)->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*)self)->Err("Could not set specifics of capture window.\n");
149 return -1;
150 }
151
152 return 0;
153}
154
155int V4LCamera_impl::AllocV4LBuffers() {
156 struct v4l2_requestbuffers req;
157 memset(&req, 0, sizeof (v4l2_requestbuffers));
158 nbBuffers=DEFAULT_V4L_BUFFERS;
159
160 try_again:
161
162 req.count = nbBuffers;
163 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
164 req.memory = V4L2_MEMORY_MMAP;
165
166 if(-1 == xioctl(device, VIDIOC_REQBUFS, &req)) {
167 if (EINVAL == errno) {
168 ((Thread*)self)->Err("camera does not support memory mapping\n");
169 } else {
170 ((Thread*)self)->Err("VIDIOC_REQBUFS failed\n");
171 }
172 return -1;
173 }
174
175 if(req.count < nbBuffers) {
176 if (nbBuffers == 1) {
177 ((Thread*)self)->Err("Insufficient buffer memory\n");
178 return -1;
179 } else {
180 nbBuffers--;
181 ((Thread*)self)->Warn("Insufficient buffer memory -- decreaseing buffers to %i\n",nbBuffers);
182 goto try_again;
183 }
184 }
185
186 for(int i=0; i<req.count; i++) {
187 struct v4l2_buffer buf;
188 memset(&buf, 0, sizeof (v4l2_buffer));
189
190 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
191 buf.memory = V4L2_MEMORY_MMAP;
192 buf.index = i;
193
194 if(-1 == xioctl(device, VIDIOC_QUERYBUF, &buf)) {
195 ((Thread*)self)->Err("VIDIOC_QUERYBUF error\n");
196 return -1;
197 }
198
199 if(self->output->GetDataType().GetSize()!=buf.length) {
200 ((Thread*)self)->Err("buf size is not as exepcted %i/%i\n",buf.length,self->output->GetDataType().GetSize());
201 return -1;
202 }
203
204 buffers[i]=mmap(NULL,buf.length,PROT_READ | PROT_WRITE,MAP_SHARED,device, buf.m.offset);
205
206 if(MAP_FAILED == buffers[i]) {
207 ((Thread*)self)->Err("mmap error\n");
208 return -1;
209 }
210 }
211
212 //allocate output data
213 imageData = Image::AllocFunction(self->output->GetDataType().GetSize());
214 //Printf("cmem allocated %i at %x\n",output->GetDataType().GetSize(),imageData);
215
216 return 1;
217};
218
219int V4LCamera_impl::AllocUserBuffers(void) {
220 struct v4l2_requestbuffers req;
221 memset(&req, 0, sizeof (v4l2_requestbuffers));
222 nbBuffers=DEFAULT_V4L_BUFFERS;
223
224 req.count = nbBuffers;
225 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
226 req.memory = V4L2_MEMORY_USERPTR;
227
228 if (xioctl (device, VIDIOC_REQBUFS, &req)==-1) {
229 if (errno==EINVAL) {
230 ((Thread*)self)->Err("VIDIOC_REQBUFS user memory not supported\n");
231 } else {
232 ((Thread*)self)->Err ("VIDIOC_REQBUFS xioctl\n");
233 }
234 return -1;
235 }
236
237 for (int i=0; i<nbBuffers; i++) {
238 buffers[i] =Image::AllocFunction(self->output->GetDataType().GetSize());
239 }
240
241 return 1;
242};
243
244int V4LCamera_impl::GrabFrame(void) {
245 fd_set fds;
246 struct timeval tv;
247 FD_ZERO (&fds);
248 FD_SET (device, &fds);
249
250 tv.tv_sec = 0;
251 tv.tv_usec = 100000;
252
253 int r=select(device+1, &fds, NULL, NULL, &tv);
254
255 if (r==-1) {
256 char errorMsg[256];
257 ((Thread*)self)->Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
258 return -1;
259 }
260
261 if (r==0) {
262 ((Thread*)self)->Err("select timeout\n");
263 return -1;
264 }
265
266 struct v4l2_buffer buf;
267 memset(&buf, 0, sizeof (v4l2_buffer));
268 buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
269 if(useMemoryUsrPtr) {
270 buf.memory=V4L2_MEMORY_USERPTR;
271 } else {
272 buf.memory=V4L2_MEMORY_MMAP;
273 }
274
275 //get last captured buffer
276 dQueuedBuffer.index=-1;
277 for(int i=0;i<nbBuffers;i++) {
278 if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
279 if (errno==EAGAIN) {//when no new buffer is available for reading (non blocking)
280 //Printf("%s eagain\n",((Thread*)self)->ObjectName().c_str());
281 break;
282 } else {
283 ((Thread*)self)->Err("VIDIOC_DQBUF xioctl\n");
284 return -1;
285 }
286 } else {
287 //Printf("%s %i dqueued\n",((Thread*)self)->ObjectName().c_str(),buf.index);
288 //Printf("%i %x %i %i\n",buf.bytesused,buf.flags,buf.field,buf.sequence);
289 if(dQueuedBuffer.index!=-1) {
290 //Printf("%s %i queued\n",((Thread*)self)->ObjectName().c_str(),dQueuedBuffer.index);
291 QueueBuffer(&dQueuedBuffer);
292 }
293 dQueuedBuffer=buf;
294 }
295 }
296 //Printf("%s %i\n",((Thread*)self)->ObjectName().c_str(),dQueuedBuffer.index);
297
298 return 1;
299}
300
301void V4LCamera_impl::Run(void) {
302 Time cam_time, new_time, fpsNow, fpsPrev;
303 int fpsCounter = 0;
304
305 cam_time = GetTime();
306 fpsPrev = cam_time;
307
308 while (!self->ToBeStopped()) {
309 // fps counter
310 fpsCounter++;
311 if (fpsCounter == 100) {
312 fpsNow = GetTime();
313 fps->SetText("fps: %.1f",
314 fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
315 fpsCounter = 0;
316 fpsPrev = fpsNow;
317 }
318
319 // cam properties
320 if (gain->ValueChanged() == true && autogain->Value() == false)
321 self->SetGain(gain->Value());
322 if (exposure->ValueChanged() == true && autoexposure->Value() == false)
323 self->SetExposure(exposure->Value());
324 if (bright->ValueChanged() == true)
325 self->SetBrightness(bright->Value());
326 if (sat->ValueChanged() == true)
327 self->SetSaturation(sat->Value());
328 if (contrast->ValueChanged() == true)
329 self->SetContrast(contrast->Value());
330 if (hue->ValueChanged() == true)
331 self->SetHue(hue->Value());
332 //if (sharpness->ValueChanged() == true)
333 // cvSetCaptureProperty(capture, CV_CAP_PROP_SHARPNESS, sharpness->Value());
334 if (autogain->ValueChanged() == true) {
335 self->SetAutoGain(autogain->Value());
336 if (autogain->Value() == true) {
337 gain->setEnabled(false);
338 } else {
339 gain->setEnabled(true);
340 self->SetGain(gain->Value());
341 }
342 }
343 if (autoexposure->ValueChanged() == true) {
344 self->SetAutoExposure(autoexposure->Value());
345 if (autoexposure->Value() == true) {
346 exposure->setEnabled(false);
347 } else {
348 exposure->setEnabled(true);
349 self->SetExposure(exposure->Value());
350 }
351 }
352 //if (awb->ValueChanged() == true)
353 // cvSetCaptureProperty(capture, CV_CAP_PROP_AWB, awb->Value());
354
355 // cam pictures
356 if (!GrabFrame()) {
357 Printf("Could not grab a frame\n");
358 }
359
360 //check for ps3eye deconnection in hds uav
361 new_time = GetTime();
362 if(new_time-cam_time>100*1000*1000) {
363 ((Thread*)self)->Warn("delta t trop grand\n");
364 hasProblems=true;
365 }/*
366 if(hasProblems==false) {
367 struct v4l2_format form;
368 form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
369 xioctl(device, VIDIOC_G_FMT,&form);
370 if(xioctl (device, VIDIOC_G_FMT,&form)<0) {
371 ((Thread*)self)->Warn("camera disconnected\n");
372 hasProblems=true;
373 }
374 }*/
375
376 //select buffer
377 if(useMemoryUsrPtr) {//buffers are already in CMEM
378 imageData=(char*)buffers[dQueuedBuffer.index];
379 //dequeue it latter (buffer used by ProcessUpdate)
380 } else {//copy to CMEM allocated buffer
381 memcpy(imageData,(char *)buffers[dQueuedBuffer.index],self->output->GetDataType().GetSize());
382 QueueBuffer(&dQueuedBuffer);//we can do it right now
383 }
384
385 self->output->GetMutex();
386 self->output->buffer=imageData;
387 self->output->ReleaseMutex();
388
389 self->output->SetDataTime(cam_time);
390 self->ProcessUpdate(self->output);
391 cam_time = new_time;
392
393 if(useMemoryUsrPtr) {
394 QueueBuffer(&dQueuedBuffer);//now it is possible to dequeue
395 }
396 }
397}
398
399int V4LCamera_impl::QueueBuffer(struct v4l2_buffer *buf) {
400 if(buf->index>=0 && buf->index<nbBuffers) {
401 int ret=xioctl (device, VIDIOC_QBUF,buf);
402 if (ret==-1) {
403 ((Thread*)self)->Err("VIDIOC_QBUF xioctl %s\n",strerror(-ret));
404 return -1;
405 }
406 }
407 return 0;
408}
409
410int V4LCamera_impl::QueueBuffer(int index) {
411 struct v4l2_buffer buf;
412 memset(&buf, 0, sizeof (v4l2_buffer));
413
414 buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
415 buf.index=(unsigned long)index;
416 if(useMemoryUsrPtr) {
417 buf.memory=V4L2_MEMORY_USERPTR;
418 buf.m.userptr=(unsigned long)(buffers[index]);
419 buf.length=self->output->GetDataType().GetSize();
420 } else {
421 buf.memory=V4L2_MEMORY_MMAP;
422 }
423 return QueueBuffer(&buf);
424}
425
426void V4LCamera_impl::SetAutoGain(bool value) {
427 SetProperty(V4L2_CID_AUTOGAIN, value);
428}
429
430void V4LCamera_impl::SetAutoExposure(bool value) {
431 ((Thread*)self)->Warn("not implemented\n");
432}
433
434void V4LCamera_impl::SetGain(float value) {
435 SetProperty(V4L2_CID_GAIN, value);
436}
437
438void V4LCamera_impl::SetExposure(float value) {
439 SetProperty(V4L2_CID_EXPOSURE, value);
440}
441
442void V4LCamera_impl::SetBrightness(float value) {
443 SetProperty(V4L2_CID_BRIGHTNESS, value);
444}
445
446void V4LCamera_impl::SetSaturation(float value) {
447 SetProperty(V4L2_CID_SATURATION, value);
448}
449
450void V4LCamera_impl::SetHue(float value) {
451 SetProperty(V4L2_CID_HUE, value);
452}
453
454void V4LCamera_impl::SetContrast(float value) {
455 SetProperty(V4L2_CID_CONTRAST, value);
456}
457
458float V4LCamera_impl::GetProperty(int property) {
459 //get min and max value
460 struct v4l2_queryctrl queryctrl;
461 queryctrl.id = property;
462 if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) return -1;
463 int min = queryctrl.minimum;
464 int max = queryctrl.maximum;
465
466 //set value
467 struct v4l2_control control;
468 memset (&control, 0, sizeof (v4l2_control));
469 control.id = property;
470 if(xioctl (device,VIDIOC_G_CTRL, &control)==-1) return -1;
471
472 return ((float)control.value - min + 1) / (max - min);
473}
474
475void V4LCamera_impl::SetProperty(int property,float value) {
476 //get min and max value
477 struct v4l2_queryctrl queryctrl;
478 queryctrl.id = property;
479 if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) {
480 ((Thread*)self)->Warn("prop %x, VIDIOC_QUERYCTRL failed\n",property);
481 }
482 int min = queryctrl.minimum;
483 int max = queryctrl.maximum;
484
485 //set value
486 struct v4l2_control control;
487 memset (&control, 0, sizeof (v4l2_control));
488 control.id = property;
489 control.value = (int)(value * (max - min) + min);
490 if(xioctl (device,VIDIOC_S_CTRL, &control)==-1) {
491 ((Thread*)self)->Warn("prop %x, VIDIOC_S_CTRL failed\n",property);
492 }
493}
494
495int V4LCamera_impl::xioctl( int fd, int request, void *arg) {
496 int r;
497
498 do r = ioctl (fd, request, arg);
499 while (-1 == r && EINTR == errno);
500
501 return r;
502}
Note: See TracBrowser for help on using the repository browser.