Changeset 348 in flair-src for trunk/lib/FlairSensorActuator/src/V4LCamera.cpp

Timestamp:

Feb 17, 2020, 3:44:56 PM (5 years ago)

Author:

Sanahuja Guillaume

Message:

update v4l cam, ok on gcs

File:

• trunk/lib/FlairSensorActuator/src/V4LCamera.cpp (modified) (15 diffs)

trunk/lib/FlairSensorActuator/src/V4LCamera.cpp

@@ -33 +33 @@
 #define DEFAULT_V4L_BUFFERS 4
 
-#define CLEAR(x) memset (&(x), 0, sizeof (x))
-
 using std::string;
 using namespace flair::core;
@@ -55 +53 @@
   }
 
-  /* w/o memset some parts  arent initialized - AKA: Fill it with zeros so it is clean */
-  memset(&capture,0,sizeof(CvCaptureCAM_V4L));
-  n_buffers = 0;
-
-  
   if(format == Image::Type::Format::UYVY) {
     if(init(width,height,V4L2_PIX_FMT_UYVY) == -1) {
@@ -77 +70 @@
   allocBuffers();
 
-  for (capture.bufferIndex = 0;capture.bufferIndex < ((int)capture.req.count);++capture.bufferIndex) {
+  for (int i=0;i < nbBuffers;i++) {
     struct v4l2_buffer buf;
-
-    CLEAR (buf);
+    memset(&buf, 0, sizeof (v4l2_buffer));
 
     buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     buf.memory      = V4L2_MEMORY_MMAP;
-    buf.index       = (unsigned long)capture.bufferIndex;
+    buf.index       = (unsigned long)i;
 
     if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
@@ -92 +84 @@
 
   /* enable the streaming */
-  capture.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-  if (-1 == xioctl (device, VIDIOC_STREAMON,&capture.type)) {
-    /* error enabling the stream */
+  v4l2_buf_type type;
+  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  if (-1 == xioctl (device, VIDIOC_STREAMON,&type)) {
     Thread::Err("VIDIOC_STREAMON error\n");
   }
@@ -115 +107 @@
 
 V4LCamera::~V4LCamera() {
-  for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) {
-       //FreeFunction((char*)buffers[n_buffers].start);
+  for (int i = 0; i < nbBuffers; i++) {
+       //FreeFunction((char*)buffers[i].start);
   }
   SafeStop();
   Join();
+  close(device);
 }
 
 int V4LCamera::init(int width, int height,unsigned long colorspace) {
-  CLEAR (capture.cap);
-  if(-1 == xioctl(device, VIDIOC_QUERYCAP, &capture.cap)) {
+  struct v4l2_capability cap;
+  memset(&cap, 0, sizeof (v4l2_capability));
+  
+  if(-1 == xioctl(device, VIDIOC_QUERYCAP, &cap)) {
     return -1;
-  } else {
-    CLEAR (capture.capability);
-    capture.capability.type = capture.cap.capabilities;
-
-    /* Query channels number */
-    if (-1 == xioctl(device, VIDIOC_G_INPUT, &capture.capability.channels)) {
-      return -1;
-    }
   }
      
-  if ((capture.cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
+  if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
     Thread::Err("device is unable to capture video memory.\n");
     return -1;
   }
 
-  /* Find Window info */
-  CLEAR (capture.form);
-  capture.form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-      
-  if (-1 == xioctl (device, VIDIOC_G_FMT, &capture.form)) {
-     Thread::Err("Could not obtain specifics of capture window.\n");
-     return -1;
+  struct v4l2_format form;
+  memset(&form, 0, sizeof (v4l2_format));
+  form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+  /* read the current setting */
+  if(-1 == xioctl(device, VIDIOC_G_FMT, &form)) {
+   Thread::Err("Could not obtain specifics of capture window.\n");
+   return -1;
+  }
+
+  /* set the values we want to change */
+  form.fmt.pix.width = width; 
+  form.fmt.pix.height = height;
+  form.fmt.win.chromakey = 0;
+  form.fmt.win.field = V4L2_FIELD_ANY;
+  form.fmt.win.clips = 0;
+  form.fmt.win.clipcount = 0;
+  form.fmt.pix.field = V4L2_FIELD_ANY;
+  form.fmt.pix.pixelformat = colorspace;
+
+  /* ask the device to change the size*/
+  if(-1 == xioctl (device, VIDIOC_S_FMT, &form)) {
+    Thread::Err("Could not set specifics of capture window.\n");
+    return -1;
+  }
+
+  /* Get window info again, to get the real value */
+  if(-1 == xioctl (device, VIDIOC_G_FMT, &form)) {
+    Thread::Err("Could not obtain specifics of capture window.\n");
+    return -1;
   }
   
-  setVideoSize(width, height,colorspace);
-
-  unsigned int min;
-
-  /* Buggy driver paranoia. */
-  min = capture.form.fmt.pix.width * 2;
-  if (capture.form.fmt.pix.bytesperline < min) {
-   capture.form.fmt.pix.bytesperline = min;
-  }
-
-  min = capture.form.fmt.pix.bytesperline * capture.form.fmt.pix.height;
-  if (capture.form.fmt.pix.sizeimage < min) {
-   capture.form.fmt.pix.sizeimage = min;
-  }
-
-  return 1;
-}
-
-int V4LCamera::setVideoSize(int width, int height,unsigned long colorspace) {
-
-    CLEAR (capture.crop);
-    capture.crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    capture.crop.c.left       = 0; 
-    capture.crop.c.top        = 0; 
-    capture.crop.c.height     = height*24;
-    capture.crop.c.width      = width*24;
-
-    /* set the crop area, but don't exit if the device don't support croping */
-    xioctl (device, VIDIOC_S_CROP, &capture.crop);
-
-    CLEAR (capture.form);
-    capture.form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-    /* read the current setting */
-    xioctl (device, VIDIOC_G_FMT, &capture.form);
-
-    /* set the values we want to change */
-    capture.form.fmt.pix.width = width; 
-    capture.form.fmt.pix.height = height;
-    capture.form.fmt.win.chromakey = 0;
-    capture.form.fmt.win.field = V4L2_FIELD_ANY;
-    capture.form.fmt.win.clips = 0;
-    capture.form.fmt.win.clipcount = 0;
-    capture.form.fmt.pix.field = V4L2_FIELD_ANY;
-    capture.form.fmt.pix.pixelformat = colorspace;
-
-    /* ask the device to change the size*/
-    if (-1 == xioctl (device, VIDIOC_S_FMT, &capture.form)) {
-      Thread::Err("Could not set specifics of capture window.\n");
-      return -1;
-    }
-
-    /* Get window info again, to get the real value */
-    if (-1 == xioctl (device, VIDIOC_G_FMT, &capture.form)) {
-      Thread::Err("Could not obtain specifics of capture window.\n");
-      return -1;
-    }
-    return 0;
-
+  return 0;
 }
 
 /*
-V4LCamera::V4LCamera(string name,
-                     uint8_t camera_index, uint16_t width, uint16_t height,
-                     Image::Type::Format format, uint8_t priority)
-    : Thread(getFrameworkManager(), name, priority),
-      Camera(name, width, height, format) {
-  
-    string deviceName="/dev/video"+std::to_string(camera_index);
-    device = open(deviceName.c_str(), O_RDWR | O_NONBLOCK);
-    if (device == -1) {
-        Thread::Err("Cannot open %s\n",deviceName.c_str());
-    } else {
-        Printf("V4LCamera %s, opened %s\n",name.c_str(),deviceName.c_str());
-    }
-
-    struct v4l2_capability cap;
-    memset(&cap, 0, sizeof (v4l2_capability));
-    if (xioctl (device, VIDIOC_QUERYCAP, &cap)==-1) {
-        Thread::Err("VIDIOC_QUERYCAP xioctl\n");
-    }
-    if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
-        Thread::Err("device is unable to capture video memory.\n");
-    }
-
-    //get v4l2_format
-    struct v4l2_format form;
-    form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    if(xioctl (device, VIDIOC_G_FMT,&form)==-1) {
-        Thread::Err("VIDIOC_G_FMT xioctl\n");
-    }
-  
-    //set width, height and format
-    if (format == Image::Type::Format::UYVY) {
-        form.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY; 
-    } else if (format == Image::Type::Format::YUYV) {
-        form.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; 
-    } else {
-        Thread::Err("format not supported\n");
-    }
-  
-    form.fmt.pix.width = width; 
-    form.fmt.pix.height = height;
-    form.fmt.win.chromakey = 0;
-    form.fmt.win.field = V4L2_FIELD_ANY;
-    form.fmt.win.clips = 0;
-    form.fmt.win.clipcount = 0;
-    form.fmt.pix.field = V4L2_FIELD_ANY;
-    if(xioctl (device, VIDIOC_S_FMT,&form)==-1) {
-        Thread::Err("VIDIOC_S_FMT xioctl\n");
-    }
-  
-    //alloc and queue bufs
-    AllocBuffers();
-    for (int bufferIndex = 0; bufferIndex < nbBuffers;++bufferIndex) {
-        QueueBuffer(bufferIndex);
-    }
-    
-    // enable the streaming
-    v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    if (xioctl (device, VIDIOC_STREAMON,&type)==-1) {
-        Thread::Err("VIDIOC_STREAMON xioctl\n");
-    }
-
-    // skip first frame. it is often bad -- this is unnotied in traditional apps,
-    //  but could be fatal if bad jpeg is enabled
-    bufferIndex=-1;
-    GrabFrame();
-
-    // ground station
-    gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
-    exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,1, 0.1);
-    bright = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
-    contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,1, 0.1);
-    hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
-    sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:", 0, 1, 0.1);
-    sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,0.1);
-    autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:");
-    autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:");
-    awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:");
-    fps = new Label(GetGroupBox()->NewRow(), "fps");
-
-    hasProblems=false;
-}
-
-V4LCamera::~V4LCamera() {
-    for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) {
-       FreeFunction((char*)buffers[n_buffers].start);
-   }
-    SafeStop();
-    Join();
-}
-
 void V4LCamera::Run(void) {
   Time cam_time, new_time, fpsNow, fpsPrev;
@@ -395 +256 @@
 */
 int V4LCamera::allocBuffers() {
-   CLEAR (capture.req);
-   
-   unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
-
-   try_again:
-   
-   capture.req.count = buffer_number;
-   capture.req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-   capture.req.memory = V4L2_MEMORY_MMAP;
-
-   if (-1 == xioctl (device, VIDIOC_REQBUFS, &capture.req)) {
-       if (EINVAL == errno) {
-         Thread::Warn("does not support memory mapping\n");
-       } else {
-         perror ("VIDIOC_REQBUFS");
-       }
-       /* free capture, and returns an error code */
-       return -1;
-   }
-
-   if (capture.req.count < buffer_number) {
-       if (buffer_number == 1) {
-           Thread::Warn("Insufficient buffer memory\n");
-           /* free capture, and returns an error code */
-           return -1;
-       } else {
-          buffer_number--;
-          Thread::Warn("Insufficient buffer memory -- decreaseing buffers\n");
-          goto try_again;
-       }
-   }
-
-   for (n_buffers = 0; n_buffers < capture.req.count; ++n_buffers) {
-       struct v4l2_buffer buf;
-       CLEAR (buf);
-
-       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-       buf.memory = V4L2_MEMORY_MMAP;
-       buf.index = n_buffers;
-
-       if (-1 == xioctl (device, VIDIOC_QUERYBUF, &buf)) {
-           Thread::Warn ("VIDIOC_QUERYBUF error\n");
-           /* free capture, and returns an error code */
-           return -1;
-       }
-
-       capture.buffers[n_buffers].length = buf.length;
-       capture.buffers[n_buffers].start =
-         mmap (NULL /* start anywhere */,
-               buf.length,
-               PROT_READ | PROT_WRITE /* required */,
-               MAP_SHARED /* recommended */,
-               device, buf.m.offset);
-
-       if (MAP_FAILED == capture.buffers[n_buffers].start) {
-           Thread::Warn("mmap error\n");
-           /* free capture, and returns an error code */
-           return -1;
-       }
-   }
-
-   /* Set up Image data */
-   /*
-   cvInitImageHeader( &capture.frame,
-                      cvSize( capture.captureWindow.width,
-                              capture.captureWindow.height ),
-                      IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );*/
-   /* Allocate space for RGBA data */
-   capture.imageSize=capture.form.fmt.pix.width*capture.form.fmt.pix.height*2;
-   capture.imageData = AllocFunction(capture.imageSize);
-   Printf("cmem allocated %i at %x\n",capture.imageSize,capture.imageData);
-   
-   return 1;
+  struct v4l2_requestbuffers req;
+  memset(&req, 0, sizeof (v4l2_requestbuffers));
+  nbBuffers=DEFAULT_V4L_BUFFERS;
+
+  try_again:
+
+  req.count = nbBuffers;
+  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  req.memory = V4L2_MEMORY_MMAP;
+
+  if(-1 == xioctl(device, VIDIOC_REQBUFS, &req)) {
+    if (EINVAL == errno) {
+      Thread::Err("camera does not support memory mapping\n");
+    } else {
+      Thread::Err("VIDIOC_REQBUFS failed\n");
+    }
+    return -1;
+  }
+
+  if(req.count < nbBuffers) {
+    if (nbBuffers == 1) {
+      Thread::Err("Insufficient buffer memory\n");
+      return -1;
+    } else {
+      nbBuffers--;
+      Thread::Warn("Insufficient buffer memory -- decreaseing buffers to %i\n",nbBuffers);
+      goto try_again;
+    }
+  }
+
+  for(int i=0; i<req.count; i++) {
+    struct v4l2_buffer buf;
+    memset(&buf, 0, sizeof (v4l2_buffer));
+
+    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    buf.memory = V4L2_MEMORY_MMAP;
+    buf.index = i;
+
+    if(-1 == xioctl(device, VIDIOC_QUERYBUF, &buf)) {
+      Thread::Err("VIDIOC_QUERYBUF error\n");
+      return -1;
+    }
+
+    if(output->GetDataType().GetSize()!=buf.length) {
+      Thread::Err("buf size is not as exepcted %i/%i\n",buf.length,output->GetDataType().GetSize());
+      return -1;
+    }
+
+    buffers[i]=mmap(NULL,buf.length,PROT_READ | PROT_WRITE,MAP_SHARED,device, buf.m.offset);
+
+    if(MAP_FAILED == buffers[i]) {
+      Thread::Err("mmap error\n");
+      return -1;
+    }
+  }
+
+  //allocate output data
+  imageData = AllocFunction(output->GetDataType().GetSize());
+  Printf("cmem allocated %i at %x\n",output->GetDataType().GetSize(),imageData);
+
+  return 1;
 };
 
+int V4LCamera::AllocBuffers(void) {
+  struct v4l2_requestbuffers requestbuffers;
+  memset(&requestbuffers, 0, sizeof (v4l2_requestbuffers));
+
+  unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
+
+  try_again:
+
+  requestbuffers.count = buffer_number;
+  requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  requestbuffers.memory = V4L2_MEMORY_USERPTR;
+
+  if (xioctl (device, VIDIOC_REQBUFS, &requestbuffers)==-1) {
+    if (errno==EINVAL) {
+      Thread::Err("VIDIOC_REQBUFS user memory not supported\n");
+    } else {
+      Thread::Err ("VIDIOC_REQBUFS xioctl\n");
+    }
+    return -1;
+  }
+
+  nbBuffers=DEFAULT_V4L_BUFFERS;
+  for (int i=0; i<nbBuffers; i++) {
+    buffers[i] =AllocFunction(output->GetDataType().GetSize());
+  }
+
+  return 1;
+};
 
 int V4LCamera::cvGrabFrame(void) {
@@ -505 +382 @@
             }
 
-            if (read_frame_v4l2 (&capture))
+            if (read_frame_v4l2 ())
                 break;
         }
@@ -512 +389 @@
 }
 
-int V4LCamera::read_frame_v4l2(CvCaptureCAM_V4L* capture) {
+int V4LCamera::read_frame_v4l2(void) {
     struct v4l2_buffer buf;
-
-    CLEAR (buf);
+    memset(&buf, 0, sizeof (v4l2_buffer));
 
     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -537 +413 @@
    }
 
-   if(buf.index >= capture->req.count) {
+   if(buf.index >= nbBuffers) {
      Thread::Err("buf.index >= capture->req.count\n");
    }
-   
-
-   capture->bufferIndex = buf.index;
-
+
+   bufferIndex = buf.index;
 
    if (-1 == xioctl (device, VIDIOC_QBUF, &buf))
@@ -611 +485 @@
    
     // cam pictures
-    cvRetrieveRawFrame(&capture);
+    cvRetrieveRawFrame();
     if (!cvGrabFrame()) {
       Printf("Could not grab a frame\n");
@@ -625 +499 @@
     output->GetMutex();
     
-    output->buffer=capture.imageData;
+    output->buffer=imageData;
     output->ReleaseMutex();
    
@@ -632 +506 @@
     cam_time = new_time;
   }
-
-  //cvReleaseCapture(&capture);
-}
-
-void V4LCamera::cvRetrieveRawFrame( CvCaptureCAM_V4L* capture) {
-
-
-
-
-   /* Now get what has already been captured as a IplImage return */
-
-   /* First, reallocate imageData if the frame size changed */
-
-
-/*
-    if(((unsigned long)capture->frame.width != capture->form.fmt.pix.width)
-       || ((unsigned long)capture->frame.height != capture->form.fmt.pix.height)) {
-           if (PALETTE_YUYV == 1 || PALETTE_UYVY == 1)
-           {
-                cvFree(&capture->frame.imageData);
-                cvInitImageHeader( &capture->frame,
-                cvSize( capture->form.fmt.pix.width,
-                capture->form.fmt.pix.height ),
-                IPL_DEPTH_8U,2, IPL_ORIGIN_TL, 4 );
-                capture->frame.imageData = (char *)cvAlloc(capture->frame.imageSize);
-           }else
-           {
-              fprintf( stderr,
-                 "HIGHGUI ERROR: V4L: raw output not supported for this palette\n");
-           }
-
-    }
-
- */
-
-
-    
-  memcpy((char *)capture->imageData,(char *)capture->buffers[capture->bufferIndex].start,capture->imageSize);
-    
- 
-
-   
-}
+}
+
+void V4LCamera::cvRetrieveRawFrame(void) {
+  memcpy(imageData,(char *)buffers[bufferIndex],output->GetDataType().GetSize());
+}
+
 int V4LCamera::QueueBuffer(int index) {
     struct v4l2_buffer buf;
@@ -682 +519 @@
         buf.memory      = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
         buf.index       = (unsigned long)index;
-        buf.m.userptr=(unsigned long)(buffers[index].start);
-        buf.length=buffers[index].length;
+        buf.m.userptr=(unsigned long)(buffers[index]);
+        buf.length=output->GetDataType().GetSize();
         
         int ret=xioctl (device, VIDIOC_QBUF, &buf);
@@ -738 +575 @@
                QueueBuffer(prevDQbuf);
            }
-           for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) {
-               if((void*)(buf.m.userptr)==buffers[n_buffers].start) {
-                   prevDQbuf=n_buffers;
-                   bufferIndex=n_buffers;
+           for (int i=0; i<nbBuffers; i++) {
+               if((void*)(buf.m.userptr)==buffers[i]) {
+                   prevDQbuf=i;
+                   bufferIndex=i;
                    break;
                }
@@ -750 +587 @@
    return 1;
 }
-
-int V4LCamera::AllocBuffers(void) {
-    struct v4l2_requestbuffers requestbuffers;
-   memset(&requestbuffers, 0, sizeof (v4l2_requestbuffers));
-   
-   unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
-
-   try_again:
-   
-   requestbuffers.count = buffer_number;
-   requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-   requestbuffers.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
-
-   if (xioctl (device, VIDIOC_REQBUFS, &requestbuffers)==-1) {
-       if (errno==EINVAL) {
-         Thread::Err("VIDIOC_REQBUFS user memory not supported\n");
-       } else {
-         Thread::Err ("VIDIOC_REQBUFS xioctl\n");
-       }
-       return -1;
-   }
-   
-   nbBuffers=DEFAULT_V4L_BUFFERS;
-   for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) {
-       buffers[n_buffers].length = output->GetDataType().GetSize();
-       buffers[n_buffers].start =AllocFunction(output->GetDataType().GetSize());
-   }
-
-   return 1;
-};
 
 bool V4LCamera::HasProblems(void) {