imx6q v4l2 输入部分ok

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <stdint.h>
#include <sys/types.h>
#include <stdint.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <unistd.h>
#include <asm/types.h>
#include <linux/videodev2.h>
#include <sys/mman.h>
#include <math.h>
#include <string.h>
#include <malloc.h>
#include <sys/time.h>
#include <linux/mxcfb.h>
#include <linux/mxc_v4l2.h>
#include <linux/ipu.h>


#include "Track.h"
#include<sys/types.h>
#include<sys/stat.h>
#include<fcntl.h>
#include <stdio.h>
#define WIDTH 720
#define HEIGHT 576


char v4l_capture_dev[100] = "/dev/video0";
char v4l_output_dev[100] = "/dev/video17";
int fd_capture_v4l = 0;
int fd_output_v4l = 0;
int g_input = 1;
int g_output_num_buffers = 4;
int g_capture_num_buffers = 3;
int g_in_width = 0;
int g_in_height = 0;


/*************track variable***************************/
//unsigned char Template[64][64];     
int dis_w, dis_h;
//int track_x;
//int track_y;
extern void Init_Addr(void);
extern int TmpH;
extern int TmpW;
extern int gSubImageWidth;
extern int gSubImageHeight;
extern unsigned short tar_x,tar_y;
extern unsigned char *gSubImageData;
static float f32_NccRatio = 0.01f;
int Cnt_Track = 0;//检测次数统计
extern unsigned char *Template;
extern unsigned char *Template2;


//*****************************************************
void WinCross(
int x_in, //波门列坐标
int y_in, //波门行坐标
int w_in, //波门宽度
int h_in, //波门高度
int flag_clear, //清除标志位
int flag_loss); //丢失标志位


void WinCubicEmpty(
int x_in, //波门列坐标
int y_in, //波门行坐标
int w_in, //波门宽度
int h_in, //波门高度
int flag_clear, //清除标志位
int flag_loss); //丢失标志位


/*******************************************/


struct testbuffer
{
unsigned char *start;
size_t offset;
unsigned int length;
};


struct testbuffer output_buffers[4];
struct testbuffer capture_buffers[3];


int v4l_capture_setup(void)
{
struct v4l2_capability cap;
struct v4l2_cropcap cropcap;
struct v4l2_control ctl;
struct v4l2_crop crop;
struct v4l2_format fmt;
struct v4l2_requestbuffers req;
struct v4l2_dbg_chip_ident chip;
struct v4l2_streamparm parm;
v4l2_std_id id = V4L2_STD_PAL;//NTSC;


if (ioctl (fd_capture_v4l, VIDIOC_QUERYCAP, &cap) < 0) {
if (EINVAL == errno) {
fprintf (stderr, "%s is no V4L2 device\n",
v4l_capture_dev);
return -1;
} else {
fprintf (stderr, "%s isn not V4L device,unknow error\n",
v4l_capture_dev);
return -1;
}
}


if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
fprintf (stderr, "%s is no video capture device\n",
v4l_capture_dev);
return -1;
}


if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
fprintf (stderr, "%s does not support streaming i/o\n",
v4l_capture_dev);
return -1;
}


if (ioctl(fd_capture_v4l, VIDIOC_DBG_G_CHIP_IDENT, &chip))
{
printf("VIDIOC_DBG_G_CHIP_IDENT failed.\n");
close(fd_capture_v4l);
return -1;
}


if (ioctl(fd_capture_v4l, VIDIOC_S_INPUT, &g_input) < 0)
{
printf("VIDIOC_S_INPUT failed\n");
close(fd_capture_v4l);
return -1;
}
/*
if (ioctl(fd_capture_v4l, VIDIOC_G_STD, &id) < 0)
{
printf("VIDIOC_G_STD failed\n");
close(fd_capture_v4l);
return -1;
}


if (ioctl(fd_capture_v4l, VIDIOC_S_STD, &id) < 0)
{
printf("VIDIOC_S_STD failed\n");
close(fd_capture_v4l);
return -1;
}
*/


/* Select video input, video standard and tune here. */


memset(&cropcap, 0, sizeof(cropcap));


cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;


if (ioctl (fd_capture_v4l, VIDIOC_CROPCAP, &cropcap) < 0) {
crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
crop.c = cropcap.defrect; /* reset to default */


if (ioctl (fd_capture_v4l, VIDIOC_S_CROP, &crop) < 0) {
switch (errno) {
case EINVAL:
/* Cropping not supported. */
fprintf (stderr, "%s  doesn't support crop\n",
v4l_capture_dev);
break;
default:
/* Errors ignored. */
break;
}
}
} else {
/* Errors ignored. */
}
ctl.id = V4L2_CID_PRIVATE_BASE;
ctl.value = 0;
if (ioctl(fd_capture_v4l, VIDIOC_S_CTRL, &ctl) < 0)
{
printf("set control failed\n");
return -1;
}


parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
parm.parm.capture.timeperframe.numerator = 1;
parm.parm.capture.timeperframe.denominator = 30;

parm.parm.capture.capturemode = 3;
if (ioctl(fd_capture_v4l, VIDIOC_S_PARM, &parm) < 0)
{
printf("VIDIOC_S_PARM failed\n");
close(fd_capture_v4l);
return -1;
}


memset(&fmt, 0, sizeof(fmt));


fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width       = 720;//WIDTH;
fmt.fmt.pix.height      = 576;//HEIGHT;
fmt.fmt.pix.bytesperline = 720;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;


if (ioctl (fd_capture_v4l, VIDIOC_S_FMT, &fmt) < 0){
fprintf (stderr, "%s iformat not supported \n",
v4l_capture_dev);
return -1;
}


/* Note VIDIOC_S_FMT may change width and height. */


if (ioctl(fd_capture_v4l, VIDIOC_G_FMT, &fmt) < 0)
{
printf("VIDIOC_G_FMT failed\n");
close(fd_capture_v4l);
return -1;
}


g_in_width = fmt.fmt.pix.width;
printf("picture width is %d\n",g_in_width);
g_in_height = fmt.fmt.pix.height;
printf("picture height is %d\n",g_in_height);


memset(&req, 0, sizeof (req));


req.count               = g_capture_num_buffers;
req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory              = V4L2_MEMORY_MMAP;


if (ioctl (fd_capture_v4l, VIDIOC_REQBUFS, &req) < 0) 
{
if (EINVAL == errno) 
{
fprintf (stderr, "%s does not support "
"memory mapping\n", v4l_capture_dev);
return -1;
} 
else
{
fprintf (stderr, "%s does not support "
"memory mapping, unknow error\n", v4l_capture_dev);
return -1;
}
}


if (req.count < 2) {
fprintf (stderr, "Insufficient buffer memory on %s\n",
v4l_capture_dev);
return -1;
}


return 0;
}


int start_capturing(void)
{
        unsigned int i;
        struct v4l2_buffer buf;
        enum v4l2_buf_type type;


        for (i = 0; i < g_capture_num_buffers; i++)
        {
                memset(&buf, 0, sizeof (buf));
                buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                buf.memory = V4L2_MEMORY_MMAP;
                buf.index = i;
                if (ioctl(fd_capture_v4l, VIDIOC_QUERYBUF, &buf) < 0)
                {
                        printf("VIDIOC_QUERYBUF error\n");
                        return -1;
                }


                capture_buffers[i].length = buf.length;
                capture_buffers[i].offset = (size_t) buf.m.offset;
                capture_buffers[i].start = mmap (NULL, capture_buffers[i].length,
                    PROT_READ | PROT_WRITE, MAP_SHARED,
                    fd_capture_v4l, capture_buffers[i].offset);
memset(capture_buffers[i].start, 0xFF, capture_buffers[i].length);
}


for (i = 0; i < g_capture_num_buffers; i++)
{
memset(&buf, 0, sizeof (buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
 buf.m.offset = capture_buffers[i].offset;
if (ioctl (fd_capture_v4l, VIDIOC_QBUF, &buf) < 0)
{
printf("VIDIOC_QBUF error\n");
return -1;
}
}


type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (ioctl (fd_capture_v4l, VIDIOC_STREAMON, &type) < 0) 
{
printf("VIDIOC_STREAMON error\n");
return -1;
}


return 0;
}


void process_image(const void *p, int size)
{
        FILE *fp_tmp;
    
        fp_tmp = fopen("buf_index1.yuv","w");
        fwrite(p,size, 1, fp_tmp);
printf("write yuv ok\n");
        fclose(fp_tmp);
}


void process_image1(const void *p, int size)
{
        FILE *fp_tmp;
    
        fp_tmp = fopen("buf_index2.yuv","w");
        fwrite(p,size, 1, fp_tmp);
printf("write yuv ok\n");
        fclose(fp_tmp);
}


int get_input_camera(struct v4l2_buffer input_buf)
{
memset(&input_buf, 0, sizeof(input_buf));
input_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
input_buf.memory = V4L2_MEMORY_MMAP;


if((ioctl (fd_capture_v4l, VIDIOC_DQBUF, &input_buf)) < 0)
{
printf("DQBUF error!\n");
}


process_image(capture_buffers[input_buf.index].start, input_buf.bytesused);//houjia
// ioctl (fd_capture_v4l, VIDIOC_QBUF, &input_buf); 
return 0;
}


int v4l_output_setup(void)
{
struct v4l2_format fmt;
struct v4l2_framebuffer fb;
struct v4l2_cropcap cropcap;
struct v4l2_capability cap;
struct v4l2_requestbuffers buf_req;


if (!ioctl(fd_output_v4l, VIDIOC_QUERYCAP, &cap)) {
printf("driver=%s, card=%s, bus=%s, "
"version=0x%08x, "
"capabilities=0x%08x\n",
cap.driver, cap.card, cap.bus_info,
cap.version,
cap.capabilities);
}


memset(&cropcap, 0, sizeof(cropcap));
cropcap.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
if (ioctl(fd_output_v4l, VIDIOC_CROPCAP, &cropcap) < 0)
{
printf("get crop capability failed\n");
close(fd_output_v4l);
return -1;
}


fb.flags = V4L2_FBUF_FLAG_OVERLAY;
ioctl(fd_output_v4l, VIDIOC_S_FBUF, &fb);


memset(&fmt, 0, sizeof(fmt));
fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
fmt.fmt.pix.width= g_in_width;
fmt.fmt.pix.height= g_in_height;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
fmt.fmt.pix.bytesperline = g_in_width;
fmt.fmt.pix.priv = 0;
fmt.fmt.pix.sizeimage = 0;
fmt.fmt.pix.field = V4L2_FIELD_INTERLACED_TB;


if (ioctl(fd_output_v4l, VIDIOC_S_FMT, &fmt) < 0)
{
printf("set format failed\n");
return -1;
}


if (ioctl(fd_output_v4l, VIDIOC_G_FMT, &fmt) < 0)
{
printf("get format failed\n");
return -1;
}


memset(&buf_req, 0, sizeof(buf_req));
buf_req.count = g_output_num_buffers;
buf_req.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
buf_req.memory = V4L2_MEMORY_MMAP;
if (ioctl(fd_output_v4l, VIDIOC_REQBUFS, &buf_req) < 0)
{
printf("request buffers failed\n");
return -1;
}


return 0;
}


int prepare_output(void)
{
int i;
struct v4l2_buffer output_buf;


for (i = 0; i < g_output_num_buffers; i++)
{
memset(&output_buf, 0, sizeof(output_buf));
output_buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
output_buf.memory = V4L2_MEMORY_MMAP;
output_buf.index = i;
if (ioctl(fd_output_v4l, VIDIOC_QUERYBUF, &output_buf) < 0)
{
printf("VIDIOC_QUERYBUF error\n");
return -1;
}


output_buffers[i].length = output_buf.length;
output_buffers[i].offset = (size_t) output_buf.m.offset;
output_buffers[i].start = mmap (NULL, output_buffers[i].length,
PROT_READ | PROT_WRITE, MAP_SHARED,
fd_output_v4l, output_buffers[i].offset);
if (output_buffers[i].start == NULL) {
printf("v4l2 tvin test: output mmap failed\n");
return -1;
}
}
return 0;
}
void draw_box(unsigned char *Y,int dot_up,int dot_left,int size_width,int size_heiht)
{
int m,n;
/***********************图片中心点画64*64的方框,而在原图中为128*64*********************/
for(m=0;m<64;m++)
{
Y[(dot_up+m)*size_width+dot_left] =255;
}

for(n=0;n<128;n++)
{
Y[(dot_up)*size_width+dot_left+n] =255;
}

for(n=0;n<128;n++)
{
Y[(dot_up+64)*size_width+(dot_left+n)] =255;
}

for(m=0;m<64;m++)
{
Y[(dot_up+m)*size_width+(dot_left+128)] =255;
}
/**************************************************************************************/


}
int 
v4l_tvin(void)
{
struct v4l2_buffer capture_buf, output_buf;
int i;
enum v4l2_buf_type type;
unsigned char picture[720*2*576] = {0};
unsigned char crop[64*64] = {0};
unsigned char Y80_crop[128*64] = {0};
int m, n;
int x, y;
int po=1;
//int tar_x, tar_y;
  
//*****************************初始化相关变量****************************************

Init_Addr();


//*********************************整个循环**********************************************
for(i=0;;i++)
{
memset(&capture_buf, 0, sizeof(capture_buf));
capture_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
capture_buf.memory = V4L2_MEMORY_MMAP;
if (ioctl(fd_capture_v4l, VIDIOC_DQBUF, &capture_buf) < 0) 
{
printf("VIDIOC_DQBUF failed.\n");
return -1;
}

memcpy(picture,  capture_buffers[capture_buf.index].start, capture_buffers[capture_buf.index].length-2048);/*将图片数据保存到数组中*/
//**************************************************************************************************
if(po==1)
{
process_image(capture_buffers[capture_buf.index].start, 720*576*2);
po=2;
}
memset(&output_buf, 0, sizeof(output_buf));

output_buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
output_buf.memory = V4L2_MEMORY_MMAP;
if (i < g_output_num_buffers) 
{
output_buf.index = i;
if (ioctl(fd_output_v4l, VIDIOC_QUERYBUF, &output_buf) < 0)
{
printf("VIDIOC_QUERYBUF failed\n");
return -1;
}
} 
else 
{
output_buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
output_buf.memory = V4L2_MEMORY_MMAP;
if (ioctl(fd_output_v4l, VIDIOC_DQBUF, &output_buf) < 0)
{
printf("VIDIOC_DQBUF failed\n");
return -1;
}
}


/****************************灰度图像保存***************************************************/
for(m=0;m<480;m++)
{
for(n=0;n<640;n++)
{
gSubImageData[m*640+n] = picture[m*1280+2*n];
}
}
/*******************************************************************************************/
/*****************裁剪图像保存 取图像中心点64*64位置并放在左上角的位置**********************/
if (i < 200)
{
for(x=0;x<64;x++)
{
for(y=0;y<64;y++)
{
crop[x*64+y] = gSubImageData[(240-32+x)*640+(320-32+y)];
}
}

for(x=0;x<64;x++)
{
for(y=0;y<64;y++)
{
Y80_crop[x*128+y*2] = crop[x*64+y];
Y80_crop[x*128+y*2+1] = 128;
}
}


for(x=0;x<64;x++)
{
for(y=0;y<128;y++)
{
picture[x*1280+y] = Y80_crop[x*128+y];
}
}
//打印帧号******************************************************************
printf("Frame = %d\n",i);
}

/**************************************************************************************/
//200帧后确定模板***************************************************************
if (i == 200)
{
for(x=0;x<64;x++)
{
for(y=0;y<64;y++)
{
crop[x*64+y] = gSubImageData[(240-32+x)*640+(320-32+y)];
}
}

for(y=0;y<64;y++)
for(x=0;x<64;x++)
Template[y*64+x] = crop[y*64+x];


//**********************将模板放在图像左上角********************************
for(x=0;x<64;x++)
{
for(y=0;y<64;y++)
{
Y80_crop[x*128+y*2] = crop[x*64+y];
Y80_crop[x*128+y*2+1] = 128;
}
}
//赋跟踪目标
tar_y=48*5;
tar_x=64*5;

//从子图像中提取64*64的模板图像
//achieveTemplate(gSubImageData,Template,gSubImageWidth,gSubImageHeight,TmpW,TmpH,tar_x,tar_y);
TrackLevel(0);//0->丢失目标局部重检 1->丢失目标全屏重检  2->更难丢失目标条件
TrackInit(&tar_x,&tar_y);
}
//*************************************************************************

//*****************************跟踪算法************************************
if (i>200)
{
f32_NccRatio = TrackTarget(gSubImageData,Template,Template2,gSubImageWidth,gSubImageHeight,TmpW,TmpH,&tar_x,&tar_y);
//tar_x、tar_y为目标坐标

printf("f32_NccRatio = %f\n",f32_NccRatio);

for(x=0;x<64;x++)
{
for(y=0;y<128;y++)
{
picture[(x)*1280+y] = Y80_crop[x*128+y];
}
}
//画跟踪框*************************************************************
draw_box(picture, tar_y-32, tar_x*2-64, 1280, 480);


}
//*************************************************************************







// memcpy(capture_buffers[capture_buf.index].start, picture, sizeof(picture));
memcpy(output_buffers[output_buf.index].start, capture_buffers[capture_buf.index].start, capture_buffers[capture_buf.index].length);
if(po==2)
{
process_image1(output_buffers[capture_buf.index].start, 720*576*2);
po=3;
}


if (ioctl(fd_capture_v4l, VIDIOC_QBUF, &capture_buf) < 0)
{
printf("VIDIOC_QBUF failed\n");
return -1;
}


if (ioctl(fd_output_v4l, VIDIOC_QBUF, &output_buf) < 0)
{
printf("VIDIOC_QBUF failed\n");
return -1;
}
if (i == 1) 
{
type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
if (ioctl(fd_output_v4l, VIDIOC_STREAMON, &type) < 0) 
{
printf("Could not start stream\n");
return -1;
}
}
}


return 0;
}






int main(int argc, char **argv)
{
char fb_device[100] = "/dev/fb0";


int fd_fb = 0, i;
struct mxcfb_gbl_alpha alpha;
enum v4l2_buf_type type;


if ((fd_capture_v4l = open(v4l_capture_dev, O_RDWR, 0)) < 0)
{
printf("Unable to open %s\n", v4l_capture_dev);
return -1;
}


if ((fd_output_v4l = open(v4l_output_dev, O_RDWR, 0)) < 0)
{
printf("Unable to open %s\n", v4l_output_dev);
return -1;
}


if (v4l_capture_setup() < 0) {
printf("Setup v4l capture failed.\n");
return -1;
}


if (v4l_output_setup() < 0) {
printf("Setup v4l output failed.\n");
close(fd_capture_v4l);
return -1;
}


if ((fd_fb = open(fb_device, O_RDWR )) < 0) 
{
printf("Unable to open frame buffer\n");
close(fd_capture_v4l);
close(fd_output_v4l);
return -1;
}


/* Overlay setting */
alpha.alpha = 0;
alpha.enable = 1;
if (ioctl(fd_fb, MXCFB_SET_GBL_ALPHA, &alpha) < 0) {
printf("Set global alpha failed\n");
close(fd_fb);
close(fd_capture_v4l);
close(fd_output_v4l);
return -1;
}


if (prepare_output() < 0)
{
printf("prepare_output failed\n");
return -1;
}


if (start_capturing() < 0)
{
printf("start_capturing failed\n");
return -1;
}


v4l_tvin();


type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
ioctl(fd_output_v4l, VIDIOC_STREAMOFF, &type);


type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ioctl(fd_capture_v4l, VIDIOC_STREAMOFF, &type);


for (i = 0; i < g_output_num_buffers; i++)
{
munmap(output_buffers[i].start, output_buffers[i].length);
}
for (i = 0; i < g_capture_num_buffers; i++)
{
munmap(capture_buffers[i].start, capture_buffers[i].length);
}


close(fd_capture_v4l);
close(fd_output_v4l);
close(fd_fb);






return 0;





}