WhyCan Forum(哇酷开发者社区)

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <getopt.h>           
#include <fcntl.h>            
#include <unistd.h>
#include <errno.h>
#include <malloc.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
 
#include <asm/types.h>        
#include <linux/videodev2.h>

#define BUF_NUM                         4
#define FRAMESINBUF                     1

struct CJ_Buffer {
        void *memory;
        int length;
};

struct CJ_Buffer *gpV4l2Buf;
 
int gMemMapLen;

int file_fd;
char *dev_name = "/dev/video0";
int fd;


int getFullSize(int * full_w, int * full_h)
{
    struct v4l2_frmsizeenum size_enum;
    size_enum.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    size_enum.pixel_format = V4L2_PIX_FMT_NV12;
    size_enum.index = 0;
    if (-1 == ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &size_enum))
    {
        perror("VIDIOC_ENUM_FRAMESIZES failed");  
        return 0;   
    }
    
    *full_w = size_enum.discrete.width;
    *full_h = size_enum.discrete.height;

        printf("getFullSize: %dx%d\n", *full_w, *full_h);
    
    return 1;
}

int tryFmtSize(int *width, int *height)
{
    int ret = -1;
    struct v4l2_format fmt;

    printf("%dx%d", *width, *height);

    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width  = *width;
    fmt.fmt.pix.height = *height;

    //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
    //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_H264;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV12;

    fmt.fmt.pix.field = V4L2_FIELD_NONE;

    ret = ioctl(fd, VIDIOC_TRY_FMT, &fmt);
    if (ret < 0)
    {
        perror("VIDIOC_TRY_FMT failed");
        return ret;
    }

    // driver surpport this size
    *width = fmt.fmt.pix.width;
    *height = fmt.fmt.pix.height;

    return 0;
}

int setContrastValue(int value)
{
    int ret = -1;
    struct v4l2_control ctrl;

    ctrl.id = V4L2_CID_CONTRAST;
    ctrl.value = value;
    ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
    if (ret < 0) {
        perror("VIDIOC_S_CTRL failed");
    } else {
        printf("setContrastValue ok\n");
    }

    return ret;
}
int setBrightnessValue(int value)
{
    struct v4l2_control ctrl;
    int ret = -1;

    ctrl.id = V4L2_CID_BRIGHTNESS;
    ctrl.value = value;
    ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
    if (ret < 0) {
        perror("VIDIOC_S_CTRL failed");
    } else {
        printf("setBrightnessValue ok\n");
    }
    return ret;
}

int setSaturationValue(int value)
{
    struct v4l2_control ctrl;
    int ret = -1;

    ctrl.id = V4L2_CID_SATURATION;
    ctrl.value = value;
    ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
    if (ret < 0) {
        perror("VIDIOC_S_CTRL failed");
    } else {
        printf("setSaturationValue ok\n");
    }
    return ret;
}

int setSharpnessValue(int value)
{
    struct v4l2_control ctrl;
    int ret = -1;

    ctrl.id = V4L2_CID_SHARPNESS;
    ctrl.value = value;
    ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
    if (ret < 0) {
        perror("VIDIOC_S_CTRL failed");
    } else {
        printf("setSharpnessValue ok\n");
    }
    return ret;
}

int setHueValue(int value)
{
    struct v4l2_control ctrl;
    int ret = -1;

    ctrl.id = V4L2_CID_HUE;
    ctrl.value = value;
    ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
    if (ret < 0) {
        perror("VIDIOC_S_CTRL failed");
    } else {
        printf("setHueValue ok\n");
    }

    return ret;
}

int setHflip(int value)
{
    struct v4l2_control ctrl;
    int ret = -1;
    
    ctrl.id = V4L2_CID_HFLIP_THUMB;
    ctrl.value = value;
    printf("V4L2CameraDevice::setHflip value=%d\n", value);
    ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
    if (ret < 0)
    {
        //setSharpnessValue failed
        perror("setHflip thumb failed\n");
    }
    ctrl.id = V4L2_CID_HFLIP;
    ctrl.value = value;
    printf("V4L2CameraDevice::setHflip value=%d\n", value);
    ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
    if (ret < 0)
    {
        //setSharpnessValue failed
        perror("setHflip failed\n");
    }
    return ret;
}

int setVflip(int value)
{
    struct v4l2_control ctrl;
    int ret = -1;
    
    ctrl.id = V4L2_CID_VFLIP_THUMB;
    ctrl.value = value;
    printf("V4L2CameraDevice::setVflip value=%d\n", value);
    ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
    if (ret < 0)
    {
        //setSharpnessValue failed
        perror("setVflip thumb failed111\n");
    }
    ctrl.id = V4L2_CID_VFLIP;
    ctrl.value = value;
    printf("V4L2CameraDevice::setVflip value=%d\n", value);
    ret = ioctl(fd, VIDIOC_S_CTRL, &ctrl);
    if (ret < 0)
    {
        //setSharpnessValue failed
        perror("setVflip failed111\n");
    }

    return ret;
}

int v4l2setCaptureParams(int framerate)
{
    printf("framerate=%d", framerate);
    int ret = -1;
    
    struct v4l2_streamparm params;
    params.parm.capture.timeperframe.numerator = 1;
    params.parm.capture.timeperframe.denominator = framerate;
    params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    params.parm.capture.capturemode = V4L2_MODE_VIDEO;

    printf("VIDIOC_S_PARM mFrameRate: %d, capture mode: %d\n", framerate, params.parm.capture.capturemode);

    ret = ioctl(fd, VIDIOC_S_PARM, &params);
    if (ret < 0) {
        perror("VIDIOC_S_PARM failed");
    } else {
        printf("v4l2setCaptureParams ok\n");
    }

    return ret;
}

void v4l2SetVideoParams(int width, int height, int sub_w, int sub_h, unsigned int pix_fmt)
{
    struct v4l2_format format;

    printf("main:%dx%d, sub:%dx%d, pfmt: 0x%x\n", width, height, sub_w, sub_h, pix_fmt);
    memset(&format, 0, sizeof(format));
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format.fmt.pix.width  = width;
    format.fmt.pix.height = height;
    //format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    //format.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
    //format.fmt.pix.pixelformat = V4L2_PIX_FMT_H264;
    format.fmt.pix.pixelformat = V4L2_PIX_FMT_NV12;
    format.fmt.pix.field = V4L2_FIELD_NONE;

    if ((sub_w > 0 && sub_w < width) && (sub_h > 0 && sub_h < height)) {
        struct v4l2_pix_format sub_fmt;
        memset(&sub_fmt, 0, sizeof(sub_fmt));
        format.fmt.pix.subchannel = &sub_fmt;
        format.fmt.pix.subchannel->width = sub_w;
        format.fmt.pix.subchannel->height = sub_h;
        format.fmt.pix.subchannel->pixelformat = pix_fmt;
        format.fmt.pix.subchannel->field = V4L2_FIELD_NONE;
        format.fmt.pix.subchannel->rot_angle = 0;
        printf("sub_w=%d, sub_h=%d\n", sub_w, sub_h);
    }

    if (ioctl(fd, VIDIOC_S_FMT, &format) < 0)
    {
        perror("VIDIOC_S_FMT failed");
        return;
    }

    return;
}

void initV4l2Buffer()
{
    gpV4l2Buf = (struct CJ_Buffer *)malloc(sizeof(struct CJ_Buffer) * BUF_NUM);
    if (gpV4l2Buf == NULL) {
        perror("alloc V4l2Buffer_tag error!!");
        return;
    }
    memset(gpV4l2Buf, 0, sizeof(struct CJ_Buffer) * BUF_NUM);
}

void setFramesInV4L2BufNum()
{
        unsigned int nFramesInBuf = FRAMESINBUF;
    printf("mFramesInV4l2buf=%u\n", FRAMESINBUF);
    if (ioctl (fd, VIDIOC_SET_NFRAME_BUF, &nFramesInBuf) < 0) {
        perror("VIDIOC_SET_NFRAME_BUF failed");
    }
}

int v4l2ReqBufs(int * buf_cnt)
{
    int ret = 0;
    struct v4l2_requestbuffers rb;

    printf("TO VIDIOC_REQBUFS count: %d\n", *buf_cnt);

    memset(&rb, 0, sizeof(rb));
    rb.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    rb.memory = V4L2_MEMORY_MMAP;
    rb.count  = *buf_cnt;

    ret = ioctl(fd, VIDIOC_REQBUFS, &rb);
    if (ret < 0)
    {
        perror("VIDIOC_REQBUFS failed");
        return ret;
    }

    *buf_cnt = rb.count;
    printf("VIDIOC_REQBUFS count: %d\n", *buf_cnt);

    return 1;
}

int v4l2QueryBuf()
{
        int i;
    int ret = 0;
    struct v4l2_buffer buf;

    for (i = 0; i < BUF_NUM; i++)
    {
        memset (&buf, 0, sizeof (struct v4l2_buffer));
        buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index  = i;

        ret = ioctl (fd, VIDIOC_QUERYBUF, &buf);
        if (ret < 0)
        {
            perror("VIDIOC_QUERYBUF failed");
            return ret;
        }

        gpV4l2Buf[i].memory = mmap(0, buf.length,
                                PROT_READ | PROT_WRITE, 
                                MAP_SHARED, 
                                fd, 
                                buf.m.offset);

                gMemMapLen = buf.length;
        printf("index: %d, mem: %x, len: %d, offset: %x\n", i, (int)gpV4l2Buf[i].memory, buf.length, buf.m.offset);
 
        if (gpV4l2Buf[i].memory == MAP_FAILED)
        {
            perror("mmap failed");
            return -1;
        }
        }

    for (i = 0; i < BUF_NUM; i++)
    {
        memset (&buf, 0, sizeof (struct v4l2_buffer));
        buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index  = i;
        // start with all buffers in queue
        ret = ioctl(fd, VIDIOC_QBUF, &buf);
        if (ret < 0)
        {
            perror("VIDIOC_QBUF failed");
            return ret;
        }
    }

    return 1;
}

int v4l2StartStreaming()
{
    int ret = 0;
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    ret = ioctl (fd, VIDIOC_STREAMON, &type);
    if (ret < 0)
    {
        perror("VIDIOC_STREAMON failed");
        return ret;
    }

    return 1;
}

int v4l2StopStreaming()
{
    int ret = 0;
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    ret = ioctl (fd, VIDIOC_STREAMOFF, &type);
    if (ret < 0)
    {
        perror("VIDIOC_STREAMOFF failed");
        return ret;
    }
    printf("V4L2Camera::v4l2StopStreaming ok\n");

    return 1;
}

int v4l2UnmapBuf()
{
        int i;
    int ret = 0;

    for (i = 0; i < BUF_NUM; i++)
    {
        ret = munmap(gpV4l2Buf[i].memory, gMemMapLen);
        if (ret < 0)
        {
            perror("munmap failed");
            return ret;
        }

        gpV4l2Buf[i].memory = NULL;
    }

    return 1;
}

int main (int argc,char ** argv)
{
        struct v4l2_capability cap;
        struct v4l2_format fmt;
        struct v4l2_pix_format sub_fmt;
        struct v4l2_requestbuffers req;
        struct v4l2_buffer buf; 
    struct v4l2_input inp;
        unsigned int i;
        enum v4l2_buf_type type;
        
        int width, height;
     
     
        fd = open(dev_name, O_RDWR | O_NONBLOCK, 0);

        ioctl (fd, VIDIOC_QUERYCAP, &cap);
        printf("Driver Name:%s\n Card Name:%s\n Bus info:%s\n\n", cap.driver, cap.card, cap.bus_info);
        printf("capabilities:%d\n", cap.capabilities);
        
    if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0 &&
                (cap.capabilities & V4L2_CAP_STREAMING) == 0) 
    {
        perror("cap capabilities format fail");  
        return -2 ;   
    }


    inp.index = 0;
    if (-1 == ioctl(fd, VIDIOC_S_INPUT, &inp))
    {
        perror("VIDIOC_S_INPUT error!");  
        return -2 ;   
    }
        
        getFullSize(&width, &height);
        
        width = 640; height = 480;
        /*tryFmtSize(&width, &height);
        setContrastValue(50);
        setBrightnessValue(50);
        setSaturationValue(0);
        setSharpnessValue(10);
        setHueValue(30);
        setHflip(0);
        setVflip(0);*/
        
        v4l2setCaptureParams(30);
        v4l2SetVideoParams(640, 480, 320, 240, V4L2_PIX_FMT_NV12);
        
        initV4l2Buffer();
        setFramesInV4L2BufNum();

    int buf_cnt = BUF_NUM;
    v4l2ReqBufs(&buf_cnt);
        
        v4l2QueryBuf();
        v4l2StartStreaming();
        
        //for (i = 0; i < 10; i++)
        //      sleep(1);

    //struct v4l2_control ctrl;

   // ctrl.id = V4L2_CID_TAKE_PICTURE;
    //if (ioctl(fd, VIDIOC_S_CTRL, &ctrl) < 0) {
  //      perror("VIDIOC_S_CTRL failed");
  //  } else {
  //      perror("setTakePictureCtrl ok\n");
 //   }
        
        
        FILE* fp = fopen("test2.nv12", "w");
        if(fp == NULL)
        {
                printf("open file error.\n");
                goto end;
        }

        for (i = 0; i < 200; i++)
        {
                fd_set fds;

                FD_ZERO(&fds);
                FD_SET(fd, &fds);

                select(fd + 1, &fds, NULL, NULL, NULL);

            memset(&buf, 0, sizeof(struct v4l2_buffer));
                buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                buf.memory = V4L2_MEMORY_MMAP;
                ioctl(fd, VIDIOC_DQBUF, &buf);

            void *pMem = gpV4l2Buf[buf.index].memory;
                printf("frame addr = %x, length=%d, %d\n", (unsigned int)pMem, gpV4l2Buf[buf.index].length, buf.length);

                fwrite(pMem, 640 * 480 * 3 / 2, 1, fp);
#if 0
                if (i == 199)
                {
                        file_fd = open("test.jpg", O_RDWR | O_CREAT, 0777);
                        write(file_fd, pMem, 640 * 480 * 3 / 2);
                        close (file_fd);
                        printf("frame index = %d, size = %d.\n", buf.index, buf.length);
                }
#endif
                ioctl(fd, VIDIOC_QBUF, &buf);
        }

end:
        fclose(fp);     
        v4l2StopStreaming();
        v4l2UnmapBuf();
        

        close (fd);
        printf("Camera Done.\n");


        return 0;
}