docapture.c~

基于ARM9TDMI的CMOS摄像头驱动源程序

	//		G : [gggggggg gggggggg gggggggg gggggggg] & [11111100 00000000 00000000 00000000] => [gggggg00 00000000 00000000 00000000]
	//		B : [bbbbbbbb bbbbbbbb bbbbbbbb bbbbbbbb] & [11111000 00000000 00000000 00000000] => [bbbbb000 00000000 00000000 00000000]
	//		                                          & [00000000 11111000 00000000 00000000] => [00000000 rrrrr000 00000000 00000000]
	//		                                          & [00000000 11111100 00000000 00000000] => [00000000 gggggg00 00000000 00000000]
	//	                                              & [00000000 11111000 00000000 00000000] => [00000000 bbbbb000 00000000 00000000]
	//	2. Align to the correct bit positon
	//
	//		R :	[rrrrr000 00000000 00000000 00000000]       => [rrrrr000 00000000 00000000 00000000]
	//		G : [gggggg00 00000000 00000000 00000000] >>  5 => [00000ggg ggg00000 00000000 00000000]
	//		B : [bbbbb000 00000000 00000000 00000000] >> 11 => [00000000 000bbbbb 00000000 00000000]
	//      R : [00000000 rrrrr000 00000000 00000000] >>  8 => [00000000 00000000 rrrrr000 00000000]
	//		G : [00000000 gggggg00 00000000 00000000] >> 13 => [00000000 00000000 00000ggg ggg00000]
	//	    B : [00000000 bbbbb000 00000000 00000000] >> 19 => [00000000 00000000 00000000 000bbbbb]
	//
	//	3. Combine R,G,B to form 16-bit words, using bitwise OR
	//
	//	            |<----- k ----->| |<---- k+1 ---->|
	//		R :	   [rrrrr000 00000000 00000000 00000000]
	//		G : or [00000ggg ggg00000 00000000 00000000]
	//		B :	or [00000000 000bbbbb 00000000 00000000]
	//      R : or [00000000 00000000 rrrrr000 00000000]
	//		G : or [00000000 00000000 00000ggg ggg00000]
	//	    B : or [00000000 00000000 00000000 000bbbbb]
	//	           -------------------------------------
	//	         =>[rrrrrggg gggbbbbb rrrrrggg gggbbbbb]
	//
	U32 i, j;

	
	for(i = 0, j = 0; i < imgSize / 4; i ++)
	{
	//
	//	Read 8-bit pixel in Little endian style
	//	Write 16-bit pixel in Big endian style
	//
	//	   U32 [(k+3)(k+2)  (k+1)(k  )]
	//	=> U32 [(k  )(k+1)][(k+2)(k+3)]
	//
		register U32 r = ((U32 *)_R)[i];
		register U32 g = ((U32 *)_G)[i];
		register U32 b = ((U32 *)_B)[i];

#ifdef OUTPUT_IN_BE
		((U32 *)_out)[j ++]
		= ((r & 0x000000F8) << 24)	//pixel k th
		| ((g & 0x000000FC) << 19)
		| ((b & 0x000000F8) << 13)
		| ((r & 0x0000F800)      )	//pixel k+1 th
		| ((g & 0x0000FC00) >>  5)
		| ((b & 0x0000F800) >> 11);

		((U32 *)_out)[j ++]
		= ((r & 0x00F80000) <<  8)	//pixel k+2 th
		| ((g & 0x00FC0000) <<  3)
		| ((b & 0x00F80000) >>  3)
		| ((r & 0xF8000000) >> 16)	//pixel k+3 th
		| ((g & 0xFC000000) >> 21)
		| ((b & 0xF8000000) >> 27);
#else
// Let's make it output in little endian		
		((U32 *)_out)[j ++]
		= ((r & 0x000000F8) << 8 )		//pixel k th
		| ((g & 0x000000FC) << 3 )
		| ((b & 0x000000F8) >> 3 )
		| ((r & 0x0000F800) << 16)	//pixel k+1 th
		| ((g & 0x0000FC00) << 11)
		| ((b & 0x0000F800) << 5 );

		((U32 *)_out)[j ++]
		= ((r & 0x00F80000) >> 8 )	//pixel k+2 th
		| ((g & 0x00FC0000) >> 13)
		| ((b & 0x00F80000) >> 19)
		| ((r & 0xF8000000)      )	//pixel k+3 th
		| ((g & 0xFC000000) >> 5 )
		| ((b & 0xF8000000) >> 11);
#endif
	}
		
	return;
}

void clearScreen()
{
	int i;
	U16 *p=fb;
	
	for (i=0; i<240*320; i++)
		*(p++) = 0;
}

void clearWindow()
{
	int i, j;
	U16 *p=fb+240*80;
	
	for (i=0; i<160; i++)
	{
		p+=2;
		for (j=2; j<238; j++)
			*(p++) = 0;
		p+=2;
	}
}

void displayViewfinderImage()
{
	U16 *p1=fb+ORIGIN_OFFSET;
//	U16 *p2=display_temp;
	U16 *p2=displayBuffer;
	int row, col;
	
	for (row=0; row<VF_HEIGHT; row++)
	{
		for (col=0; col<VF_WIDTH; col++)
		{
			*(p1++) = *(p2++);
		}
		p1 += ROW_OFFSET;
	}
}

//#define DISPLAY_BAYER

void displayCapturedImage()
{
#ifdef DISPLAY_BAYER
	int row, col;
	U16 *p=(U16 *)fb;
	char *b=rawData;
			
	// write to screen with resolution of 640/3 x 480/3 
	p += 80*240+13;
	for (row=0; row<160; row++)
	{
		for (col=0; col<213; col++)
		{
			*(p++) = (((U16)*b>>3)<<11) | (((U16)*b>>2)<<5) | ((U16)*b>>3);
			b += 3;
		}
		b++;
		p += 27;
		b += 640*2;
	}
#else	
	U16 *p1=fb;
//	U16 *p2=display_temp;
	U16 *p2=displayBuffer;
	int row, col;

	// write to screen with resolution of 640/3 x 480/3 
	p1 += 80*240+13;
	for (row=0; row<160; row++)
	{
		for (col=0; col<213; col++)
		{
			*(p1++) = *p2;
			p2 += 3;
		}
		p2++;
		p1 += 27;
		p2 += 640*2;
	}
#endif
}

void detectEnterFromConsole()
{
	int fd_console;
	fd_set fdset;
	struct timeval timeout;
	char c;
	
	if ((fd_console = open("/dev/tty", O_RDONLY)) == 0)
	{
		printf("*** ERROR: cannot open /dev/tty ***\n");
	}
	else
	{
		FD_ZERO(&fdset);
		FD_SET(fd_console, &fdset);
		timeout.tv_sec = 0;	// set timeout to 0 => blocking select
		timeout.tv_usec = 0;
		if (select(fd_console+1, &fdset, 0, 0, &timeout) > 0)
		{
			do {
				read(fd_console, &c, 1);
			} while (c != 10);	// i.e. line-feed 
			#ifdef __STANDALONE__
			quit=1;
			#endif
		}
		close(fd_console);			
	}
}

void writeImageToFile()
{
	U16 header[27] = {0x4D42, 0x1036, 0x000E, 0x0000, 0x0000, 0x0036, 0x0000, 0x0028, 
							0x0000, 0x0280, 0x0000, 0x01E0, 0x0000, 0x0001, 0x0018, 0x0000,
							0x0000, 0x1000, 0x000E, 0x0EC4, 0x0000, 0x0EC4, 0x0000, 0x0000,
							0x0000, 0x0000, 0x0000};
	FILE *bmpFile;
	char *p;
	int row;
	
//	if ((bmpFile=fopen("/tmp/web/photo.bmp", "wb")) == 0)
	if ((bmpFile=fopen("/tmp/photo.bmp", "wb")) == 0)
	{
		printf("*** ERROR: cannot open bitmap file for write ***\n");
		return;
	}
	fwrite(header, sizeof(U16), 27, bmpFile);
	for (row=0; row<480; row++)
	{
		p = (char *)displayBuffer + (479-row)*3*640;
		fwrite(p, sizeof(char), 640*3, bmpFile);
	}
	fclose(bmpFile);
}

void RGBtoBitmap24()
{
	char *p=(char *)displayBuffer;
	char *r=(char *)redData;
	char *g=(char *)greenData;
	char *b=(char *)blueData;
	U32 i;
	
	for (i=0; i<640*480; i++)
	{
		*(p++) = *(b++);
		*(p++) = *(g++);
		*(p++) = *(r++);
	}
}

int AllocateBuffer()
{
	if ((rawData = (U8 *)malloc(IMAGE_SIZE)))
	{
		printf("Allocate memory buffer for raw image data.\n");
	}
	else
	{
		printf("*** Failed to allocate memory buffer for raw image data ***\n");
		return -1;
	}

	if ((redData = (U8 *)malloc(IMAGE_SIZE)))
	{
		printf("Allocate memory buffer for RED data.\n");
	}
	else
	{
		printf("*** Failed to allocate memory buffer for RED data ***\n");
		free(rawData);
		return -1;
	}

	if ((greenData = (U8 *)malloc(IMAGE_SIZE)))
	{
		printf("Allocate memory buffer for GREEN data.\n");
	}
	else
	{
		printf("*** Failed to allocate memory buffer for GREEN data ***\n");
		free(redData);
		free(rawData);
		return -1;
	}

	if ((blueData = (U8 *)malloc(IMAGE_SIZE)))
	{
		printf("Allocate memory buffer for BLUE data.\n");
	}
	else
	{
		printf("*** Failed to allocate memory buffer for BLUE data ***\n");
		free(greenData);
		free(redData);
		free(rawData);
		return -1;
	}

	if ((displayBuffer = (U16 *)malloc(IMAGE_SIZE*3)))
	{
		printf("Allocate memory for display buffer.\n");
	}
	else
	{
		printf("*** Failed to allocate memory for display buffer ***\n");
		free(blueData);
		free(greenData);
		free(redData);
		free(rawData);
		return -1;
	}
	return 0;
}

void freeBuffer()
{
	free(displayBuffer);
	free(blueData);
	free(greenData);
	free(redData);
	free(rawData);
}

#ifdef __STANDALONE__
int main(int argc, char *argv[])
#else
int doCapture(U8 mode)
#endif
{
	#ifdef __STANDALONE__
//	printf("CSI test: version 0.2 compiled at "__DATE__" / "__TIME__"\n");		
	gAECmode = AEC_WITH_GAIN;
	// mode specified ?
	if (argc == 2)
	{
		if (argv[1][0] == 'v')
		{
			gAECmode = AEC_WITH_VF;
			printf("AEC is controlled by virtual frame size\n");
		}
		else
			printf("AEC is controlled by global gain\n");
	}
	#else	// i.e. under QT
	if (mode == MODE_INIT_AEC_GAIN)
		gAECmode = AEC_WITH_GAIN;
	if (mode == MODE_INIT_AEC_VF)
		gAECmode = AEC_WITH_VF;
	#endif

	#ifndef __STANDALONE__
	if ((mode == MODE_INIT_AEC_GAIN) || (mode == MODE_INIT_AEC_VF))
	{
	#endif		
		if ((fd_csi2c = open("/tmp/csi2c", O_RDWR)) < 0)
		{
			printf("Device csi2c open error !\n");
			exit(-1);
		}
		
		fd_fb = open("/dev/fb0", O_RDWR);
	
		// Map the device to memory
		fb = (U16 *)mmap(0, 240*320*2, PROT_READ | PROT_WRITE, MAP_SHARED,fd_fb, 0);
		if ((int)fb == -1)
		{
			printf("Error: failed to map framebuffer device to memory.\n");
			close(fd_csi2c);
			exit(-1);
		}
		else
			printf("Frame buffer mapping succeed.\n");
		#ifdef __STANDALONE__			
		clearScreen();
		#else	
		clearWindow();
		#endif	
		ioctl(fd_csi2c, IOCTL_CSI_INIT, (BUS_CLOCK << 8) | SENSOR_CLOCK);
//		ioctl(fd_csi2c, IOCTL_RESET_ASSERT, 0);
//		ioctl(fd_csi2c, IOCTL_RESET_RELEASE, 0);

		captureInit();
		if (AllocateBuffer() < 0)
			exit(-1);
		ioctl(fd_csi2c, IOCTL_DMA_CAPTURE, VF_IMAGE_SIZE);
	#ifndef __STANDALONE__
	}
	#endif

	#ifdef __STANDALONE__
	printf("\nPress <ENTER> to do image capture ...\n\n");
	// viewfinder loop
	while (!quit)
	{	
	#else		// i.e. under QT
	if (mode	== MODE_VIEWFINDER)
	{
	#endif		
		viewfinderCapture();
		ioctl(fd_csi2c, IOCTL_INC_FRM, 0);
		printcmsg("Convert image from 24-bit to 16-bit format\n");
		bmp24to16(redData, greenData, blueData, VF_IMAGE_SIZE, displayBuffer);
		displayViewfinderImage();

		#ifdef __STANDALONE__		
		detectEnterFromConsole();
		#endif		
	}	// while or if loop	

	#ifndef __STANDALONE__
	if (mode == MODE_CAPTURE)
	{
	#endif		
		// stop viewfinder capture	
		ioctl(fd_csi2c, IOCTL_STOP_CAPTURE, 0);
		ioctl(fd_csi2c, IOCTL_SUBSAMPLE, 1);
		ioctl(fd_csi2c, IOCTL_DMA_CAPTURE, 640*480);
		read(fd_csi2c, rawData, 640*480);
		read(fd_csi2c, rawData, 640*480);
		read(fd_csi2c, rawData, 640*480);
//		read(fd_csi2c, rawData, 640*480);
		while (!read(fd_csi2c, rawData, 640*480));
		ioctl(fd_csi2c, IOCTL_STOP_CAPTURE, 0);
		#ifndef DISPLAY_BAYER
		white_balance(640, 480);
		ci_bilinear_GRBg(rawData, 640, 480, redData, greenData, blueData);
		// Here we use a different saturation value for the built-in LCD
		// and the output bitmap file
		deltaGreen2(LCD_SATURATION, 640*480, redData, greenData, blueData);	
		bmp24to16(redData, greenData, blueData, 640*480, displayBuffer);
		#endif
		displayCapturedImage();
		ci_bilinear_GRBg(rawData, 640, 480, redData, greenData, blueData);
		deltaGreen2(BMP_SATURATION, 640*480, redData, greenData, blueData);	
		RGBtoBitmap24();
		writeImageToFile();
				
	#ifndef __STANDALONE__
	}
	#endif
	
	#ifndef __STANDALONE__
	if (mode == MODE_CLEANUP)
	{
	#endif		
		freeBuffer();		
		close(fd_fb);
		close(fd_csi2c);
	#ifndef __STANDALONE__
	}
	#else
	printf("Capture done.\n");
	#endif
	
	return 0;
}