v4l2採集視頻 framebuffer顯示 左上角顯示logo

/*
 *   logo 128*128 24bmp
 *
 *   preview : 320x240  overlay on 320x240  16bpp LCD
 *
 *   TFT LCD size : 320x240 
 */
#include <sys/time.h>
#include <sys/types.h>
#include <asm/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/mman.h>
#include <errno.h>
#include <arpa/inet.h>


#include <linux/fs.h>
#include <linux/kernel.h>
#include "videodev.h"
#include "videodev2.h"
#include <linux/fb.h>




#define PIXFMT_NUM 5
#define INPUT_NUM 5
#define CTRL_NUM 100


#define V4L2_DEV_NODE "/dev/camera"
#define FB_DEV_NODE "/dev/fb0"


//typedef struct v4l2_input    V_INPUT;
//typedef struct v4l2_format   V_FORMAT;
//typedef struct v4l2_fmtdesc  V_FMTDESC;
//typedef struct v4l2_queryctrl V_QRYCTRL;
typedef struct  
{  
    char cfType[2];
    long cfSize;
    long cfReserved;
    long cfoffBits;
}__attribute__((packed)) BITMAPFILEHEADER;  
 
typedef struct  
{  
    char ciSize[4];
    long ciWidth;
    long ciHeight; 
    char ciPlanes[2];
    int ciBitCount;
    char ciCompress[4];
    char ciSizeImage[4];
    char ciXPelsPerMeter[4];
    char ciYPelsPerMeter[4];
    char ciClrUsed[4];
    char ciClrImportant[4];
}__attribute__((packed)) BITMAPINFOHEADER;  
  
typedef struct  
{  
    unsigned char blue;
    unsigned char green;  
    unsigned char red;   
}__attribute__((packed)) PIXEL;
/****************************************************/
BITMAPFILEHEADER FileHead;
BITMAPINFOHEADER InfoHead;


typedef struct fb_var_screeninfo F_VINFO;
typedef struct fb_fix_screeninfo F_FINFO;
unsigned int x_lcd_size=0, y_lcd_size=0;
int bits_per_pixel = 0;


static void v4l2_show_on_fb(char *bmpfile,int fd, char *fbmem, int frames)
{
unsigned int pic1=0;
int ret;
char preview_buf[640*480*2];
  int addr_x=0,addr_y=0,count_now=0;
  long int location = 0 ;

PIXEL pix;
  FILE *fp;
  int rc,count=0;
        
         
  fp = fopen( bmpfile, "rb" );
  if (fp == NULL){
      printf("read fp error!\n");
  }
  rc = fread( &FileHead, sizeof(BITMAPFILEHEADER),1, fp );
  if ( rc != 1){
      printf("read header error!\n");
      fclose( fp );
  }
  if (memcmp(FileHead.cfType, "BM", 2) != 0){
  printf("it's not a BMP file\n");
      fclose( fp );
  }
  rc = fread( (char *)&InfoHead, sizeof(BITMAPINFOHEADER),1, fp );
  if ( rc != 1){
     printf("read infoheader error!\n");
     fclose( fp );
  }
  fseek(fp,FileHead.cfoffBits, SEEK_SET);        
  printf("width=%d, height=%d, bitCount=%d, offset=%d\n", InfoHead.ciWidth, InfoHead.ciHeight, InfoHead.ciBitCount,FileHead.cfoffBits); 


unsigned short int color[(FileHead.cfSize-FileHead.cfoffBits)/3];        
  while(!feof(fp))
  {
    if(InfoHead.ciBitCount == 24){
     rc = fread( (char *)&pix, 1, 3, fp );
     if (rc != 3 )
        { break; }
     color[count]=(pix.red >>3)<<11 | (pix.green>>2)<<5 | pix.blue>>3;  
  count++;
    }
  }
fclose( fp );
  printf("count=%d,FileHead.cfSize=%d\n",count,FileHead.cfSize);


while(1) 
{
if ((ret = read (fd, &preview_buf, 640*480*2)) < 0) 
{
perror("read");
return;
}
int y;
/* for (y = 0; y < 240; y++) 
memcpy(fbmem + 320*2*y, preview_buf + (y+120)*640*2+160*2, 320*2);*/
for (y = 0; y <128; y++) 
memcpy(fbmem + 320*2*y+128*2, preview_buf + (y+120)*640*2+160*2+128*2, (320-128)*2);//show the 128*128 bmp picture
for (y = 128; y < 240; y++) 
memcpy(fbmem + 320*2*y, preview_buf + (y+120)*640*2+160*2, 320*2);
/*****************************show bmp**********************************************/
addr_x = 0;addr_y=0;count_now=0;pic1++;
if(pic1==1)
    {pic1=0;
while(1)
{
   location = addr_x * bits_per_pixel / 8 + (InfoHead.ciHeight - addr_y - 1) * x_lcd_size * bits_per_pixel / 8;
   *((unsigned short int*)(fbmem + location)) = color[count_now];
 
  addr_x++;count_now++;
  if (addr_x == InfoHead.ciWidth ){
      addr_x = 0;addr_y++;
  if(addr_y== InfoHead.ciHeight)
         {
           break;
         }
   }
  }
    }
 printf("count_now=%d\n",count_now);
/************************************************************************************/
 fflush(stdout);
}

printf("over \n");
}


static unsigned int fb_grab(int fd, char **fbmem)
{
F_VINFO vinfo;
  F_FINFO finfo;
unsigned int length;


if (ioctl(fd, FBIOGET_VSCREENINFO, &vinfo) < 0) {
perror("FBIOGET_VSCREENINFO");
exit (EXIT_FAILURE);
}
if (ioctl(fd, FBIOGET_FSCREENINFO, &finfo)){        
     printf("Error reading fixed information.\n");
     exit(EXIT_FAILURE);
  }
length = vinfo.xres * vinfo.yres * (vinfo.bits_per_pixel >> 3);


x_lcd_size=vinfo.xres;             //width of tft lcd
y_lcd_size=vinfo.yres;             //height of tft lcd
  bits_per_pixel = vinfo.bits_per_pixel;
printf("fb memory info=xres (%d) x yres (%d), %d bpp\n",
vinfo.xres, vinfo.yres, vinfo.bits_per_pixel);
  
*fbmem = mmap(0, length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
if (*fbmem < 0) {
perror("mmap()");
length = 0;
}

return length;
}


static void fb_ungrab(char **fbmem, unsigned int length)
{
if (*fbmem)
munmap(*fbmem, length);
}




int main(int argc, char *argv[])
{
int v4l2_fd = -1;
int fb_fd = -1;
char *fbmem = NULL;
unsigned int fb_length = 0;


int preview_frames = 180;
int tmp;


printf("Camera Test Program, Start !\n");
if (argc > 2) {
if (sscanf(argv[2], "%d", &tmp) == 1)
preview_frames = tmp;
}


v4l2_fd = open(V4L2_DEV_NODE, O_RDWR);
if (v4l2_fd < 0) {
perror(V4L2_DEV_NODE);
goto out;
}


fb_fd = open(FB_DEV_NODE, O_RDWR);
if (fb_fd < 0) {
perror(FB_DEV_NODE);
goto out;
}

fflush(stdout);
if ((fb_length = fb_grab(fb_fd, &fbmem)) == 0)
goto out;
memset(fbmem, 0, fb_length);


printf("Press Ctrl+C to stop !\n");
fflush(stdout);
v4l2_show_on_fb(argv[1],v4l2_fd, fbmem, preview_frames);

printf("\n");


out:


if (v4l2_fd > 0)
close(v4l2_fd);


fb_ungrab(&fbmem, fb_length);


return 0;
}