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

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

use less bandwidth in vprnlite

File size: 13.0 KB
RevLine 
[3]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[3]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 <cvimage.h>
24#include <FrameworkManager.h>
[330]25#include <fcntl.h>
[3]26#include <linux/videodev2.h>
27
[330]28#include <sys/ioctl.h>
29#include <unistd.h>
30#include <cstring>
31#include <sys/mman.h>
32
33
34#define DEFAULT_V4L_BUFFERS 4
35
[3]36using std::string;
37using namespace flair::core;
38using namespace flair::gui;
39
[15]40namespace flair {
41namespace sensor {
[3]42
[137]43V4LCamera::V4LCamera(string name,
[15]44                     uint8_t camera_index, uint16_t width, uint16_t height,
45                     cvimage::Type::Format format, uint8_t priority)
[137]46    : Thread(getFrameworkManager(), name, priority),
47      Camera(name, width, height, format) {
[330]48 
49  string deviceName="/dev/video"+std::to_string(camera_index);
50  device = open(deviceName.c_str(), O_RDWR | O_NONBLOCK);
51  if (device == -1) {
52     Thread::Err("Cannot open %s\n");
53  }
54   
55  //get v4l2_format
56  struct v4l2_format form;
57  form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
58  xioctl (device, VIDIOC_G_FMT,&form);
59 
60  //set width, height and format
[15]61  if (format == cvimage::Type::Format::UYVY) {
[330]62    form.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY; 
[15]63  } else if (format == cvimage::Type::Format::YUYV) {
[330]64    form.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; 
[15]65  } else {
66    Thread::Err("format not supported\n");
67  }
[330]68 
69  form.fmt.pix.width = width; 
70  form.fmt.pix.height = height;
71  form.fmt.win.chromakey = 0;
72  form.fmt.win.field = V4L2_FIELD_ANY;
73  form.fmt.win.clips = 0;
74  form.fmt.win.clipcount = 0;
75  form.fmt.pix.field = V4L2_FIELD_ANY;
76  xioctl (device, VIDIOC_S_FMT, &form);
77 
78  /* This is just a technicality, but all buffers must be filled up before any
79   staggered SYNC is applied.  SO, filler up. (see V4L HowTo) */
[3]80
[330]81  AllocBuffers();
82
83  for (int bufferIndex = 0; bufferIndex < ((int)requestbuffers.count);++bufferIndex) {
84    struct v4l2_buffer buf;
85
86    memset(&buf, 0, sizeof (v4l2_buffer));
87
88    buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
89    buf.memory      = V4L2_MEMORY_MMAP;
90    buf.index       = (unsigned long)bufferIndex;
91
92    if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
93        Thread::Err("VIDIOC_QBUF xioctl\n");
94        break;
95    }
96  }
97
98  // enable the streaming
99  v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
100  if (-1 == xioctl (device, VIDIOC_STREAMON,&type)) {
101      Thread::Err("VIDIOC_STREAMON xioctl\n");
102  }
103
104
105  // skip first frame. it is often bad -- this is unnotied in traditional apps,
106  //  but could be fatal if bad jpeg is enabled
107  GrabFrame();
108   
109   
110   
[15]111  // station sol
112  gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
113  exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,
114                               1, 0.1);
115  bright =
116      new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
117  contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,
118                               1, 0.1);
119  hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
120  sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:",
121                                0, 1, 0.1);
122  sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,
123                          0.1);
124  autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:");
125  autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:");
126  awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:");
127  fps = new Label(GetGroupBox()->NewRow(), "fps");
[165]128 
129  hasProblems=false;
[3]130}
131
[15]132V4LCamera::~V4LCamera() {
133  SafeStop();
134  Join();
[3]135}
136
[15]137void V4LCamera::Run(void) {
138  Time cam_time, new_time, fpsNow, fpsPrev;
[330]139  char* buffer; // raw image
[15]140  int fpsCounter = 0;
[3]141
[15]142  // init image old
[330]143  GrabFrame();
[15]144  cam_time = GetTime();
145  fpsPrev = cam_time;
146
147  while (!ToBeStopped()) {
[165]148    //check for ps3eye deconnection in hds uav
[330]149    if(hasProblems==false) {
150      struct v4l2_format form;
151      form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
152      xioctl(device, VIDIOC_G_FMT,&form);
153      if(xioctl (device, VIDIOC_G_FMT,&form)<0) {
154        Thread::Warn("camera disconnected\n");
155        hasProblems=true;
156      }
[165]157    }
158   
[15]159    // fps counter
160    fpsCounter++;
[307]161    if (GetTime() > (fpsPrev + 5 * (Time)1000000000)) {
162      // toute les 5 secondes
[15]163      fpsNow = GetTime();
164      fps->SetText("fps: %.1f",
[307]165      fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
[15]166      fpsCounter = 0;
167      fpsPrev = fpsNow;
[307]168    } 
169   
[15]170    // cam properties
171    if (gain->ValueChanged() == true && autogain->Value() == false)
172      SetGain(gain->Value());
173    if (exposure->ValueChanged() == true && autoexposure->Value() == false)
174      SetExposure(exposure->Value());
175    if (bright->ValueChanged() == true)
176      SetBrightness(bright->Value());
177    if (sat->ValueChanged() == true)
178      SetSaturation(sat->Value());
179    if (contrast->ValueChanged() == true)
180      SetContrast(contrast->Value());
181    if (hue->ValueChanged() == true)
182      SetHue(hue->Value());
183    if (sharpness->ValueChanged() == true)
[330]184      SetProperty(V4L2_CID_SHARPNESS, sharpness->Value());
[15]185    if (autogain->ValueChanged() == true) {
186      if (autogain->Value() == true) {
187        gain->setEnabled(false);
188      } else {
189        gain->setEnabled(true);
190        SetGain(gain->Value());
191      }
192      SetAutoGain(autogain->Value());
193    }
194    if (autoexposure->ValueChanged() == true) {
195      if (autoexposure->Value() == true) {
196        exposure->setEnabled(false);
197      } else {
198        exposure->setEnabled(true);
199        SetExposure(exposure->Value());
200      }
201      SetAutoExposure(autoexposure->Value());
202    }
203    if (awb->ValueChanged() == true)
[330]204      SetProperty(V4L2_CID_AUTO_WHITE_BALANCE, awb->Value());
[3]205
[15]206    // cam pictures
[330]207    buffer = RetrieveRawFrame();
208    GrabFrame();
[15]209    new_time = GetTime();
[165]210   
211    //check for ps3eye deconnection in hds uav
212    if(new_time-cam_time>100*1000*1000) {
213      Thread::Warn("delta trop grand\n");
214      hasProblems=true;
215    }
[3]216
[15]217    output->GetMutex();
[330]218    output->buffer = buffer;
[15]219    output->ReleaseMutex();
[3]220
[15]221    output->SetDataTime(cam_time);
222    ProcessUpdate(output);
[3]223
[15]224    cam_time = new_time;
225  }
[3]226
[330]227  close(device);
[3]228}
229
[330]230void V4LCamera::GrabFrame(void) {
231    unsigned int count;
232
233    count = 1;
234
235    while (count-- > 0) {
236        for (;;) {
237            fd_set fds;
238            struct timeval tv;
239            int r;
240
241            FD_ZERO (&fds);
242            FD_SET (device, &fds);
243
244            /* Timeout. */
245            tv.tv_sec = 2;
246            tv.tv_usec = 0;
247
248            r = select (device+1, &fds, NULL, NULL, &tv);
249
250            if (-1 == r) {
251                if (EINTR == errno) continue;
252                Thread::Err("select\n");
253            }
254
255            if (0 == r) {
256                Thread::Err("select timeout\n");
257                /* end the infinite loop */
258                break;
259            }
260
261            if (read_frame_v4l2 ()) break;
262        }
263    }
264}
265
266int V4LCamera::read_frame_v4l2(void) {
267  struct v4l2_buffer buf;
268  memset(&buf, 0, sizeof (v4l2_buffer));
269 
270
271    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
272    buf.memory = V4L2_MEMORY_MMAP;
273
274    if (-1 == xioctl (device, VIDIOC_DQBUF, &buf)) {
275        switch (errno) {
276        case EAGAIN:
277            return 0;
278
279        case EIO:
280            /* Could ignore EIO, see spec. */
281
282            /* fall through */
283
284        default:
285            /* display the error and stop processing */
286            Thread::Err("VIDIOC_DQBUF xioctl\n");
287            return 1;
288        }
289   }
290
291   if(buf.index >= requestbuffers.count) {
292     Thread::Err("buf.index >= requestbuffers.count\n");
293   }
294   
295#ifdef USE_TEMP_BUFFER
296   memcpy(capture->buffers[MAX_V4L_BUFFERS].start, 
297          capture->buffers[buf.index].start, 
298          capture->buffers[MAX_V4L_BUFFERS].length );
299   capture->bufferIndex = MAX_V4L_BUFFERS;
300   //printf("got data in buff %d, len=%d, flags=0x%X, seq=%d, used=%d)\n",
301   //     buf.index, buf.length, buf.flags, buf.sequence, buf.bytesused);
302#else
303   bufferIndex = buf.index;
304#endif
305
306   if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
307       Thread::Err ("VIDIOC_QBUF xioctl\n");
308   }
309
310   return 1;
311}
312
313int V4LCamera::AllocBuffers(void) {
314   memset(&requestbuffers, 0, sizeof (v4l2_requestbuffers));
315   
316   unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
317
318   try_again:
319   
320   requestbuffers.count = buffer_number;
321   requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
322   requestbuffers.memory = V4L2_MEMORY_MMAP;
323
324   if (-1 == xioctl (device, VIDIOC_REQBUFS, &requestbuffers)) {
325       if (EINVAL == errno) {
326         Thread::Err("not support memory mapping not supportted\n");
327       } else {
328         Thread::Err ("VIDIOC_REQBUFS xioctl\n");
329       }
330       return -1;
331   }
332
333   if (requestbuffers.count < buffer_number) {
334       if (buffer_number == 1) {
335        Thread::Err("Insufficient buffer memory\n");
336        return -1;
337       } else {
338        buffer_number--;
339        Thread::Warn ("Insufficient buffer memory, decreasing buffers\n");
340        goto try_again;
341       }
342   }
343
344   for (int n_buffers = 0; n_buffers < requestbuffers.count; ++n_buffers) {
345       struct v4l2_buffer buf;
346
347       memset(&buf, 0, sizeof (v4l2_buffer));
348
349       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
350       buf.memory = V4L2_MEMORY_MMAP;
351       buf.index = n_buffers;
352
353       if (-1 == xioctl (device, VIDIOC_QUERYBUF, &buf)) {
354           Thread::Err("VIDIOC_QUERYBUF xioctl\n");
355           return -1;
356       }
357
358       buffers[n_buffers].length = buf.length;
359       buffers[n_buffers].start =
360         mmap (NULL /* start anywhere */,
361               buf.length,
362               PROT_READ | PROT_WRITE /* required */,
363               MAP_SHARED /* recommended */,
364               device, buf.m.offset);
365
366       if (MAP_FAILED == buffers[n_buffers].start) {
367           Thread::Err("mmap\n");
368           return -1;
369       }
370   }
371
372  //todo: verifier cette alloc, pas de double buffeinrg?
373  //peut on initialiser l'image dans le constrcteur de la camera?
374   
375   output->buffer=output->allocFunction(output->dataType.GetSize());
376   return 1;
377};
378
379char *V4LCamera::RetrieveRawFrame(void) {
380 
381  /* [FD] this really belongs here */
382  if (ioctl(device, VIDIOCSYNC, &mmaps[bufferIndex].frame) == -1) {
383    Thread::Err("Could not SYNC to video stream. %s\n", strerror(errno));
384  }
385
386  /* Now get what has already been captured as a IplImage return */
387  if (output->dataType.GetFormat() == cvimage::Type::Format::YUYV || output->dataType.GetFormat() == cvimage::Type::Format::UYVY) {
388    #ifdef USE_TEMP_BUFFER
389    capture->frame.imageData=(char*)capture->buffers[capture->bufferIndex].start;
390    #else
391Printf("frame is not allocated\n");
392    memcpy((char *)frame,(char *)buffers[bufferIndex].start,output->GetDataType().GetSize());
393    #endif
394  } else {
395    Thread::Err("palette %d not supported for raw output\n",output->dataType.GetFormat());
396  }
397
398  return(frame);
399}
400
[165]401bool V4LCamera::HasProblems(void) {
402  return hasProblems;
403}
404
[15]405void V4LCamera::SetAutoGain(bool value) {
[330]406  SetProperty(V4L2_CID_AUTOGAIN, value);
[3]407}
408
[15]409void V4LCamera::SetAutoExposure(bool value) {
410  Thread::Warn("not implemented in opencv\n");
[3]411}
412
[15]413void V4LCamera::SetGain(float value) {
[330]414  SetProperty(V4L2_CID_GAIN, value);
[3]415}
416
[15]417void V4LCamera::SetExposure(float value) {
[330]418  SetProperty(V4L2_CID_EXPOSURE, value);
[3]419}
420
[15]421void V4LCamera::SetBrightness(float value) {
[330]422  SetProperty(V4L2_CID_BRIGHTNESS, value);
[3]423}
424
[15]425void V4LCamera::SetSaturation(float value) {
[330]426  SetProperty(V4L2_CID_SATURATION, value);
[3]427}
428
[15]429void V4LCamera::SetHue(float value) {
[330]430  SetProperty(V4L2_CID_HUE, value);
[3]431}
432
[15]433void V4LCamera::SetContrast(float value) {
[330]434  SetProperty(V4L2_CID_CONTRAST, value);
[3]435}
436
[330]437float V4LCamera::GetProperty(int property) {
438  //get min and max value
439  struct v4l2_queryctrl queryctrl;
440  queryctrl.id = property;
441  if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) return -1;
442  int min = queryctrl.minimum;
443  int max = queryctrl.maximum;
444 
445  //set value
446  struct v4l2_control control;
447  memset (&control, 0, sizeof (v4l2_control));
448  control.id = property;
449  if(xioctl (device,VIDIOC_G_CTRL, &control)==-1) return -1;
450 
451  return ((float)control.value - min + 1) / (max - min);
452}
453
454void V4LCamera::SetProperty(int property,float value) {
455  //get min and max value
456  struct v4l2_queryctrl queryctrl;
457  queryctrl.id = property;
458  xioctl (device, VIDIOC_QUERYCTRL,&queryctrl);
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  control.value = (int)(value * (max - min) + min);
467  xioctl (device,VIDIOC_S_CTRL, &control);
468}
469
470int V4LCamera::xioctl( int fd, int request, void *arg) {
471  int r;
472
473  do r = ioctl (fd, request, arg);
474  while (-1 == r && EINTR == errno);
475
476  return r;
477}
478
[3]479} // end namespace sensor
480} // end namespace flair
Note: See TracBrowser for help on using the repository browser.