Index: trunk/lib/FlairSensorActuator/src/V4LCamera.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/V4LCamera.cpp	(revision 330)
+++ trunk/lib/FlairSensorActuator/src/V4LCamera.cpp	(revision 338)
@@ -21,14 +21,13 @@
 #include <CheckBox.h>
 #include <Label.h>
-#include <cvimage.h>
+#include <Image.h>
 #include <FrameworkManager.h>
 #include <fcntl.h>
 #include <linux/videodev2.h>
-
 #include <sys/ioctl.h>
 #include <unistd.h>
 #include <cstring>
 #include <sys/mman.h>
-
+#include <VisionFilter.h>
 
 #define DEFAULT_V4L_BUFFERS 4
@@ -43,99 +42,95 @@
 V4LCamera::V4LCamera(string name,
                      uint8_t camera_index, uint16_t width, uint16_t height,
-                     cvimage::Type::Format format, uint8_t priority)
+                     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");
-  }
+    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);
+    }
     
-  //get v4l2_format
-  struct v4l2_format form;
-  form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-  xioctl (device, VIDIOC_G_FMT,&form);
-  
-  //set width, height and format
-  if (format == cvimage::Type::Format::UYVY) {
-    form.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY; 
-  } else if (format == cvimage::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;
-  xioctl (device, VIDIOC_S_FMT, &form);
-  
-  /* This is just a technicality, but all buffers must be filled up before any
-   staggered SYNC is applied.  SO, filler up. (see V4L HowTo) */
-
-  AllocBuffers();
-
-  for (int bufferIndex = 0; bufferIndex < ((int)requestbuffers.count);++bufferIndex) {
-    struct v4l2_buffer buf;
-
-    memset(&buf, 0, sizeof (v4l2_buffer));
-
-    buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    buf.memory      = V4L2_MEMORY_MMAP;
-    buf.index       = (unsigned long)bufferIndex;
-
-    if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
-        Thread::Err("VIDIOC_QBUF xioctl\n");
-        break;
-    }
-  }
-
-  // enable the streaming
-  v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-  if (-1 == xioctl (device, VIDIOC_STREAMON,&type)) {
-      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
-  GrabFrame();
-    
-    
-    
-  // station sol
-  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;
+    // 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() {
-  SafeStop();
-  Join();
+    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;
-  char* buffer; // raw image
   int fpsCounter = 0;
 
@@ -160,5 +155,5 @@
     fpsCounter++;
     if (GetTime() > (fpsPrev + 5 * (Time)1000000000)) {
-      // toute les 5 secondes
+      // every 5 secondes
       fpsNow = GetTime();
       fps->SetText("fps: %.1f",
@@ -204,6 +199,5 @@
       SetProperty(V4L2_CID_AUTO_WHITE_BALANCE, awb->Value());
 
-    // cam pictures
-    buffer = RetrieveRawFrame();
+    // get picture
     GrabFrame();
     new_time = GetTime();
@@ -216,5 +210,5 @@
 
     output->GetMutex();
-    output->buffer = buffer;
+    output->buffer=(char*)buffers[bufferIndex].start;
     output->ReleaseMutex();
 
@@ -228,88 +222,82 @@
 }
 
-void V4LCamera::GrabFrame(void) {
-    unsigned int count;
-
-    count = 1;
-
-    while (count-- > 0) {
-        for (;;) {
-            fd_set fds;
-            struct timeval tv;
-            int r;
-
-            FD_ZERO (&fds);
-            FD_SET (device, &fds);
-
-            /* Timeout. */
-            tv.tv_sec = 2;
-            tv.tv_usec = 0;
-
-            r = select (device+1, &fds, NULL, NULL, &tv);
-
-            if (-1 == r) {
-                if (EINTR == errno) continue;
-                Thread::Err("select\n");
+int V4LCamera::QueueBuffer(int index) {
+    struct v4l2_buffer buf;
+    if(index>=0 && index<nbBuffers) {
+        memset(&buf, 0, sizeof (v4l2_buffer));
+        buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        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;
+        
+        int ret=xioctl (device, VIDIOC_QBUF, &buf);
+        if (ret==-1) {
+            Thread::Err("VIDIOC_QBUF xioctl %s\n",strerror(-ret));
+            return -1;
+        }
+    }
+    return 0;
+}
+
+int V4LCamera::GrabFrame(void) {
+    //queue previous buffer
+    if(QueueBuffer(bufferIndex)<0) return -1;
+   
+    fd_set fds;
+    struct timeval tv;
+    FD_ZERO (&fds);
+    FD_SET (device, &fds);
+
+    tv.tv_sec = 0;
+    tv.tv_usec = 100000;
+
+    int r = select (device+1, &fds, NULL, NULL, &tv);
+
+    if (-1 == r) {
+        char errorMsg[256];
+        Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
+        return -1;
+    }
+
+    if (0 == r) {
+        Thread::Err("select timeout\n");
+        return -1;
+    }
+
+    struct v4l2_buffer buf;
+    memset(&buf, 0, sizeof (v4l2_buffer));
+    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
+
+    //get last captured image
+    int prevDQbuf=-1;
+    for(int i=0;i<4;i++) {
+        if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
+            if (errno==EAGAIN) {
+                break;
+            } else {
+                Thread::Err("VIDIOC_DQBUF xioctl\n");
+                return -1;
             }
-
-            if (0 == r) {
-                Thread::Err("select timeout\n");
-                /* end the infinite loop */
-                break;
-            }
-
-            if (read_frame_v4l2 ()) break;
-        }
-    }
-}
-
-int V4LCamera::read_frame_v4l2(void) {
-  struct v4l2_buffer buf;
-  memset(&buf, 0, sizeof (v4l2_buffer));
-  
-
-    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    buf.memory = V4L2_MEMORY_MMAP;
-
-    if (-1 == xioctl (device, VIDIOC_DQBUF, &buf)) {
-        switch (errno) {
-        case EAGAIN:
-            return 0;
-
-        case EIO:
-            /* Could ignore EIO, see spec. */
-
-            /* fall through */
-
-        default:
-            /* display the error and stop processing */
-            Thread::Err("VIDIOC_DQBUF xioctl\n");
-            return 1;
-        }
-   }
-
-   if(buf.index >= requestbuffers.count) {
-     Thread::Err("buf.index >= requestbuffers.count\n");
-   }
+       } else {
+           if(prevDQbuf!=-1) {
+               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;
+                   break;
+               }
+           }
+       }
+    }
    
-#ifdef USE_TEMP_BUFFER
-   memcpy(capture->buffers[MAX_V4L_BUFFERS].start, 
-	  capture->buffers[buf.index].start, 
-	  capture->buffers[MAX_V4L_BUFFERS].length );
-   capture->bufferIndex = MAX_V4L_BUFFERS;
-   //printf("got data in buff %d, len=%d, flags=0x%X, seq=%d, used=%d)\n", 
-   //	  buf.index, buf.length, buf.flags, buf.sequence, buf.bytesused);
-#else
-   bufferIndex = buf.index;
-#endif
-
-   if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
-       Thread::Err ("VIDIOC_QBUF xioctl\n");
-   }
-
    return 1;
 }
 
 int V4LCamera::AllocBuffers(void) {
+    struct v4l2_requestbuffers requestbuffers;
    memset(&requestbuffers, 0, sizeof (v4l2_requestbuffers));
    
@@ -320,9 +308,9 @@
    requestbuffers.count = buffer_number;
    requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-   requestbuffers.memory = V4L2_MEMORY_MMAP;
-
-   if (-1 == xioctl (device, VIDIOC_REQBUFS, &requestbuffers)) {
-       if (EINVAL == errno) {
-         Thread::Err("not support memory mapping not supportted\n");
+   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");
@@ -330,73 +318,14 @@
        return -1;
    }
-
-   if (requestbuffers.count < buffer_number) {
-       if (buffer_number == 1) {
-        Thread::Err("Insufficient buffer memory\n");
-        return -1;
-       } else {
-        buffer_number--;
-        Thread::Warn ("Insufficient buffer memory, decreasing buffers\n");
-        goto try_again;
-       }
+   
+   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());
    }
 
-   for (int n_buffers = 0; n_buffers < requestbuffers.count; ++n_buffers) {
-       struct v4l2_buffer buf;
-
-       memset(&buf, 0, sizeof (v4l2_buffer));
-
-       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-       buf.memory = V4L2_MEMORY_MMAP;
-       buf.index = n_buffers;
-
-       if (-1 == xioctl (device, VIDIOC_QUERYBUF, &buf)) {
-           Thread::Err("VIDIOC_QUERYBUF xioctl\n");
-           return -1;
-       }
-
-       buffers[n_buffers].length = buf.length;
-       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 == buffers[n_buffers].start) {
-           Thread::Err("mmap\n");
-           return -1;
-       }
-   }
-
-  //todo: verifier cette alloc, pas de double buffeinrg?
-  //peut on initialiser l'image dans le constrcteur de la camera?
-   
-   output->buffer=output->allocFunction(output->dataType.GetSize());
    return 1;
 };
 
-char *V4LCamera::RetrieveRawFrame(void) {
-  
-  /* [FD] this really belongs here */
-  if (ioctl(device, VIDIOCSYNC, &mmaps[bufferIndex].frame) == -1) {
-    Thread::Err("Could not SYNC to video stream. %s\n", strerror(errno));
-  }
-
-  /* Now get what has already been captured as a IplImage return */
-  if (output->dataType.GetFormat() == cvimage::Type::Format::YUYV || output->dataType.GetFormat() == cvimage::Type::Format::UYVY) {
-    #ifdef USE_TEMP_BUFFER
-    capture->frame.imageData=(char*)capture->buffers[capture->bufferIndex].start;
-    #else
-Printf("frame is not allocated\n");
-    memcpy((char *)frame,(char *)buffers[bufferIndex].start,output->GetDataType().GetSize());
-    #endif
-  } else {
-    Thread::Err("palette %d not supported for raw output\n",output->dataType.GetFormat());
-  }
-
-  return(frame);
-}
-
 bool V4LCamera::HasProblems(void) {
   return hasProblems;
@@ -408,5 +337,5 @@
 
 void V4LCamera::SetAutoExposure(bool value) {
-  Thread::Warn("not implemented in opencv\n");
+  Thread::Warn("not implemented\n");
 }
 
