Index: trunk/lib/FlairSensorActuator/src/V4LCamera.cpp
===================================================================
--- trunk/lib/FlairSensorActuator/src/V4LCamera.cpp	(revision 346)
+++ trunk/lib/FlairSensorActuator/src/V4LCamera.cpp	(revision 347)
@@ -34,26 +34,4 @@
 
 #define CLEAR(x) memset (&(x), 0, sizeof (x))
-#define DEFAULT_V4L_WIDTH  640
-#define DEFAULT_V4L_HEIGHT 480
-//#define CV_CAP_PROP_POS_MSEC       0
-//#define CV_CAP_PROP_POS_FRAMES     1
-//#define CV_CAP_PROP_POS_AVI_RATIO  2
-//#define CV_CAP_PROP_FRAME_WIDTH    3
-//#define CV_CAP_PROP_FRAME_HEIGHT   4
-//#define CV_CAP_PROP_FPS            5
-//#define CV_CAP_PROP_FOURCC         6
-//#define CV_CAP_PROP_FRAME_COUNT    7
-#define CV_CAP_PROP_FORMAT         8
-#define CV_CAP_PROP_MODE           9
-#define CV_CAP_PROP_BRIGHTNESS    10
-#define CV_CAP_PROP_CONTRAST      11
-#define CV_CAP_PROP_SATURATION    12
-#define CV_CAP_PROP_HUE           13
-#define CV_CAP_PROP_GAIN          14
-//#define CV_CAP_PROP_CONVERT_RGB   15
-#define CV_CAP_PROP_SHARPNESS     16
-#define CV_CAP_PROP_EXPOSURE      17
-#define CV_CAP_PROP_AUTOGAIN      18
-#define CV_CAP_PROP_AWB           19
 
 using std::string;
@@ -79,22 +57,43 @@
   /* w/o memset some parts  arent initialized - AKA: Fill it with zeros so it is clean */
   memset(&capture,0,sizeof(CvCaptureCAM_V4L));
-  capture.FirstCapture = 1;
-  PALETTE_YUYV = 0;
-  PALETTE_UYVY= 0;
   n_buffers = 0;
-  
-  if(init(width,height) == -1) {
-   Thread::Err("initialisation failed\n");
-  }
+
   
   if(format == Image::Type::Format::UYVY) {
-    if (setV4LProperty(CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_UYVY)<0)
-      Thread::Err("set capture property error\n");
+    if(init(width,height,V4L2_PIX_FMT_UYVY) == -1) {
+     Thread::Err("initialisation failed\n");
+    }
   } else if (format == Image::Type::Format::YUYV) {
-    if (setV4LProperty(CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_YUYV) <
-        0)
-      Thread::Err("set capture property error\n");
+    if(init(width,height,V4L2_PIX_FMT_YUYV) == -1) {
+     Thread::Err("initialisation failed\n");
+    }
   } else {
     Thread::Err("format not supported\n");
+  }
+  
+  /* 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 (capture.bufferIndex = 0;capture.bufferIndex < ((int)capture.req.count);++capture.bufferIndex) {
+    struct v4l2_buffer buf;
+
+    CLEAR (buf);
+
+    buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    buf.memory      = V4L2_MEMORY_MMAP;
+    buf.index       = (unsigned long)capture.bufferIndex;
+
+    if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
+      Thread::Err("VIDIOC_QBUF error\n");
+    }
+  }
+
+  /* enable the streaming */
+  capture.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+  if (-1 == xioctl (device, VIDIOC_STREAMON,&capture.type)) {
+    /* error enabling the stream */
+    Thread::Err("VIDIOC_STREAMON error\n");
   }
   
@@ -123,5 +122,5 @@
 }
 
-int V4LCamera::init(int width, int height) {
+int V4LCamera::init(int width, int height,unsigned long colorspace) {
   CLEAR (capture.cap);
   if(-1 == xioctl(device, VIDIOC_QUERYCAP, &capture.cap)) {
@@ -136,35 +135,4 @@
     }
   }
-
-  /* Init V4L2 control variables */
-  capture.v4l2_brightness = 0;
-  capture.v4l2_contrast = 0;
-  capture.v4l2_saturation = 0;
-  capture.v4l2_hue = 0;
-  capture.v4l2_gain = 0;
-  capture.v4l2_sharpness = 0;
-  capture.v4l2_exposure = 0;
-  capture.v4l2_autogain = 0;
-  capture.v4l2_awb = 0;
-
-  capture.v4l2_brightness_min = 0;
-  capture.v4l2_contrast_min = 0;
-  capture.v4l2_saturation_min = 0;
-  capture.v4l2_hue_min = 0;
-  capture.v4l2_gain_min = 0;
-  capture.v4l2_sharpness_min = 0;
-  capture.v4l2_exposure_min = 0;
-  capture.v4l2_autogain_min = 0;
-  capture.v4l2_awb_min = 0;
-
-  capture.v4l2_brightness_max = 0;
-  capture.v4l2_contrast_max = 0;
-  capture.v4l2_saturation_max = 0;
-  capture.v4l2_hue_max = 0;
-  capture.v4l2_gain_max = 0;
-  capture.v4l2_sharpness_max = 0;
-  capture.v4l2_exposure_max = 0;
-  capture.v4l2_autogain_max = 0;
-  capture.v4l2_awb_max = 0;
      
   if ((capture.cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
@@ -181,10 +149,6 @@
      return -1;
   }
-
-  if (autosetup_capture_mode_v4l2(&capture) == -1) {
-     return -1;
-  }
-
-  icvSetVideoSize(&capture, width, height);
+  
+  setVideoSize(width, height,colorspace);
 
   unsigned int min;
@@ -204,282 +168,44 @@
 }
 
-int V4LCamera::setV4LProperty(int property_id, double value){
-  int retval=0;
-  switch (property_id) {
-    case CV_CAP_PROP_FORMAT:
-      capture.form.fmt.pix.pixelformat = (unsigned long)value; 
-      retval == xioctl(device, VIDIOC_S_FMT, &capture.form);
-      if ((unsigned long)value != capture.form.fmt.pix.pixelformat) {
-        retval=-1;
-      }
-      PALETTE_YUYV = 0;
-      PALETTE_UYVY= 0;
-      if((unsigned long)value== V4L2_PIX_FMT_YUYV) {
-        PALETTE_YUYV = 1;
-      }
-      if((unsigned long)value== V4L2_PIX_FMT_UYVY) {
-        PALETTE_UYVY = 1;
-      }
-      break;
-    default:
-      Thread::Warn("setting property #%d is not supported\n",property_id);
-  }
-
-  return retval;
-}
-
-int V4LCamera::icvSetControl(CvCaptureCAM_V4L* capture,int property_id, double value) {
-  
-  /* limitation of the input value */
-  if (value < 0.0) {
-    value = 0.0;
-  } else if (value > 1.0) {
-    value = 1.0;
-  }
-
-
-    /* default value for min and max */
-    int v4l2_min = 0;
-    int v4l2_max = 255;
-
-    /* initialisations */
-    CLEAR (capture->control);
-
-    /* set which control we want to set */
-    switch (property_id) {
-
-    case CV_CAP_PROP_BRIGHTNESS:
-        capture->control.id = V4L2_CID_BRIGHTNESS;
-        break;
-    case CV_CAP_PROP_CONTRAST:
-        capture->control.id = V4L2_CID_CONTRAST;
-        break;
-    case CV_CAP_PROP_SATURATION:
-        capture->control.id = V4L2_CID_SATURATION;
-        break;
-    case CV_CAP_PROP_HUE:
-        capture->control.id = V4L2_CID_HUE;
-        break;
-    case CV_CAP_PROP_GAIN:
-        capture->control.id = V4L2_CID_GAIN;
-        break;
-    case CV_CAP_PROP_SHARPNESS:
-        capture->control.id = V4L2_CID_SHARPNESS;
-        break;
-    case CV_CAP_PROP_EXPOSURE:
-        capture->control.id = V4L2_CID_EXPOSURE;
-        break;
-    case CV_CAP_PROP_AUTOGAIN:
-        capture->control.id = V4L2_CID_AUTOGAIN;
-        break;
-    case CV_CAP_PROP_AWB:
-        capture->control.id = V4L2_CID_AUTO_WHITE_BALANCE;
-	break;
-    default:
-        fprintf(stderr,
-                "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n",
-                property_id);
-        return -1;
-    }
-
-    /* get the min and max values */
-    if (-1 == xioctl (device,VIDIOC_G_CTRL, &capture->control)) {
-//          perror ("VIDIOC_G_CTRL for getting min/max values");
-          return -1;
-    }
-
-    /* get the min/max values */
-    switch (property_id) {
-
-    case CV_CAP_PROP_BRIGHTNESS:
-        v4l2_min = capture->v4l2_brightness_min;
-        v4l2_max = capture->v4l2_brightness_max;
-        break;
-    case CV_CAP_PROP_CONTRAST:
-        v4l2_min = capture->v4l2_contrast_min;
-        v4l2_max = capture->v4l2_contrast_max;
-        break;
-    case CV_CAP_PROP_SATURATION:
-        v4l2_min = capture->v4l2_saturation_min;
-        v4l2_max = capture->v4l2_saturation_max;
-        break;
-    case CV_CAP_PROP_HUE:
-        v4l2_min = capture->v4l2_hue_min;
-        v4l2_max = capture->v4l2_hue_max;
-        break;
-    case CV_CAP_PROP_GAIN:
-        v4l2_min = capture->v4l2_gain_min;
-        v4l2_max = capture->v4l2_gain_max;
-	break;
-    case CV_CAP_PROP_SHARPNESS:
-        v4l2_min = capture->v4l2_sharpness_min;
-        v4l2_max = capture->v4l2_sharpness_max;
-	break;
-    case CV_CAP_PROP_EXPOSURE:
-        v4l2_min = capture->v4l2_exposure_min;
-        v4l2_max = capture->v4l2_exposure_max;
-        break;
-    case CV_CAP_PROP_AUTOGAIN:
-        v4l2_min = capture->v4l2_autogain_min;
-        v4l2_max = capture->v4l2_autogain_max;
-        break;
-    case CV_CAP_PROP_AWB:
-        v4l2_min = capture->v4l2_awb_min;
-        v4l2_max = capture->v4l2_awb_max;
-        break;
-    }
-
-    /* initialisations */
-    CLEAR (capture->control);
-
-    /* set which control we want to set */
-    switch (property_id) {
-
-    case CV_CAP_PROP_BRIGHTNESS:
-        capture->control.id = V4L2_CID_BRIGHTNESS;
-        break;
-    case CV_CAP_PROP_CONTRAST:
-        capture->control.id = V4L2_CID_CONTRAST;
-        break;
-    case CV_CAP_PROP_SATURATION:
-        capture->control.id = V4L2_CID_SATURATION;
-        break;
-    case CV_CAP_PROP_HUE:
-        capture->control.id = V4L2_CID_HUE;
-        break;
-    case CV_CAP_PROP_GAIN:
-        capture->control.id = V4L2_CID_GAIN;
-        break;
-   case CV_CAP_PROP_SHARPNESS:
-        capture->control.id = V4L2_CID_SHARPNESS;
-        break;
-    case CV_CAP_PROP_EXPOSURE:
-        capture->control.id = V4L2_CID_EXPOSURE;
-        break;
-    case CV_CAP_PROP_AUTOGAIN:
-        capture->control.id = V4L2_CID_AUTOGAIN;
-        break;
-    case CV_CAP_PROP_AWB:
-        capture->control.id = V4L2_CID_AUTO_WHITE_BALANCE;
-        break;
-    default:
-        fprintf(stderr,
-                "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n",
-                property_id);
-        return -1;
-    }
-
-    /* set the value we want to set to the scaled the value */
-    capture->control.value = (int)(value * (v4l2_max - v4l2_min) + v4l2_min);
-
-    /* The driver may clamp the value or return ERANGE, ignored here */
-    if (-1 == xioctl (device,VIDIOC_S_CTRL, &capture->control) && errno != ERANGE) {
-        perror ("VIDIOC_S_CTRL");
-        return -1;
-    }
-
-  /* all was OK */
-  return 0;
-
-}
-
-
-int V4LCamera::icvSetVideoSize( CvCaptureCAM_V4L* capture, int w, int h) {
-
-
-
-    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     = h*24;
-    capture->crop.c.width      = w*24;
+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, mainly to retreive the pixelformat information */
-    xioctl (device, VIDIOC_G_FMT, &capture->form);
+    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 = w; 
-    capture->form.fmt.pix.height = h;
-    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;
-
-    /* ask the device to change the size
-     * don't test if the set of the size is ok, because some device
-     * don't allow changing the size, and we will get the real size
-     * later */
-    xioctl (device, VIDIOC_S_FMT, &capture->form);
-
-    /* try to set framerate to 30 fps */
-    struct v4l2_streamparm setfps;  
-    memset (&setfps, 0, sizeof(struct v4l2_streamparm));
-    setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    setfps.parm.capture.timeperframe.numerator = 1;
-    setfps.parm.capture.timeperframe.denominator = 30;
-    xioctl (device, VIDIOC_S_PARM, &setfps);
-
-    /* we need to re-initialize some things, like buffers, because the size has
-     * changed */
-    capture->FirstCapture = 1;
+    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))
-    {
-      fprintf(stderr, "HIGHGUI ERROR: V4L/V4L2: Could not obtain specifics of capture window.\n\n");
-
-      
-
-      return 0;
-    }
-
+    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;
-
-}
-int V4LCamera::try_palette_v4l2(CvCaptureCAM_V4L* capture, unsigned long colorspace)
-{
-  CLEAR (capture->form);
-
-  capture->form.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-  capture->form.fmt.pix.pixelformat = colorspace;
-  capture->form.fmt.pix.field       = V4L2_FIELD_ANY;
-  capture->form.fmt.pix.width = DEFAULT_V4L_WIDTH;
-  capture->form.fmt.pix.height = DEFAULT_V4L_HEIGHT;
-  
-  if (-1 == xioctl (device, VIDIOC_S_FMT, &capture->form))
-      return -1;
-
-  
-  if (colorspace != capture->form.fmt.pix.pixelformat)
-    return -1;
-  else
-    return 0;
-}
-int V4LCamera::autosetup_capture_mode_v4l2(CvCaptureCAM_V4L* capture)
-{
-  if (try_palette_v4l2(capture, V4L2_PIX_FMT_YUYV) == 0) {
-    PALETTE_YUYV = 1;
-  } else if (try_palette_v4l2(capture, V4L2_PIX_FMT_UYVY) == 0) {
-    PALETTE_UYVY = 1;
-  } else {
-	fprintf(stderr, "HIGHGUI ERROR: V4L2: Pixel format of incoming image is unsupported by OpenCV\n");
-   
-    return -1;
-  }
-  
-  return 0;
 
 }
@@ -668,7 +394,6 @@
 }
 */
-int V4LCamera::v4l2_alloc_buffers (CvCaptureCAM_V4L *capture, char *deviceName)
-{
-   CLEAR (capture->req);
+int V4LCamera::allocBuffers() {
+   CLEAR (capture.req);
    
    unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
@@ -676,42 +401,32 @@
    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)
-       {
-         fprintf (stderr, "%s does not support memory mapping\n", deviceName);
+   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)
-       {
-           fprintf (stderr, "Insufficient buffer memory on %s\n", deviceName);
-
+   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--;
-	 fprintf (stderr, "Insufficient buffer memory on %s -- decreaseing buffers\n", deviceName);
-	 
-	 goto try_again;
+          buffer_number--;
+          Thread::Warn("Insufficient buffer memory -- decreaseing buffers\n");
+          goto try_again;
        }
    }
 
-   for (n_buffers = 0; n_buffers < capture->req.count; ++n_buffers)
-   {
+   for (n_buffers = 0; n_buffers < capture.req.count; ++n_buffers) {
        struct v4l2_buffer buf;
-
        CLEAR (buf);
 
@@ -721,13 +436,11 @@
 
        if (-1 == xioctl (device, VIDIOC_QUERYBUF, &buf)) {
-           perror ("VIDIOC_QUERYBUF");
-       
+           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 =
+       capture.buffers[n_buffers].length = buf.length;
+       capture.buffers[n_buffers].start =
          mmap (NULL /* start anywhere */,
                buf.length,
@@ -736,83 +449,27 @@
                device, buf.m.offset);
 
-       if (MAP_FAILED == capture->buffers[n_buffers].start) {
-           perror ("mmap");
-       
+       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 ),
+   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);
+   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;
 };
-int V4LCamera::cvGrabFrame(CvCaptureCAM_V4L* capture) {
-
-   if (capture->FirstCapture) { 
-      /* Some general initialization must take place the first time through */
-
-      /* This is just a technicality, but all buffers must be filled up before any
-         staggered SYNC is applied.  SO, filler up. (see V4L HowTo) */
-
-
-	v4l2_alloc_buffers (capture, NULL);
-
-        for (capture->bufferIndex = 0;
-             capture->bufferIndex < ((int)capture->req.count);
-             ++capture->bufferIndex)
-        {
-
-          struct v4l2_buffer buf;
-
-          CLEAR (buf);
-
-          buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-          buf.memory      = V4L2_MEMORY_MMAP;
-          buf.index       = (unsigned long)capture->bufferIndex;
-
-          if (-1 == xioctl (device, VIDIOC_QBUF, &buf)) {
-              perror ("VIDIOC_QBUF");
-              return 0;
-          }
-        }
-
-        /* enable the streaming */
-        capture->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        if (-1 == xioctl (device, VIDIOC_STREAMON,
-                          &capture->type)) {
-            /* error enabling the stream */
-            perror ("VIDIOC_STREAMON");
-            return 0;
-        }
-
-      
-
-
-      /* preparation is ok */
-      capture->FirstCapture = 0;
-   }
-
-
-
-     mainloop_v4l2(capture);
-
-
-
-   return(1);
-}
-void V4LCamera::mainloop_v4l2(CvCaptureCAM_V4L* capture) {
+
+
+int V4LCamera::cvGrabFrame(void) {
     unsigned int count;
 
@@ -848,9 +505,11 @@
             }
 
-            if (read_frame_v4l2 (capture))
+            if (read_frame_v4l2 (&capture))
                 break;
         }
     }
-}
+    return(1);
+}
+
 int V4LCamera::read_frame_v4l2(CvCaptureCAM_V4L* capture) {
     struct v4l2_buffer buf;
@@ -891,4 +550,5 @@
    return 1;
 }
+
 void V4LCamera::Run(void) {
   Time cam_time, new_time, fpsNow, fpsPrev;
@@ -897,5 +557,5 @@
 
   // init image old
-  if (!cvGrabFrame(&capture)) {
+  if (!cvGrabFrame()) {
     Printf("Could not grab a frame\n");
   }
@@ -904,6 +564,4 @@
 
   while (!ToBeStopped()) {
-    
-   
     // fps counter
     fpsCounter++;
@@ -954,5 +612,5 @@
     // cam pictures
     cvRetrieveRawFrame(&capture);
-    if (!cvGrabFrame(&capture)) {
+    if (!cvGrabFrame()) {
       Printf("Could not grab a frame\n");
     }
@@ -1010,15 +668,7 @@
 
 
-    if (PALETTE_YUYV == 1){
-         memcpy((char *)capture->imageData,
-             (char *)capture->buffers[capture->bufferIndex].start,
-             capture->imageSize);
-    }
-    if (PALETTE_UYVY == 1){
-        memcpy((char *)capture->imageData,
-             (char *)capture->buffers[capture->bufferIndex].start,
-             capture->imageSize);
-    }
-  
+    
+  memcpy((char *)capture->imageData,(char *)capture->buffers[capture->bufferIndex].start,capture->imageSize);
+    
  
 
@@ -1188,5 +838,7 @@
   struct v4l2_queryctrl queryctrl;
   queryctrl.id = property;
-  xioctl (device, VIDIOC_QUERYCTRL,&queryctrl);
+  if(xioctl (device, VIDIOC_QUERYCTRL,&queryctrl)==-1) {
+    Thread::Warn("prop %x, VIDIOC_QUERYCTRL failed\n",property);
+  }
   int min = queryctrl.minimum;
   int max = queryctrl.maximum;
@@ -1197,5 +849,7 @@
   control.id = property;
   control.value = (int)(value * (max - min) + min);
-  xioctl (device,VIDIOC_S_CTRL, &control);
+  if(xioctl (device,VIDIOC_S_CTRL, &control)==-1) {
+    Thread::Warn("prop %x, VIDIOC_S_CTRL failed\n",property);
+  }
 }
 
Index: trunk/lib/FlairSensorActuator/src/V4LCamera.h
===================================================================
--- trunk/lib/FlairSensorActuator/src/V4LCamera.h	(revision 346)
+++ trunk/lib/FlairSensorActuator/src/V4LCamera.h	(revision 347)
@@ -168,5 +168,4 @@
     //int deviceHandle;
     int bufferIndex;
-    int FirstCapture;
     struct video_capability capability;
     struct video_window     captureWindow;
@@ -192,31 +191,11 @@
    struct v4l2_queryctrl queryctrl;
    struct v4l2_querymenu querymenu;
-
-   /* V4L2 control variables */
-   int v4l2_brightness, v4l2_brightness_min, v4l2_brightness_max;
-   int v4l2_contrast, v4l2_contrast_min, v4l2_contrast_max;
-   int v4l2_saturation, v4l2_saturation_min, v4l2_saturation_max;
-   int v4l2_sharpness, v4l2_sharpness_min, v4l2_sharpness_max;
-   int v4l2_exposure, v4l2_exposure_min, v4l2_exposure_max;
-   int v4l2_hue, v4l2_hue_min, v4l2_hue_max;
-   int v4l2_gain, v4l2_gain_min, v4l2_gain_max;
-   int v4l2_autogain, v4l2_autogain_min, v4l2_autogain_max;
-   int v4l2_awb, v4l2_awb_min, v4l2_awb_max;
-
-
-
 } CvCaptureCAM_V4L;
 
 CvCaptureCAM_V4L capture;
-int init(int width, int height);
-int autosetup_capture_mode_v4l2(CvCaptureCAM_V4L* capture);
-int icvSetVideoSize( CvCaptureCAM_V4L* capture, int w, int h);
-int try_palette_v4l2(CvCaptureCAM_V4L* capture, unsigned long colorspace);
-int  PALETTE_YUYV,PALETTE_UYVY;
-int setV4LProperty(int property_id, double value );
-int icvSetControl (CvCaptureCAM_V4L* capture,int property_id, double value) ;
-int cvGrabFrame(CvCaptureCAM_V4L* capture);
-int v4l2_alloc_buffers (CvCaptureCAM_V4L *capture, char *deviceName);
-void mainloop_v4l2(CvCaptureCAM_V4L* capture);
+int init(int width, int height,unsigned long colorspace);
+int setVideoSize(int width, int height,unsigned long colorspace);
+int cvGrabFrame(void);
+int allocBuffers (void);
 int read_frame_v4l2(CvCaptureCAM_V4L* capture);
 void cvRetrieveRawFrame( CvCaptureCAM_V4L* capture);
