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, 6 months 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.