v4l2uvc.c

机器人仿真软件

    int ret;

    if (!vd->isstreaming)
       if (video_enable(vd))
           goto err;
    memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
    vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    vd->buf.memory = V4L2_MEMORY_MMAP;
    ret = ioctl(vd->fd, VIDIOC_DQBUF, &vd->buf);
    if (ret < 0) {
       printf("Unable to dequeue buffer (%d).\n", errno);
       goto err;
    }
    switch (vd->formatIn) {
    case V4L2_PIX_FMT_MJPEG:

       memcpy(vd->tmpbuffer, vd->mem[vd->buf.index], HEADERFRAME1);
       memcpy(vd->tmpbuffer + HEADERFRAME1, dht_data, DHT_SIZE);
       memcpy(vd->tmpbuffer + HEADERFRAME1 + DHT_SIZE,
              vd->mem[vd->buf.index] + HEADERFRAME1,
              (vd->buf.bytesused - HEADERFRAME1));
       if (debug)
           printf("bytes in used %d \n", vd->buf.bytesused);
       break;
    case V4L2_PIX_FMT_YUYV:
       if (vd->buf.bytesused > vd->framesizeIn)
           memcpy(vd->framebuffer, vd->mem[vd->buf.index],
                 (size_t) vd->framesizeIn);
       else
           memcpy(vd->framebuffer, vd->mem[vd->buf.index],
                 (size_t) vd->buf.bytesused);
       break;
    default:
       goto err;
       break;
    }
    ret = ioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
    if (ret < 0) {
       printf("Unable to requeue buffer (%d).\n", errno);
       goto err;
    }

    if (convertframe(vd) < 0)
       goto err;
    return 0;
  err:
    vd->signalquit = 0;
    return -1;
}
int close_v4l2(struct vdIn *vd)
{
	int i,ret;
		
    if (vd->isstreaming)
       video_disable(vd);

    /* unmap the buffers */
    for (i = 0; i < NB_BUFFER; i++) {
       memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
       vd->buf.index = i;
       vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
       vd->buf.memory = V4L2_MEMORY_MMAP;
       ret = ioctl(vd->fd, VIDIOC_QUERYBUF, &vd->buf);
       if (ret < 0) {
           printf("Unable to query buffer (%d).\n", errno);
           continue;
       }
       ret = munmap(vd->mem[i], vd->buf.length);
       if (ret < 0) {
           printf("Unable to UNmap buffer (%d)\n", errno);
       }
    }


    close(vd->fd);

    if (vd->tmpbuffer)
       free(vd->tmpbuffer);
    vd->tmpbuffer = NULL;
    free(vd->framebuffer);
    vd->framebuffer = NULL;
    free(vd->videodevice);
    free(vd->status);
    free(vd->pictName);
    vd->videodevice = NULL;
    vd->status = NULL;
    vd->pictName = NULL;
}

/* return >= 0 ok otherwhise -1 */
static int isv4l2Control(struct vdIn *vd, int control,
                      struct v4l2_queryctrl *queryctrl)
{
int err =0;
    queryctrl->id = control;
    if ((err= ioctl(vd->fd, VIDIOC_QUERYCTRL, queryctrl)) < 0) {
       printf("ioctl querycontrol error %d \n",errno);
    } else if (queryctrl->flags & V4L2_CTRL_FLAG_DISABLED) {
       printf("control %s disabled \n", (char *) queryctrl->name);
    } else if (queryctrl->flags & V4L2_CTRL_TYPE_BOOLEAN) {
       return 1;
    } else if (queryctrl->type & V4L2_CTRL_TYPE_INTEGER) {
       return 0;
    } else {
       printf("contol %s unsupported  \n", (char *) queryctrl->name);
    }
    return -1;
}

int v4l2GetControl(struct vdIn *vd, int control)
{
    struct v4l2_queryctrl queryctrl;
    struct v4l2_control control_s;
    int err;
    if (isv4l2Control(vd, control, &queryctrl) < 0)
       return -1;
    control_s.id = control;
    if ((err = ioctl(vd->fd, VIDIOC_G_CTRL, &control_s)) < 0) {
       printf("ioctl get control error\n");
       return -1;
    }
    return control_s.value;
}

int v4l2SetControl(struct vdIn *vd, int control, int value)
{
    struct v4l2_control control_s;
    struct v4l2_queryctrl queryctrl;
    int min, max, current, step, val_def;
    int err;
    if (isv4l2Control(vd, control, &queryctrl) < 0)
       return -1;
    min = queryctrl.minimum;
    max = queryctrl.maximum;
    step = queryctrl.step;
    val_def = queryctrl.default_value;
    if ((value >= min) && (value <= max)) {
       control_s.id = control;
       control_s.value = value;
       if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
           printf("ioctl set control error\n");
           return -1;
       }
    }
    return 0;
}
int v4l2UpControl(struct vdIn *vd, int control)
{
    struct v4l2_control control_s;
    struct v4l2_queryctrl queryctrl;
    int min, max, current, step, val_def;
    int err;

    if (isv4l2Control(vd, control, &queryctrl) < 0)
       return -1;
    min = queryctrl.minimum;
    max = queryctrl.maximum;
    step = queryctrl.step;
    val_def = queryctrl.default_value;
    current = v4l2GetControl(vd, control);
    current += step;
    if (current <= max) {
       control_s.id = control;
       control_s.value = current;
       if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
           printf("ioctl set control error\n");
           return -1;
       }
    }
    return control_s.value;
}
int v4l2DownControl(struct vdIn *vd, int control)
{
    struct v4l2_control control_s;
    struct v4l2_queryctrl queryctrl;
    int min, max, current, step, val_def;
    int err;
    if (isv4l2Control(vd, control, &queryctrl) < 0)
       return -1;
    min = queryctrl.minimum;
    max = queryctrl.maximum;
    step = queryctrl.step;
    val_def = queryctrl.default_value;
    current = v4l2GetControl(vd, control);
    current -= step;
    if (current >= min) {
       control_s.id = control;
       control_s.value = current;
       if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
           printf("ioctl set control error\n");
           return -1;
       }
    }
    return control_s.value;
}
int v4l2ToggleControl(struct vdIn *vd, int control)
{
    struct v4l2_control control_s;
    struct v4l2_queryctrl queryctrl;
    int current;
    int err;
    if (isv4l2Control(vd, control, &queryctrl) != 1)
       return -1;
    current = v4l2GetControl(vd, control);
    control_s.id = control;
    control_s.value = !current;
    if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
       printf("ioctl toggle control error\n");
       return -1;
    }
    return control_s.value;
}
int v4l2ResetControl(struct vdIn *vd, int control)
{
    struct v4l2_control control_s;
    struct v4l2_queryctrl queryctrl;
    int val_def;
    int err;
    if (isv4l2Control(vd, control, &queryctrl) < 0)
       return -1;
    val_def = queryctrl.default_value;
    control_s.id = control;
    control_s.value = val_def;
    if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
       printf("ioctl reset control error\n");
       return -1;
    }

    return 0;
}
int v4l2ResetPanTilt(struct vdIn *vd,int pantilt)
{
    int control = V4L2_CID_PANTILT_RESET;
    struct v4l2_control control_s;
    struct v4l2_queryctrl queryctrl;
    unsigned char val;
    int err;
    if (isv4l2Control(vd, control, &queryctrl) < 0)
       return -1;
    val = (unsigned char) pantilt;
    control_s.id = control;
    control_s.value = val;
    if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
       printf("ioctl reset Pan control error\n");
       return -1;
    }

    return 0;
}
union pantilt {
       struct {
              short pan;
              short tilt;
       } s16;
       int value;
} pantilt;
       
int v4L2UpDownPan(struct vdIn *vd, short inc)
{   int control = V4L2_CID_PANTILT_RELATIVE;
    struct v4l2_control control_s;
    struct v4l2_queryctrl queryctrl;
    int err;
    
   union pantilt pan;
   
       control_s.id = control;
     if (isv4l2Control(vd, control, &queryctrl) < 0)
        return -1;

  pan.s16.pan = inc;
  pan.s16.tilt = 0;
 
       control_s.value = pan.value ;
    if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
       printf("ioctl pan updown control error\n");
       return -1;
       }
       return 0;
}

int v4L2UpDownTilt(struct vdIn *vd, short inc)
{   int control = V4L2_CID_PANTILT_RELATIVE;
    struct v4l2_control control_s;
    struct v4l2_queryctrl queryctrl;
    int err;
     union pantilt pan;  
       control_s.id = control;
     if (isv4l2Control(vd, control, &queryctrl) < 0)
       return -1;  

    pan.s16.pan= 0;
    pan.s16.tilt = inc;
  
       control_s.value = pan.value;
    if ((err = ioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
       printf("ioctl tiltupdown control error\n");
       return -1;
       }
       return 0;
}