v4l2测试源码截图

#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
 
 
#define NB_BUFFER 4
#define FB_DEVICE_NAME "/dev/fb0"
static int iWidth;
static int iHeight;
static int iVideoBufCnt;
static unsigned char *pucVideBuf[NB_BUFFER];
static int VideoBufMaxLen;
static int iFd;
static int VideoBufCurIndex;
static int iPixelFormat;
static struct fb_var_screeninfo g_tFBVar;
static int g_aiSupportedFormats[] = {V4L2_PIX_FMT_YUYV, V4L2_PIX_FMT_MJPEG, V4L2_PIX_FMT_RGB565};
 
 
 
 
static int *LutYr = NULL;
static int *LutYg = NULL;;
static int *LutYb = NULL;;
static int *LutVr = NULL;;
static int *LutVrY = NULL;;
static int *LutUb = NULL;;
static int *LutUbY = NULL;;
static int *LutRv = NULL;
static int *LutGu = NULL;
static int *LutGv = NULL;
static int *LutBu = NULL;
void initLut(void)
{
    int i;
    #define Rcoef 299
    #define Gcoef 587
    #define Bcoef 114
    #define Vrcoef 711 //656 //877
    #define Ubcoef 560 //500 //493 564
    
    #define CoefRv 1402
    #define CoefGu 714 // 344
    #define CoefGv 344 // 714
    #define CoefBu 1772
    
    LutYr = malloc(256*sizeof(int));
    LutYg = malloc(256*sizeof(int));
    LutYb = malloc(256*sizeof(int));
    LutVr = malloc(256*sizeof(int));
    LutVrY = malloc(256*sizeof(int));
    LutUb = malloc(256*sizeof(int));
    LutUbY = malloc(256*sizeof(int));
    
    LutRv = malloc(256*sizeof(int));
    LutGu = malloc(256*sizeof(int));
    LutGv = malloc(256*sizeof(int));
    LutBu = malloc(256*sizeof(int));
    for (i= 0;i < 256;i++){
        LutYr[i] = i*Rcoef/1000 ;
        LutYg[i] = i*Gcoef/1000 ;
        LutYb[i] = i*Bcoef/1000 ;
        LutVr[i] = i*Vrcoef/1000;
        LutUb[i] = i*Ubcoef/1000;
        LutVrY[i] = 128 -(i*Vrcoef/1000);
        LutUbY[i] = 128 -(i*Ubcoef/1000);
        LutRv[i] = (i-128)*CoefRv/1000;
        LutBu[i] = (i-128)*CoefBu/1000;
        LutGu[i] = (128-i)*CoefGu/1000;
        LutGv[i] = (128-i)*CoefGv/1000;
    }    
}
#define CLIP(color) (unsigned char)(((color)>0xFF)?0xff:(((color)<0)?0:(color)))
R_FROMYV(unsigned char y, unsigned char v)
{
return CLIP((y) + LutRv[(v)]);
}
unsigned char
G_FROMYUV(unsigned char y, unsigned char u, unsigned char v)
{
return CLIP((y) + LutGu[(u)] + LutGv[(v)]);
}
unsigned char
B_FROMYU(unsigned char y, unsigned char u)
{
return CLIP((y) + LutBu[(u)]);
}
static void
Pyuv422torgb24(unsigned char * input_ptr, unsigned char * output_ptr, unsigned int image_width, unsigned int image_height)
{
    unsigned int i, size;
    unsigned char Y, Y1, U, V;
    unsigned char *buff = input_ptr;
    unsigned char *output_pt = output_ptr;
    size = image_width * image_height /2;
    for (i = size; i > 0; i--) {
        /* bgr instead rgb ?? */
        Y = buff[0] ;
        U = buff[1] ;
        Y1 = buff[2];
        V = buff[3];
        buff += 4;
        *output_pt++ = R_FROMYV(Y,V);
        *output_pt++ = G_FROMYUV(Y,U,V); //b
        *output_pt++ = B_FROMYU(Y,U); //v
            
        *output_pt++ = R_FROMYV(Y1,V);
        *output_pt++ = G_FROMYUV(Y1,U,V); //b
        *output_pt++ = B_FROMYU(Y1,U); //v
    }
    
    return ;
}  
static void  
 getPictureName (char *Picture, int fmt)
{
char temp[80];
  char *myext[] = { "pnm", "jpg" };
  int i;
  time_t curdate;
  struct tm *tdate;
  memset (temp, '\0', sizeof (temp));
  time (&curdate);
  tdate = localtime (&curdate);
  snprintf (temp, 26, "P-%02d:%02d:%04d-%02d:%02d:%02d.%s\0",
        tdate->tm_mon + 1, tdate->tm_mday, tdate->tm_year + 1900,
        tdate->tm_hour, tdate->tm_min, tdate->tm_sec, myext[fmt]);
 
  memcpy (Picture, temp, strlen (temp));
}
int
get_pictureYV2(unsigned char *buf,int width,int height)
{
FILE *foutpict;
unsigned char *picture = NULL;
char *name = NULL;
name = calloc(80,1);
getPictureName (name, 0);
picture = (unsigned char *)malloc(width*height*3*sizeof(char));
if(picture){
    Pyuv422torgb24(buf, picture, width, height);
}else{
    printf(" no room to take a picture \n");
    return 0;
}
if(name){
    foutpict = fopen (name, "wb");
    printf("%s",name);
    fprintf (foutpict, "P6\n%d %d\n255\n", width, height);
    fwrite (picture, sizeof (char), width * height * 3, foutpict);
    fclose (foutpict);
    free(name);
}
free(picture);
picture = NULL;
return 0;
}
static int isSupportThisFormat(int iPixelFormat)
{
    int i;
    for (i = 0; i < sizeof(g_aiSupportedFormats)/sizeof(g_aiSupportedFormats[0]); i++)
    {
        if (g_aiSupportedFormats[i] == iPixelFormat)
            return 1;
    }
    return 0;
}
 
 
static int V4l2InitDevice(char *strDevName)
{
           int iError;
    int g_fd;
    int i;
     
           struct v4l2_capability tV4l2Cap;
    struct v4l2_fmtdesc tFmtDesc;
    struct v4l2_format  tV4l2Fmt;
    struct v4l2_requestbuffers tV4l2ReqBuffs;
    struct v4l2_buffer tV4l2Buf;
 
    initLut();
           iFd = open("/dev/video0", O_RDWR);
           if (iFd < 0)
               {
                   printf("can not open \n");
                   return -1;
               }
 
 
 
    
           iError = ioctl(iFd, VIDIOC_QUERYCAP, &tV4l2Cap);
           memset(&tV4l2Cap, 0, sizeof(struct v4l2_capability));
           iError = ioctl(iFd, VIDIOC_QUERYCAP, &tV4l2Cap);
           if (iError)
               {
                   printf("Error opening device enable to query device.\n");
        }
           if(!(tV4l2Cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
               {
                   printf("this is not a video capture device\n");
                   goto err_exit;
               }
    if (tV4l2Cap.capabilities & V4L2_CAP_STREAMING)
               {
                    
               }
    if (tV4l2Cap.capabilities & V4L2_CAP_READWRITE)
        {
                   printf("supports readwrite i/o\n");
        }
 
 
 
    memset(&tFmtDesc, 0, sizeof(tFmtDesc));
    tFmtDesc.index = 0;
    tFmtDesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    while((iError = ioctl(iFd, VIDIOC_ENUM_FMT, &tFmtDesc))==0)
    {
        if (isSupportThisFormat(tFmtDesc.pixelformat))
            {
                iPixelFormat = tFmtDesc.pixelformat;
                break;
            }
            tFmtDesc.index++;
    }
    if (!iPixelFormat)
    {
        printf("can not support the format of this device\n");
                goto err_exit;         
        }
 
 
 
 
 
    g_fd = open(FB_DEVICE_NAME, O_RDWR);
       ioctl(g_fd, FBIOGET_VSCREENINFO, &g_tFBVar);
     memset(&tV4l2Fmt, 0, sizeof(struct v4l2_format));
        tV4l2Fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
         tV4l2Fmt.fmt.pix.pixelformat = iPixelFormat;
        tV4l2Fmt.fmt.pix.width         = g_tFBVar.xres;
         tV4l2Fmt.fmt.pix.height       = g_tFBVar.yres;
         tV4l2Fmt.fmt.pix.field            = V4L2_FIELD_ANY;
    iError = ioctl(iFd, VIDIOC_S_FMT, &tV4l2Fmt);
        if (iError)
          {
                printf("Unable to set format\n");
                    goto err_exit;         
            }
 
 
     
        iWidth  = tV4l2Fmt.fmt.pix.width;
        iHeight = tV4l2Fmt.fmt.pix.height;
            memset(&tV4l2ReqBuffs, 0, sizeof(struct v4l2_requestbuffers));
        tV4l2ReqBuffs.count = NB_BUFFER;
        tV4l2ReqBuffs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        tV4l2ReqBuffs.memory = V4L2_MEMORY_MMAP;
        iError = ioctl(iFd, VIDIOC_REQBUFS, &tV4l2ReqBuffs);
        if (iError)
        {
            printf("Unable to allocate buffers.\n");
                goto err_exit;         
        }
        iVideoBufCnt = tV4l2ReqBuffs.count;
     
       if (tV4l2Cap.capabilities & V4L2_CAP_STREAMING)
        {
                for (i = 0; i < iVideoBufCnt; i++)
                {
            memset(&tV4l2Buf, 0, sizeof(struct v4l2_buffer));
                    tV4l2Buf.index = i;
                    tV4l2Buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                    tV4l2Buf.memory = V4L2_MEMORY_MMAP;
                    iError = ioctl(iFd, VIDIOC_QUERYBUF, &tV4l2Buf);
                    if (iError)
                    {
                           printf("Unable to query buffer.\n");
                            goto err_exit;
                    }
                    VideoBufMaxLen = tV4l2Buf.length;
                    pucVideBuf[i] = mmap(0 ,tV4l2Buf.length, PROT_READ, MAP_SHARED, iFd,tV4l2Buf.m.offset);
            //for this i do not know why use the prot_read or map_shared ,just from man mmap
            //you should use              int munmap(void *addr, size_t length);
            //in order to free the mmap
                    if (pucVideBuf == MAP_FAILED)
                    {
                            printf("Unable to map buffer\n");
                            goto err_exit;
                    }
                }         
        for (i = 0; i < iVideoBufCnt; i++)
                {
                    memset(&tV4l2Buf, 0, sizeof(struct v4l2_buffer));
                    tV4l2Buf.index = i;
                    tV4l2Buf.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                    tV4l2Buf.memory = V4L2_MEMORY_MMAP;
                    iError = ioctl(iFd, VIDIOC_QBUF, &tV4l2Buf);
                    if (iError)
                    {
                            printf("Unable to queue buffer111111111.\n");
                            goto err_exit;
                    }
                }
         
    }
     
 
   // ptVideoDevice->ptOPr = &g_tV4l2VideoOpr;
        return 0;
 
    err_exit:     
        close(g_fd);
        close(iFd);
    return -1;     
}
 
static int V4l2StartDevice()
{
    int iType = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        int iError;
        iError = ioctl(iFd, VIDIOC_STREAMON, &iType);
        if (iError)
        {
            printf("Unable to start capture.\n");
            return -1;
        }
        return 0;
}
static int V4l2StopDevice()
{
        int iType = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        int iError;
        iError = ioctl(iFd, VIDIOC_STREAMOFF, &iType);
        if (iError)
        {
            printf("Unable to stop capture.\n");
            return -1;
        }
    printf("stream is off\n");
        return 0;
}
static int V4l2GetFrameForStreaming()
{
    struct pollfd tFds[1];
        int iRet;
       int fp;
    char *name = NULL;
 
    
    FILE *foutpict;
 
 
    
        struct v4l2_buffer tV4l2Buf;
           unsigned char *pucVideBufout;
     
        tFds[0].fd     = iFd;         
        tFds[0].events = POLLIN;
 
        iRet = poll(tFds, 1, -1);
       if (iRet <= 0)
        {
                printf("poll error!\n");
                return -1;
        }
     
        memset(&tV4l2Buf, 0, sizeof(struct v4l2_buffer));
        tV4l2Buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        tV4l2Buf.memory = V4L2_MEMORY_MMAP;
        iRet = ioctl(iFd, VIDIOC_DQBUF, &tV4l2Buf);
        if (iRet < 0)
        {
            printf("Unable to dequeue buffer.\n");
            return -1;
        }
    VideoBufCurIndex = tV4l2Buf.index;
    get_pictureYV2(pucVideBuf[tV4l2Buf.index],iWidth,iHeight);
    sleep(2);        
       return 0;
}
 
 
static int V4l2PutFrameForStreaming()
{
        /* VIDIOC_QBUF */
        struct v4l2_buffer tV4l2Buf;         
        int iError;
     
    memset(&tV4l2Buf, 0, sizeof(struct v4l2_buffer));
    tV4l2Buf.index  = VideoBufCurIndex;
    tV4l2Buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    tV4l2Buf.memory = V4L2_MEMORY_MMAP;
    iError = ioctl(iFd, VIDIOC_QBUF, &tV4l2Buf);
    if (iError)
            {
                return -1;
        }
        return 0;
}
static int V4l2ExitDevice()
{
    int i;
    for (i = 0; i < iVideoBufCnt; i++)
    {
        if (pucVideBuf[i])
        {
            munmap(pucVideBuf[i], VideoBufMaxLen);
            pucVideBuf[i] = NULL;
        }
    }
         
    close(iFd);
    return 0;
}
main()
{
    V4l2InitDevice("ok");
    V4l2StartDevice();
 
     
    while(1)
    {
        if(V4l2GetFrameForStreaming())
        {
            printf("error,please check!\n");
            break;
        }
        V4l2PutFrameForStreaming();
    }
    V4l2StopDevice();
    V4l2ExitDevice();

}

love 钓鱼竿