Index: trunk/lib/FlairSensorActuator/src/V4LCamera.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/V4LCamera.cpp	(revision 349)
+++ trunk/lib/FlairSensorActuator/src/V4LCamera.cpp	(revision 350)
@@ -40,5 +40,5 @@
 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),
+    : Thread(getFrameworkManager(), name, priority,1024*1024),
       Camera(name, width, height, format) {
   
@@ -52,9 +52,9 @@
 
   if(format == Image::Type::Format::UYVY) {
-    if(init(width,height,V4L2_PIX_FMT_UYVY) == -1) {
+    if(Init(width,height,V4L2_PIX_FMT_UYVY) == -1) {
      Thread::Err("initialisation failed\n");
     }
   } else if (format == Image::Type::Format::YUYV) {
-    if(init(width,height,V4L2_PIX_FMT_YUYV) == -1) {
+    if(Init(width,height,V4L2_PIX_FMT_YUYV) == -1) {
      Thread::Err("initialisation failed\n");
     }
@@ -63,18 +63,14 @@
   }
   
-
-  allocBuffers();
-
+  dQueuedBuffer.index=-1;//mark current buffer not valid for initialisation
+  useMemoryUsrPtr=false;
+  if(useMemoryUsrPtr) {
+    AllocUserBuffers();
+  } else {
+    AllocV4LBuffers();
+  }
+  
   for (int i=0;i < nbBuffers;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       = (unsigned long)i;
-
-    if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
-      Thread::Err("VIDIOC_QBUF error\n");
-    }
+    QueueBuffer(i);
   }
 
@@ -103,6 +99,10 @@
 
 V4LCamera::~V4LCamera() {
-  for (int i = 0; i < nbBuffers; i++) {
-       //FreeFunction((char*)buffers[i].start);
+  if(useMemoryUsrPtr) {
+    for (int i = 0; i < nbBuffers; i++) {
+      FreeFunction((char*)buffers[i]);
+    }
+  } else {
+      FreeFunction(imageData);
   }
   SafeStop();
@@ -111,5 +111,5 @@
 }
 
-int V4LCamera::init(int width, int height,unsigned long colorspace) {
+int V4LCamera::Init(int width, int height,unsigned long colorspace) {
   struct v4l2_capability cap;
   memset(&cap, 0, sizeof (v4l2_capability));
@@ -245,5 +245,5 @@
 }
 */
-int V4LCamera::allocBuffers() {
+int V4LCamera::AllocV4LBuffers() {
   struct v4l2_requestbuffers req;
   memset(&req, 0, sizeof (v4l2_requestbuffers));
@@ -309,5 +309,5 @@
 };
 
-int V4LCamera::AllocBuffers(void) {
+int V4LCamera::AllocUserBuffers(void) {
   struct v4l2_requestbuffers req;
   memset(&req, 0, sizeof (v4l2_requestbuffers));
@@ -333,91 +333,125 @@
   return 1;
 };
-
-int V4LCamera::cvGrabFrame(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;
-
-                perror ("select");
-            }
-
-            if (0 == r) {
-                fprintf (stderr, "select timeout\n");
-
-                /* end the infinite loop */
-                break;
-            }
-
-            if (read_frame_v4l2 ())
-                break;
-        }
-    }
-    return(1);
-}
-
-int V4LCamera::read_frame_v4l2(void) {
+/*
+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_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 */
-            perror ("VIDIOC_DQBUF");
-            return 1;
-        }
-   }
-
-   if(buf.index >= nbBuffers) {
-     Thread::Err("buf.index >= capture->req.count\n");
-   }
-
-   bufferIndex = buf.index;
-
-   if (-1 == xioctl (device, VIDIOC_QBUF, &buf))
-       perror ("VIDIOC_QBUF");
-
+    buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
+
+    //get last captured image
+    int prevDQbuf=-1;
+    for(int i=0;i<nbBuffers;i++) {
+        if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
+            if (errno==EAGAIN) {
+                break;
+            } else {
+                Thread::Err("VIDIOC_DQBUF xioctl\n");
+                return -1;
+            }
+       } else {
+           if(prevDQbuf!=-1) {
+               QueueBuffer(prevDQbuf);
+           }
+           for (int i=0; i<nbBuffers; i++) {
+               if((void*)(buf.m.userptr)==buffers[i]) {
+                   prevDQbuf=i;
+                   bufferIndex=i;
+                   break;
+               }
+           }
+       }
+    }
+   
    return 1;
+}
+*/
+int V4LCamera::cvGrabFrame(void) {
+  if(QueueBuffer(&dQueuedBuffer)<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 (r==-1) {
+    char errorMsg[256];
+    Thread::Err("select (%s)\n", strerror_r(-r, errorMsg, sizeof(errorMsg)));
+    return -1;
+  }
+
+  if (r==0) {
+    Thread::Err("select timeout\n");
+    return -1;
+  }
+  
+  struct v4l2_buffer buf;
+  memset(&buf, 0, sizeof (v4l2_buffer));
+  buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  if(useMemoryUsrPtr) {
+    buf.memory=V4L2_MEMORY_USERPTR;
+  } else {
+    buf.memory=V4L2_MEMORY_MMAP;
+  }
+
+   //get last captured image
+  dQueuedBuffer.index=-1;
+  for(int i=0;i<nbBuffers;i++) {
+    if (xioctl (device, VIDIOC_DQBUF, &buf)==-1) {
+      if (errno==EAGAIN) {//when no new buffer is available for reading (non blocking)
+        //Printf("%s eagain\n",Thread::ObjectName().c_str());
+        break;
+      } else {
+        Thread::Err("VIDIOC_DQBUF xioctl\n");
+        return -1;
+      }
+    } else {
+      //Printf("%s %i dqueued\n",Thread::ObjectName().c_str(),buf.index);
+      //Printf("%i %x %i %i\n",buf.bytesused,buf.flags,buf.field,buf.sequence);
+      if(dQueuedBuffer.index!=-1) {
+        //Printf("%s %i queued\n",Thread::ObjectName().c_str(),dQueuedBuffer.index);
+        QueueBuffer(&dQueuedBuffer);
+      }
+      dQueuedBuffer=buf;
+    }
+  }
+  //Printf("%s %i\n",Thread::ObjectName().c_str(),dQueuedBuffer.index);
+
+  return 1;
 }
 
 void V4LCamera::Run(void) {
   Time cam_time, new_time, fpsNow, fpsPrev;
-  //IplImage *img; // raw image
   int fpsCounter = 0;
 
-  // init image old
-  if (!cvGrabFrame()) {
-    Printf("Could not grab a frame\n");
-  }
   cam_time = GetTime();
   fpsPrev = cam_time;
@@ -471,18 +505,30 @@
    
     // cam pictures
-    cvRetrieveRawFrame();
     if (!cvGrabFrame()) {
       Printf("Could not grab a frame\n");
     }
-    new_time = GetTime();
+    
+    //copy to CMEM allocated buffer if necessary
+    if(!useMemoryUsrPtr) {
+      memcpy(imageData,(char *)buffers[dQueuedBuffer.index],output->GetDataType().GetSize());
+    }
         
     //check for ps3eye deconnection in hds uav
+    new_time = GetTime();
     if(new_time-cam_time>100*1000*1000) {
-      Thread::Warn("delta trop grand\n");
+      Thread::Warn("delta t trop grand\n");
       hasProblems=true;
-    }
+    }/*
+    if(hasProblems==false) {
+      struct v4l2_format form;
+      form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+      xioctl(device, VIDIOC_G_FMT,&form);
+      if(xioctl (device, VIDIOC_G_FMT,&form)<0) {
+        Thread::Warn("camera disconnected\n");
+        hasProblems=true;
+      }
+    }*/
 
     output->GetMutex();
-    
     output->buffer=imageData;
     output->ReleaseMutex();
@@ -494,82 +540,29 @@
 }
 
-void V4LCamera::cvRetrieveRawFrame(void) {
-  memcpy(imageData,(char *)buffers[bufferIndex],output->GetDataType().GetSize());
+int V4LCamera::QueueBuffer(struct v4l2_buffer *buf) {
+  if(buf->index>=0 && buf->index<nbBuffers) {
+    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::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;
-        buf.index       = (unsigned long)index;
-        buf.m.userptr=(unsigned long)(buffers[index]);
-        buf.length=output->GetDataType().GetSize();
-        
-        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;
-            }
-       } else {
-           if(prevDQbuf!=-1) {
-               QueueBuffer(prevDQbuf);
-           }
-           for (int i=0; i<nbBuffers; i++) {
-               if((void*)(buf.m.userptr)==buffers[i]) {
-                   prevDQbuf=i;
-                   bufferIndex=i;
-                   break;
-               }
-           }
-       }
-    }
-   
-   return 1;
+  struct v4l2_buffer buf;
+  memset(&buf, 0, sizeof (v4l2_buffer));
+  
+  buf.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  buf.index=(unsigned long)index;
+  if(useMemoryUsrPtr) {
+    buf.memory=V4L2_MEMORY_USERPTR;
+    buf.m.userptr=(unsigned long)(buffers[index]);
+    buf.length=output->GetDataType().GetSize();
+  } else {
+    buf.memory=V4L2_MEMORY_MMAP;
+  }
+  return QueueBuffer(&buf);
 }
 
Index: trunk/lib/FlairSensorActuator/src/V4LCamera.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/V4LCamera.h	(revision 349)
+++ trunk/lib/FlairSensorActuator/src/V4LCamera.h	(revision 350)
@@ -16,8 +16,6 @@
 #include <Thread.h>
 #include "Camera.h"
-//todo use pimpl and remove this
+
 #include <linux/videodev2.h>
-#include <libv4l1-videodev.h>
-
 #define DEFAULT_V4L_BUFFERS 4
 
@@ -152,20 +150,21 @@
   void* buffers[DEFAULT_V4L_BUFFERS];
   int nbBuffers;
-  int bufferIndex;
   char *imageData;
+  bool useMemoryUsrPtr;
   
-  int init(int width, int height,unsigned long colorspace);
-  static int xioctl( int fd, int request, void *arg);
+  int Init(int width, int height,unsigned long colorspace);
+  int xioctl( int fd, int request, void *arg);
   void SetProperty(int property,float value);
   float GetProperty(int property);
-  int GrabFrame(void);
-  int AllocBuffers(void);
-  int allocBuffers (void);
+  //int GrabFrame(void);
+  int AllocV4LBuffers(void);
+  int AllocUserBuffers(void);
   int QueueBuffer(int index);
+  int QueueBuffer(struct v4l2_buffer *buf);
+  struct v4l2_buffer dQueuedBuffer;
   
 
 int cvGrabFrame(void);
-int read_frame_v4l2(void);
-void cvRetrieveRawFrame(void);
+
 
 
