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

Last change on this file since 403 was 403, checked in by Sanahuja Guillaume, 6 months ago

select user memory pointer (v4l) if using custom alloc function in images (ie if initdsp was called)

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