Ignore:
Timestamp:
11/28/19 17:08:04 (2 years ago)
Author:
Sanahuja Guillaume
Message:

add servos

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/V4LCamera.cpp

    r338 r340  
    3333#define DEFAULT_V4L_BUFFERS 4
    3434
     35#define CHANNEL_NUMBER 1
     36#define CLEAR(x) memset (&(x), 0, sizeof (x))
     37#define DEFAULT_V4L_WIDTH  640
     38#define DEFAULT_V4L_HEIGHT 480
     39#define CV_CAP_PROP_POS_MSEC       0
     40#define CV_CAP_PROP_POS_FRAMES     1
     41#define CV_CAP_PROP_POS_AVI_RATIO  2
     42#define CV_CAP_PROP_FRAME_WIDTH    3
     43#define CV_CAP_PROP_FRAME_HEIGHT   4
     44#define CV_CAP_PROP_FPS            5
     45#define CV_CAP_PROP_FOURCC         6
     46#define CV_CAP_PROP_FRAME_COUNT    7
     47#define CV_CAP_PROP_FORMAT         8
     48#define CV_CAP_PROP_MODE           9
     49#define CV_CAP_PROP_BRIGHTNESS    10
     50#define CV_CAP_PROP_CONTRAST      11
     51#define CV_CAP_PROP_SATURATION    12
     52#define CV_CAP_PROP_HUE           13
     53#define CV_CAP_PROP_GAIN          14
     54#define CV_CAP_PROP_CONVERT_RGB   15
     55#define CV_CAP_PROP_SHARPNESS     16
     56#define CV_CAP_PROP_EXPOSURE      17
     57#define CV_CAP_PROP_AUTOGAIN      18
     58#define CV_CAP_PROP_AWB           19
     59
     60#define HAVE_CAMV4L2
     61
    3562using std::string;
    3663using namespace flair::core;
     
    3966namespace flair {
    4067namespace sensor {
    41 
     68 
     69  V4LCamera::V4LCamera(string name,
     70                     uint8_t camera_index, uint16_t width, uint16_t height,
     71                     Image::Type::Format format, uint8_t priority)
     72    : Thread(getFrameworkManager(), name, priority),
     73      Camera(name, width, height, format) {
     74 
     75    string deviceName="/dev/video"+std::to_string(camera_index);
     76    device = open(deviceName.c_str(), O_RDWR | O_NONBLOCK);
     77    if (device == -1) {
     78        Thread::Err("Cannot open %s\n",deviceName.c_str());
     79    } else {
     80        Printf("V4LCamera %s, opened %s\n",name.c_str(),deviceName.c_str());
     81    }
     82
     83    /* w/o memset some parts  arent initialized - AKA: Fill it with zeros so it is clean */
     84   memset(&capture,0,sizeof(CvCaptureCAM_V4L));
     85   /* Present the routines needed for V4L funtionality.  They are inserted as part of
     86      the standard set of cv calls promoting transparency.  "Vector Table" insertion. */
     87   capture.FirstCapture = 1;
     88   capture.deviceHandle = device;
     89 
     90     PALETTE_YUYV = 0;
     91     PALETTE_UYVY= 0;
     92   
     93   n_buffers = 0;
     94   if (_capture_V4L2 (&capture) == -1) {
     95     Thread::Err("_capture_V4L2 failed\n");
     96   }
     97 
     98if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FRAME_WIDTH, width)<0)
     99    Thread::Err("cvSetCaptureProperty error\n");
     100  if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FRAME_HEIGHT, height)<0)
     101    Thread::Err("cvSetCaptureProperty error\n");
     102
     103  if (format == Image::Type::Format::UYVY) {
     104    if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_UYVY)<0)
     105      Thread::Err("cvSetCaptureProperty error\n");
     106  } else if (format == Image::Type::Format::YUYV) {
     107    if (icvSetPropertyCAM_V4L(&capture, CV_CAP_PROP_FORMAT, V4L2_PIX_FMT_YUYV) <
     108        0)
     109      Thread::Err("cvSetCaptureProperty error\n");
     110  } else {
     111    Thread::Err("format not supported\n");
     112  }
     113    // ground station
     114    gain = new DoubleSpinBox(GetGroupBox()->NewRow(), "gain:", 0, 1, 0.1);
     115    exposure = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "exposure:", 0,1, 0.1);
     116    bright = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "bright:", 0, 1, 0.1);
     117    contrast = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "contrast:", 0,1, 0.1);
     118    hue = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "hue:", 0, 1, 0.1);
     119    sharpness = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "sharpness:", 0, 1, 0.1);
     120    sat = new DoubleSpinBox(GetGroupBox()->LastRowLastCol(), "saturation:", 0, 1,0.1);
     121    autogain = new CheckBox(GetGroupBox()->NewRow(), "autogain:");
     122    autoexposure = new CheckBox(GetGroupBox()->LastRowLastCol(), "autoexposure:");
     123    awb = new CheckBox(GetGroupBox()->LastRowLastCol(), "awb:");
     124    fps = new Label(GetGroupBox()->NewRow(), "fps");
     125
     126    hasProblems=false;
     127}
     128
     129V4LCamera::~V4LCamera() {
     130    for (int n_buffers = 0; n_buffers < nbBuffers; n_buffers++) {
     131       //FreeFunction((char*)buffers[n_buffers].start);
     132   }
     133    SafeStop();
     134    Join();
     135}
     136int V4LCamera::icvSetPropertyCAM_V4L( CvCaptureCAM_V4L* capture,
     137                                  int property_id, double value ){
     138    static int width = 0, height = 0;
     139    int retval;
     140
     141    /* initialization */
     142    retval = 0;
     143
     144    /* two subsequent calls setting WIDTH and HEIGHT will change
     145       the video size */
     146    /* the first one will return an error, though. */
     147
     148    switch (property_id) {
     149    case CV_CAP_PROP_FRAME_WIDTH:
     150        width = value;
     151        if(width !=0 && height != 0) {
     152            retval = icvSetVideoSize( capture, width, height);
     153            width = height = 0;
     154        }
     155        break;
     156    case CV_CAP_PROP_FRAME_HEIGHT:
     157        height = value;
     158        if(width !=0 && height != 0) {
     159            retval = icvSetVideoSize( capture, width, height);
     160            width = height = 0;
     161        }
     162        break;
     163    case CV_CAP_PROP_FORMAT:
     164
     165        capture->form.fmt.pix.pixelformat = (unsigned long)value;
     166        retval == xioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form);
     167        if ((unsigned long)value != capture->form.fmt.pix.pixelformat)
     168                retval=-1;
     169
     170       
     171        PALETTE_YUYV = 0;
     172        PALETTE_UYVY= 0;
     173       
     174
     175        if ((unsigned long)value== V4L2_PIX_FMT_YUYV)
     176        {
     177                PALETTE_YUYV = 1;
     178        }
     179        if ((unsigned long)value== V4L2_PIX_FMT_UYVY)
     180        {
     181                PALETTE_UYVY = 1;
     182        }
     183       
     184
     185        break;
     186    case CV_CAP_PROP_BRIGHTNESS:
     187    case CV_CAP_PROP_CONTRAST:
     188    case CV_CAP_PROP_SATURATION:
     189    case CV_CAP_PROP_HUE:
     190    case CV_CAP_PROP_GAIN:
     191    case CV_CAP_PROP_SHARPNESS:
     192    case CV_CAP_PROP_EXPOSURE:
     193    case CV_CAP_PROP_AUTOGAIN:
     194    case CV_CAP_PROP_AWB:
     195        retval = icvSetControl(capture, property_id, value);
     196        break;
     197    default:
     198        fprintf(stderr,
     199                "HIGHGUI ERROR: V4L: setting property #%d is not supported\n",
     200                property_id);
     201    }
     202
     203    /* return the the status */
     204    return retval;
     205}
     206int V4LCamera::icvSetControl (CvCaptureCAM_V4L* capture,
     207                          int property_id, double value) {
     208 
     209  /* limitation of the input value */
     210  if (value < 0.0) {
     211    value = 0.0;
     212  } else if (value > 1.0) {
     213    value = 1.0;
     214  }
     215
     216
     217    /* default value for min and max */
     218    int v4l2_min = 0;
     219    int v4l2_max = 255;
     220
     221    /* initialisations */
     222    CLEAR (capture->control);
     223
     224    /* set which control we want to set */
     225    switch (property_id) {
     226
     227    case CV_CAP_PROP_BRIGHTNESS:
     228        capture->control.id = V4L2_CID_BRIGHTNESS;
     229        break;
     230    case CV_CAP_PROP_CONTRAST:
     231        capture->control.id = V4L2_CID_CONTRAST;
     232        break;
     233    case CV_CAP_PROP_SATURATION:
     234        capture->control.id = V4L2_CID_SATURATION;
     235        break;
     236    case CV_CAP_PROP_HUE:
     237        capture->control.id = V4L2_CID_HUE;
     238        break;
     239    case CV_CAP_PROP_GAIN:
     240        capture->control.id = V4L2_CID_GAIN;
     241        break;
     242    case CV_CAP_PROP_SHARPNESS:
     243        capture->control.id = V4L2_CID_SHARPNESS;
     244        break;
     245    case CV_CAP_PROP_EXPOSURE:
     246        capture->control.id = V4L2_CID_EXPOSURE;
     247        break;
     248    case CV_CAP_PROP_AUTOGAIN:
     249        capture->control.id = V4L2_CID_AUTOGAIN;
     250        break;
     251    case CV_CAP_PROP_AWB:
     252        capture->control.id = V4L2_CID_AUTO_WHITE_BALANCE;
     253        break;
     254    default:
     255        fprintf(stderr,
     256                "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n",
     257                property_id);
     258        return -1;
     259    }
     260
     261    /* get the min and max values */
     262    if (-1 == xioctl (capture->deviceHandle,
     263                      VIDIOC_G_CTRL, &capture->control)) {
     264//          perror ("VIDIOC_G_CTRL for getting min/max values");
     265          return -1;
     266    }
     267
     268    /* get the min/max values */
     269    switch (property_id) {
     270
     271    case CV_CAP_PROP_BRIGHTNESS:
     272        v4l2_min = capture->v4l2_brightness_min;
     273        v4l2_max = capture->v4l2_brightness_max;
     274        break;
     275    case CV_CAP_PROP_CONTRAST:
     276        v4l2_min = capture->v4l2_contrast_min;
     277        v4l2_max = capture->v4l2_contrast_max;
     278        break;
     279    case CV_CAP_PROP_SATURATION:
     280        v4l2_min = capture->v4l2_saturation_min;
     281        v4l2_max = capture->v4l2_saturation_max;
     282        break;
     283    case CV_CAP_PROP_HUE:
     284        v4l2_min = capture->v4l2_hue_min;
     285        v4l2_max = capture->v4l2_hue_max;
     286        break;
     287    case CV_CAP_PROP_GAIN:
     288        v4l2_min = capture->v4l2_gain_min;
     289        v4l2_max = capture->v4l2_gain_max;
     290        break;
     291    case CV_CAP_PROP_SHARPNESS:
     292        v4l2_min = capture->v4l2_sharpness_min;
     293        v4l2_max = capture->v4l2_sharpness_max;
     294        break;
     295    case CV_CAP_PROP_EXPOSURE:
     296        v4l2_min = capture->v4l2_exposure_min;
     297        v4l2_max = capture->v4l2_exposure_max;
     298        break;
     299    case CV_CAP_PROP_AUTOGAIN:
     300        v4l2_min = capture->v4l2_autogain_min;
     301        v4l2_max = capture->v4l2_autogain_max;
     302        break;
     303    case CV_CAP_PROP_AWB:
     304        v4l2_min = capture->v4l2_awb_min;
     305        v4l2_max = capture->v4l2_awb_max;
     306        break;
     307    }
     308
     309    /* initialisations */
     310    CLEAR (capture->control);
     311
     312    /* set which control we want to set */
     313    switch (property_id) {
     314
     315    case CV_CAP_PROP_BRIGHTNESS:
     316        capture->control.id = V4L2_CID_BRIGHTNESS;
     317        break;
     318    case CV_CAP_PROP_CONTRAST:
     319        capture->control.id = V4L2_CID_CONTRAST;
     320        break;
     321    case CV_CAP_PROP_SATURATION:
     322        capture->control.id = V4L2_CID_SATURATION;
     323        break;
     324    case CV_CAP_PROP_HUE:
     325        capture->control.id = V4L2_CID_HUE;
     326        break;
     327    case CV_CAP_PROP_GAIN:
     328        capture->control.id = V4L2_CID_GAIN;
     329        break;
     330   case CV_CAP_PROP_SHARPNESS:
     331        capture->control.id = V4L2_CID_SHARPNESS;
     332        break;
     333    case CV_CAP_PROP_EXPOSURE:
     334        capture->control.id = V4L2_CID_EXPOSURE;
     335        break;
     336    case CV_CAP_PROP_AUTOGAIN:
     337        capture->control.id = V4L2_CID_AUTOGAIN;
     338        break;
     339    case CV_CAP_PROP_AWB:
     340        capture->control.id = V4L2_CID_AUTO_WHITE_BALANCE;
     341        break;
     342    default:
     343        fprintf(stderr,
     344                "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n",
     345                property_id);
     346        return -1;
     347    }
     348
     349    /* set the value we want to set to the scaled the value */
     350    capture->control.value = (int)(value * (v4l2_max - v4l2_min) + v4l2_min);
     351
     352    /* The driver may clamp the value or return ERANGE, ignored here */
     353    if (-1 == xioctl (capture->deviceHandle,
     354                      VIDIOC_S_CTRL, &capture->control) && errno != ERANGE) {
     355        perror ("VIDIOC_S_CTRL");
     356        return -1;
     357    }
     358
     359  /* all was OK */
     360  return 0;
     361
     362}
     363int V4LCamera::_capture_V4L2 (CvCaptureCAM_V4L *capture)
     364{
     365   int detect_v4l2 = 0;
     366
     367   detect_v4l2 = try_init_v4l2(capture);
     368
     369   if (detect_v4l2 != 1) {
     370       /* init of the v4l2 device is not OK */
     371       return -1;
     372   }
     373
     374   /* Init V4L2 control variables */
     375   capture->v4l2_brightness = 0;
     376   capture->v4l2_contrast = 0;
     377   capture->v4l2_saturation = 0;
     378   capture->v4l2_hue = 0;
     379   capture->v4l2_gain = 0;
     380   capture->v4l2_sharpness = 0;
     381   capture->v4l2_exposure = 0;
     382   capture->v4l2_autogain = 0;
     383   capture->v4l2_awb = 0;
     384
     385   capture->v4l2_brightness_min = 0;
     386   capture->v4l2_contrast_min = 0;
     387   capture->v4l2_saturation_min = 0;
     388   capture->v4l2_hue_min = 0;
     389   capture->v4l2_gain_min = 0;
     390   capture->v4l2_sharpness_min = 0;
     391   capture->v4l2_exposure_min = 0;
     392   capture->v4l2_autogain_min = 0;
     393   capture->v4l2_awb_min = 0;
     394
     395   capture->v4l2_brightness_max = 0;
     396   capture->v4l2_contrast_max = 0;
     397   capture->v4l2_saturation_max = 0;
     398   capture->v4l2_hue_max = 0;
     399   capture->v4l2_gain_max = 0;
     400   capture->v4l2_sharpness_max = 0;
     401   capture->v4l2_exposure_max = 0;
     402   capture->v4l2_autogain_max = 0;
     403   capture->v4l2_awb_max = 0;
     404     
     405 
     406   if ((capture->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
     407      /* Nope. */
     408      fprintf( stderr, "HIGHGUI ERROR: V4L2: device is unable to capture video memory.\n");
     409     
     410      return -1;
     411   }
     412
     413 
     414
     415   /* Find Window info */
     416   CLEAR (capture->form);
     417   capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     418       
     419   if (-1 == xioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form)) {
     420       fprintf( stderr, "HIGHGUI ERROR: V4L2: Could not obtain specifics of capture window.\n\n");
     421       
     422       return -1;
     423   }
     424
     425 
     426
     427   if (autosetup_capture_mode_v4l2(capture) == -1)
     428       return -1;
     429
     430   icvSetVideoSize(capture, DEFAULT_V4L_WIDTH, DEFAULT_V4L_HEIGHT);
     431
     432   unsigned int min;
     433
     434   /* Buggy driver paranoia. */
     435   min = capture->form.fmt.pix.width * 2;
     436
     437   if (capture->form.fmt.pix.bytesperline < min)
     438       capture->form.fmt.pix.bytesperline = min;
     439
     440   min = capture->form.fmt.pix.bytesperline * capture->form.fmt.pix.height;
     441 
     442   if (capture->form.fmt.pix.sizeimage < min)
     443       capture->form.fmt.pix.sizeimage = min;
     444
     445   return 1;
     446}
     447int V4LCamera::icvSetVideoSize( CvCaptureCAM_V4L* capture, int w, int h) {
     448
     449
     450
     451    CLEAR (capture->crop);
     452    capture->crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     453    capture->crop.c.left       = 0;
     454    capture->crop.c.top        = 0;
     455    capture->crop.c.height     = h*24;
     456    capture->crop.c.width      = w*24;
     457
     458    /* set the crop area, but don't exit if the device don't support croping */
     459    xioctl (capture->deviceHandle, VIDIOC_S_CROP, &capture->crop);
     460
     461    CLEAR (capture->form);
     462    capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     463
     464    /* read the current setting, mainly to retreive the pixelformat information */
     465    xioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form);
     466
     467    /* set the values we want to change */
     468    capture->form.fmt.pix.width = w;
     469    capture->form.fmt.pix.height = h;
     470    capture->form.fmt.win.chromakey = 0;
     471    capture->form.fmt.win.field = V4L2_FIELD_ANY;
     472    capture->form.fmt.win.clips = 0;
     473    capture->form.fmt.win.clipcount = 0;
     474    capture->form.fmt.pix.field = V4L2_FIELD_ANY;
     475
     476    /* ask the device to change the size
     477     * don't test if the set of the size is ok, because some device
     478     * don't allow changing the size, and we will get the real size
     479     * later */
     480    xioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form);
     481
     482    /* try to set framerate to 30 fps */
     483    struct v4l2_streamparm setfps; 
     484    memset (&setfps, 0, sizeof(struct v4l2_streamparm));
     485    setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     486    setfps.parm.capture.timeperframe.numerator = 1;
     487    setfps.parm.capture.timeperframe.denominator = 30;
     488    xioctl (capture->deviceHandle, VIDIOC_S_PARM, &setfps);
     489
     490    /* we need to re-initialize some things, like buffers, because the size has
     491     * changed */
     492    capture->FirstCapture = 1;
     493
     494    /* Get window info again, to get the real value */
     495    if (-1 == xioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form))
     496    {
     497      fprintf(stderr, "HIGHGUI ERROR: V4L/V4L2: Could not obtain specifics of capture window.\n\n");
     498
     499     
     500
     501      return 0;
     502    }
     503
     504    return 0;
     505
     506 
     507
     508   
     509  return 0;
     510
     511}
     512int V4LCamera::try_palette_v4l2(CvCaptureCAM_V4L* capture, unsigned long colorspace)
     513{
     514  CLEAR (capture->form);
     515
     516  capture->form.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     517  capture->form.fmt.pix.pixelformat = colorspace;
     518  capture->form.fmt.pix.field       = V4L2_FIELD_ANY;
     519  capture->form.fmt.pix.width = DEFAULT_V4L_WIDTH;
     520  capture->form.fmt.pix.height = DEFAULT_V4L_HEIGHT;
     521 
     522  if (-1 == xioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form))
     523      return -1;
     524
     525 
     526  if (colorspace != capture->form.fmt.pix.pixelformat)
     527    return -1;
     528  else
     529    return 0;
     530}
     531int V4LCamera::autosetup_capture_mode_v4l2(CvCaptureCAM_V4L* capture)
     532{
     533  if (try_palette_v4l2(capture, V4L2_PIX_FMT_YUYV) == 0) {
     534    PALETTE_YUYV = 1;
     535  } else if (try_palette_v4l2(capture, V4L2_PIX_FMT_UYVY) == 0) {
     536    PALETTE_UYVY = 1;
     537  } else {
     538        fprintf(stderr, "HIGHGUI ERROR: V4L2: Pixel format of incoming image is unsupported by OpenCV\n");
     539   
     540    return -1;
     541  }
     542 
     543  return 0;
     544
     545}
     546
     547
     548
     549int V4LCamera::try_init_v4l2(CvCaptureCAM_V4L* capture)
     550{
     551
     552  // if detect = -1 then unable to open device
     553  // if detect = 0 then detected nothing
     554  // if detect = 1 then V4L2 device
     555  int detect = 0;
     556
     557
     558  if (detect == 0)
     559  {
     560    CLEAR (capture->cap);
     561    if (-1 == xioctl (capture->deviceHandle, VIDIOC_QUERYCAP, &capture->cap))
     562    {
     563      detect = 0;
     564
     565     
     566    }
     567      else
     568    {
     569      CLEAR (capture->capability);
     570      capture->capability.type = capture->cap.capabilities;
     571     
     572      /* Query channels number */
     573      if (-1 != xioctl (capture->deviceHandle, VIDIOC_G_INPUT, &capture->capability.channels))
     574      {
     575        detect = 1;
     576      }
     577    }
     578  }
     579
     580  return detect;
     581
     582}
     583/*
    42584V4LCamera::V4LCamera(string name,
    43585                     uint8_t camera_index, uint16_t width, uint16_t height,
     
    221763  close(device);
    222764}
    223 
     765*/
     766int V4LCamera::v4l2_alloc_buffers (CvCaptureCAM_V4L *capture, char *deviceName)
     767{
     768   CLEAR (capture->req);
     769   
     770   unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
     771
     772   try_again:
     773   
     774   capture->req.count = buffer_number;
     775   capture->req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     776   capture->req.memory = V4L2_MEMORY_MMAP;
     777
     778   if (-1 == xioctl (capture->deviceHandle, VIDIOC_REQBUFS, &capture->req))
     779   {
     780       if (EINVAL == errno)
     781       {
     782         fprintf (stderr, "%s does not support memory mapping\n", deviceName);
     783       } else {
     784         perror ("VIDIOC_REQBUFS");
     785       }
     786       /* free capture, and returns an error code */
     787       
     788       return -1;
     789   }
     790
     791   if (capture->req.count < buffer_number)
     792   {
     793       if (buffer_number == 1)
     794       {
     795           fprintf (stderr, "Insufficient buffer memory on %s\n", deviceName);
     796
     797           /* free capture, and returns an error code */
     798           
     799           return -1;
     800       } else {
     801         buffer_number--;
     802         fprintf (stderr, "Insufficient buffer memory on %s -- decreaseing buffers\n", deviceName);
     803         
     804         goto try_again;
     805       }
     806   }
     807
     808   for (n_buffers = 0; n_buffers < capture->req.count; ++n_buffers)
     809   {
     810       struct v4l2_buffer buf;
     811
     812       CLEAR (buf);
     813
     814       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     815       buf.memory = V4L2_MEMORY_MMAP;
     816       buf.index = n_buffers;
     817
     818       if (-1 == xioctl (capture->deviceHandle, VIDIOC_QUERYBUF, &buf)) {
     819           perror ("VIDIOC_QUERYBUF");
     820       
     821           /* free capture, and returns an error code */
     822           
     823           return -1;
     824       }
     825
     826       capture->buffers[n_buffers].length = buf.length;
     827       capture->buffers[n_buffers].start =
     828         mmap (NULL /* start anywhere */,
     829               buf.length,
     830               PROT_READ | PROT_WRITE /* required */,
     831               MAP_SHARED /* recommended */,
     832               capture->deviceHandle, buf.m.offset);
     833
     834       if (MAP_FAILED == capture->buffers[n_buffers].start) {
     835           perror ("mmap");
     836       
     837           /* free capture, and returns an error code */
     838           
     839           return -1;
     840       }
     841
     842
     843   }
     844
     845   /* Set up Image data */
     846   /*
     847   cvInitImageHeader( &capture->frame,
     848                      cvSize( capture->captureWindow.width,
     849                              capture->captureWindow.height ),
     850                      IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );*/
     851   /* Allocate space for RGBA data */
     852   capture->imageSize=capture->form.fmt.pix.width*capture->form.fmt.pix.height*2;
     853   capture->imageData = AllocFunction(capture->imageSize);
     854   Printf("cmem allocated %i at %x\n",capture->imageSize,capture->imageData);
     855   
     856   return 1;
     857};
     858int V4LCamera::cvGrabFrame(CvCaptureCAM_V4L* capture) {
     859
     860   if (capture->FirstCapture) {
     861      /* Some general initialization must take place the first time through */
     862
     863      /* This is just a technicality, but all buffers must be filled up before any
     864         staggered SYNC is applied.  SO, filler up. (see V4L HowTo) */
     865
     866
     867        v4l2_alloc_buffers (capture, NULL);
     868
     869        for (capture->bufferIndex = 0;
     870             capture->bufferIndex < ((int)capture->req.count);
     871             ++capture->bufferIndex)
     872        {
     873
     874          struct v4l2_buffer buf;
     875
     876          CLEAR (buf);
     877
     878          buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     879          buf.memory      = V4L2_MEMORY_MMAP;
     880          buf.index       = (unsigned long)capture->bufferIndex;
     881
     882          if (-1 == xioctl (capture->deviceHandle, VIDIOC_QBUF, &buf)) {
     883              perror ("VIDIOC_QBUF");
     884              return 0;
     885          }
     886        }
     887
     888        /* enable the streaming */
     889        capture->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     890        if (-1 == xioctl (capture->deviceHandle, VIDIOC_STREAMON,
     891                          &capture->type)) {
     892            /* error enabling the stream */
     893            perror ("VIDIOC_STREAMON");
     894            return 0;
     895        }
     896
     897     
     898
     899
     900      /* preparation is ok */
     901      capture->FirstCapture = 0;
     902   }
     903
     904
     905
     906     mainloop_v4l2(capture);
     907
     908
     909
     910   return(1);
     911}
     912void V4LCamera::mainloop_v4l2(CvCaptureCAM_V4L* capture) {
     913    unsigned int count;
     914
     915    count = 1;
     916
     917    while (count-- > 0) {
     918        for (;;) {
     919            fd_set fds;
     920            struct timeval tv;
     921            int r;
     922
     923            FD_ZERO (&fds);
     924            FD_SET (capture->deviceHandle, &fds);
     925
     926            /* Timeout. */
     927            tv.tv_sec = 2;
     928            tv.tv_usec = 0;
     929
     930            r = select (capture->deviceHandle+1, &fds, NULL, NULL, &tv);
     931
     932            if (-1 == r) {
     933                if (EINTR == errno)
     934                    continue;
     935
     936                perror ("select");
     937            }
     938
     939            if (0 == r) {
     940                fprintf (stderr, "select timeout\n");
     941
     942                /* end the infinite loop */
     943                break;
     944            }
     945
     946            if (read_frame_v4l2 (capture))
     947                break;
     948        }
     949    }
     950}
     951int V4LCamera::read_frame_v4l2(CvCaptureCAM_V4L* capture) {
     952    struct v4l2_buffer buf;
     953
     954    CLEAR (buf);
     955
     956    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     957    buf.memory = V4L2_MEMORY_MMAP;
     958
     959    if (-1 == xioctl (capture->deviceHandle, VIDIOC_DQBUF, &buf)) {
     960        switch (errno) {
     961        case EAGAIN:
     962            return 0;
     963
     964        case EIO:
     965            /* Could ignore EIO, see spec. */
     966
     967            /* fall through */
     968
     969        default:
     970            /* display the error and stop processing */
     971            perror ("VIDIOC_DQBUF");
     972            return 1;
     973        }
     974   }
     975
     976   if(buf.index >= capture->req.count) {
     977     Thread::Err("buf.index >= capture->req.count\n");
     978   }
     979   
     980
     981   capture->bufferIndex = buf.index;
     982
     983
     984   if (-1 == xioctl (capture->deviceHandle, VIDIOC_QBUF, &buf))
     985       perror ("VIDIOC_QBUF");
     986
     987   return 1;
     988}
     989void V4LCamera::Run(void) {
     990  Time cam_time, new_time, fpsNow, fpsPrev;
     991  //IplImage *img; // raw image
     992  int fpsCounter = 0;
     993
     994  // init image old
     995  if (!cvGrabFrame(&capture)) {
     996    Printf("Could not grab a frame\n");
     997  }
     998  cam_time = GetTime();
     999  fpsPrev = cam_time;
     1000
     1001  while (!ToBeStopped()) {
     1002   
     1003   
     1004    // fps counter
     1005    fpsCounter++;
     1006    if (fpsCounter == 100) {
     1007      fpsNow = GetTime();
     1008      fps->SetText("fps: %.1f",
     1009       fpsCounter / ((float)(fpsNow - fpsPrev) / 1000000000.));
     1010      fpsCounter = 0;
     1011      fpsPrev = fpsNow;
     1012    }
     1013
     1014    // cam properties
     1015    if (gain->ValueChanged() == true && autogain->Value() == false)
     1016      SetGain(gain->Value());
     1017    if (exposure->ValueChanged() == true && autoexposure->Value() == false)
     1018      SetExposure(exposure->Value());
     1019    if (bright->ValueChanged() == true)
     1020      SetBrightness(bright->Value());
     1021    if (sat->ValueChanged() == true)
     1022      SetSaturation(sat->Value());
     1023    if (contrast->ValueChanged() == true)
     1024      SetContrast(contrast->Value());
     1025    if (hue->ValueChanged() == true)
     1026      SetHue(hue->Value());
     1027    //if (sharpness->ValueChanged() == true)
     1028    //  cvSetCaptureProperty(capture, CV_CAP_PROP_SHARPNESS, sharpness->Value());
     1029    if (autogain->ValueChanged() == true) {
     1030      if (autogain->Value() == true) {
     1031        gain->setEnabled(false);
     1032      } else {
     1033        gain->setEnabled(true);
     1034        SetGain(gain->Value());
     1035      }
     1036      SetAutoGain(autogain->Value());
     1037    }
     1038    if (autoexposure->ValueChanged() == true) {
     1039      if (autoexposure->Value() == true) {
     1040        exposure->setEnabled(false);
     1041      } else {
     1042        exposure->setEnabled(true);
     1043        SetExposure(exposure->Value());
     1044      }
     1045      SetAutoExposure(autoexposure->Value());
     1046    }
     1047    //if (awb->ValueChanged() == true)
     1048    //  cvSetCaptureProperty(capture, CV_CAP_PROP_AWB, awb->Value());
     1049   
     1050    // cam pictures
     1051    cvRetrieveRawFrame(&capture);
     1052    if (!cvGrabFrame(&capture)) {
     1053      Printf("Could not grab a frame\n");
     1054    }
     1055    new_time = GetTime();
     1056       
     1057    //check for ps3eye deconnection in hds uav
     1058    if(new_time-cam_time>100*1000*1000) {
     1059      Thread::Warn("delta trop grand\n");
     1060      hasProblems=true;
     1061    }
     1062
     1063    output->GetMutex();
     1064   
     1065    output->buffer=capture.imageData;
     1066    output->ReleaseMutex();
     1067   
     1068    output->SetDataTime(cam_time);
     1069    ProcessUpdate(output);
     1070    cam_time = new_time;
     1071  }
     1072
     1073  //cvReleaseCapture(&capture);
     1074}
     1075
     1076void V4LCamera::cvRetrieveRawFrame( CvCaptureCAM_V4L* capture) {
     1077
     1078
     1079
     1080
     1081   /* Now get what has already been captured as a IplImage return */
     1082
     1083   /* First, reallocate imageData if the frame size changed */
     1084
     1085
     1086/*
     1087    if(((unsigned long)capture->frame.width != capture->form.fmt.pix.width)
     1088       || ((unsigned long)capture->frame.height != capture->form.fmt.pix.height)) {
     1089           if (PALETTE_YUYV == 1 || PALETTE_UYVY == 1)
     1090           {
     1091                cvFree(&capture->frame.imageData);
     1092                cvInitImageHeader( &capture->frame,
     1093                cvSize( capture->form.fmt.pix.width,
     1094                capture->form.fmt.pix.height ),
     1095                IPL_DEPTH_8U,2, IPL_ORIGIN_TL, 4 );
     1096                capture->frame.imageData = (char *)cvAlloc(capture->frame.imageSize);
     1097           }else
     1098           {
     1099              fprintf( stderr,
     1100                 "HIGHGUI ERROR: V4L: raw output not supported for this palette\n");
     1101           }
     1102
     1103    }
     1104
     1105 */
     1106
     1107
     1108    if (PALETTE_YUYV == 1){
     1109         memcpy((char *)capture->imageData,
     1110             (char *)capture->buffers[capture->bufferIndex].start,
     1111             capture->imageSize);
     1112    }
     1113    if (PALETTE_UYVY == 1){
     1114        memcpy((char *)capture->imageData,
     1115             (char *)capture->buffers[capture->bufferIndex].start,
     1116             capture->imageSize);
     1117    }
     1118 
     1119 
     1120
     1121   
     1122}
    2241123int V4LCamera::QueueBuffer(int index) {
    2251124    struct v4l2_buffer buf;
Note: See TracChangeset for help on using the changeset viewer.