docapture.c~

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

	if(avg_max_index != BAYER_R)	table_gen(coef[BAYER_R], tab[BAYER_R]);
	if(avg_max_index != BAYER_B)	table_gen(coef[BAYER_B], tab[BAYER_B]);
	if(avg_max_index != BAYER_g)	table_gen(coef[BAYER_g], tab[BAYER_g]);

	//apply white balance coef
	apply_coef(rawData, rawData, imageWidth, imageHeight, tab, avg_max_index);
}

//------------------------------------------------------------------------------------
//
//	Bilinear Interpolation (GRBg pattern)
//
//	It is a very optimized implementation.
//
//	Algorithm
//	=========
//	The interpolation is done in 2 directions seperately
//
//	1. Horizontal interpolation
//	   This fill up 2 colors for each row,
//	   e.g. for GR rows, the G & R of every pixel will be filled.
//
//	   GR row : [G0][  ][G2] => [  ][G1][  ], where G1 = (G0 + G2) / 2
//	   Bg row : [B0][  ][B2] => [  ][B1][  ], where B1 = (B0 + B2) / 2
//
//	2. Vertical interpolation
//	   This fill up the last color plane for each pixel
//	   e.g. for GR rows, B will be the mean taken from the row above & row below
//
//	   Bg row : [B0]
//	   GR row :     => [B1] ,where B1 = (B0 + B2) / 2
//	   Bg row : [B2]
//
//	It is noted that for GR rows, B is actually interpolated twice.
//
//	        1. Horizontal    2. Vertical
//	[B0][  ][B2]    [  ][B1][  ]    [  ][  ][  ]
//	[  ][  ][  ] => [  ][  ][  ] => [  ][B4][  ], where B4 = (B0 + B2 + B6 + B8) / 2
//	[B6][  ][B8]    [  ][B7][  ]    [  ][  ][  ]
//
//	This also applies for the R in Bg rows.
//
//	Input	: (1) _data	: Ptr to the bayer data
//			  (2) imgH	: Image width
//			  (3) imgV	: Image height
//			  (4) _R	: Ptr to the Red buffer in the RGB space
//			  (5) _G	: Ptr to the Green buffer in the RGB space
//			  (6) _B	: Ptr to the Blue buffer in the RGB space
//
//	Output	: void
//
//	Instruction count on ARM9 simulator = 5.84M (VGA image)
//
//------------------------------------------------------------------------------------
static void ci_bilinear_GRBg(U8 * _data, U32 imgH, U32 imgV, U8 * _R, U8 * _G, U8 * _B)
{
	U32 h, v;
	U8 * _r, * _g, * _b;

//init
	_r = _R;
	_g = _G;
	_b = _B;

	//
	//	Do interpolation in horizontal direction
	//	Convert Bayer pattern to RGB plane
	//
	//	GRGRGR...GRGR => [RG ][RG ][RG ]...[RG ][RG ]
	//	BgBgBg...BgBg => [ GB][ GB][ GB]...[ GB][ GB]
	//	GRGRGR...GRGR => [RG ][RG ][RG ]...[RG ][RG ]
	//	BgBgBg...BgBg => [ GB][ GB][ GB]...[ GB][ GB]
	//	...
	//	...
	//	GRGRGR...GRGR => [RG ][RG ][RG ]...[RG ][RG ]
	//	BgBgBg...BgBg => [ GB][ GB][ GB]...[ GB][ GB]
	//
	//	There is no boundary case problem!
	//

	for(v = 0; v < imgV; v += 2)
	{
	//GR row
		for(h = 1; h < imgH; h += 2)
		{
			//
			//	G[R][G]R => d0 d1 d2 d3
			//
			register U32 d0 = _data[h - 1];
			register U32 d1 = _data[h    ];
			register U32 d2 = _data[h + 1];
			register U32 d3 = _data[h + 2];
		//R
			_r[h] =  d1;				//direct
			_g[h] = (d0 + d2) >> 1;		//interpolate
		//G
			_g[h + 1] =  d2;			//direct
			_r[h + 1] = (d1 + d3) >> 1;	//interpolate
		}

		//move ptr to next row
		_data += imgH;
		_r += imgH;
		_g += imgH;
		_b += imgH;

	//Bg row
		for(h = 1; h < imgH; h += 2)
		{
			//
			//	 B[g][B]g => d0 d1 d2 d3
			//
			register U32 d0 = _data[h - 1];
			register U32 d1 = _data[h    ];
			register U32 d2 = _data[h + 1];
			register U32 d3 = _data[h + 2];
		//g
			_g[h] =  d1;				//direct
/*???*/		_b[h] = (d0 + d2) >> 1;		//interpolate

		//B
			_b[h + 1] =  d2;			//direct
			_g[h + 1] = (d1 + d3) >> 1;	//interpolate
		}

		//move ptr to next row
		_data += imgH;
		_r += imgH;
		_g += imgH;
		_b += imgH;
	}

	//
	//	Do interpolation in vertical direction
	//	Convert Bayer pattern to RGB plane
	//
	//	GRGRGR...GRGR => [   ][   ][   ]...[   ][   ]
	//	BgBgBg...BgBg => [R  ][R  ][R  ]...[R  ][R  ]
	//	GRGRGR...GRGR => [  B][  B][  B]...[  B][  B]
	//	BgBgBg...BgBg => [R  ][R  ][R  ]...[R  ][R  ]
	//	...
	//	...
	//	GRGRGR...GRGR => [  B][  B][  B]...[  B][  B]
	//	BgBgBg...BgBg => [   ][   ][   ]...[   ][   ]
	//
	//	Top & bottom row cases are not interpolated
	//

	//looping without counting top & bottom row
	for(v = 1; v < imgV - 1; v += 2)
	{
		//move ptr to next pair of rows
		_r = _R + v * imgH;
		_b = _B + v * imgH;

		for(h = 0; h < imgH; h ++)
		{
			register U32 d0 = _r[h - imgH];
			register U32 d1 = _r[h + imgH];
			register U32 d2 = _b[h];
			register U32 d3 = _b[h + (imgH << 1)];

		//Bg row, vertical interpolate for R
			_r[h] = (d0 + d1) >> 1;
		//RG row, vertical interpolate for B
			_b[h + imgH] = (d2 + d3) >> 1;
		}
	}

	return;
}

//---------------------------------------------------------------
//
//	Generate multiplication table
//
//	This accepts a floating coefficient, then applies it to
//	a series of no. range between -255 to +255.  The result
//	is then trimmed back to -255 to +255 range
//
//	Note that _tab should point to the position of 0
//
//	Input	: (1) satlevel	: saturation level,
//			                  usually falls in 0.3 - 0.4 range
//			  (2) _tab		: table buffer
//
//	Ouput	: void
//
//---------------------------------------------------------------
static void gen_multiply_tab(float satlevel, S16 * _tab)
{
#define SHIFT16 16

	S32 coef, i, temp;

//scale up floating point to integer to avoid complex arithmetics
	coef = (S32)(satlevel * (float)(1 << SHIFT16));

//table range :
//-255 <= tab[] <= +255
	for(i = -255; i <= 255; i ++)
	{
		temp = (i * coef) >> SHIFT16;

		if(temp > 255)
			temp = 255;
		if(temp < -255)
			temp = -255;
		_tab[i] = (S16)temp;
	}
	
	return;
}

// -----------------------------------------------------------------------
//
//	Green Delta Color Correction
//
//	This method makes the image more colorful by amplifying the 
//	delta between different colors for each pixel.
//
//	Green is taken as the reference.  It's subtracted from Red or Blue
//	to give the corresponding deltas.  The delta is weighted by
//	the saturation level and then added back to the original value.
//	i.e. if satuartion level is 1.2, the resulting effect for R is
//	R + 1.2(R - G).
//
//	Typical values for saturation level is 0.3 - 0.4
//
//	Input	: (1) satlevel	: saturation level
//			  (2) _rgbi		: RGB image
//			                  (This would be modified)
//
//	Ouput	: void
//
//	Instruction count on ARM9 simulator : 7.43M (VGA image)
//
// -----------------------------------------------------------------------

//tables for fast multiply & trimming
static S16 mbuf[511] = {0};	//multiply table
static S16 * _mtab;			//pointing to the middle of table
static U8 tbuf[1023] = {0};	//trim table
static U8 * _ttab;			//pointing to the middle of table
static float current_satlevel = (float)0;

static void deltaGreen2(float satlevel, U32 imgSize, U8 * _r, U8 * _g, U8 * _b)
{
	U32 k;

//update tables for diff satlevel
	if(satlevel != current_satlevel)
	{
		_mtab = &mbuf[255];	//table points to the 0 position (middle) of buffer
		_ttab = &tbuf[511];	//table points to the 0 position (middle) of buffer

		gen_multiply_tab(satlevel, _mtab);
		gen_trim_table(_ttab);

		current_satlevel = satlevel;
	}

//
//	1. apply equation : pixel = pixel + saturation * delta
//	   (saturation * delta) is lookup from table (_mtab) for better speed
//	2. trim values to 0-255
//	   also use table (_ttab) lookup for better speed
//
	for(k = 0; k < imgSize; k ++)
	{
	//copy data to registers to reduce the addr decode overhead
		register S16 dataR = _r[k];
		register S16 dataG = _g[k];
		register S16 dataB = _b[k];

		_r[k] = _ttab[dataR + _mtab[dataR - dataG]];
		_b[k] = _ttab[dataB + _mtab[dataB - dataG]];
	}
	
	return;
}

// The following routine control the global gain to active the targeted average RED pixel intensity
//void AutoGainCtrl(BSTAT * _bstat)
void AutoGainCtrl()
{
	int avgRED, step;
	
//	avgRED = _bstat->avg[BAYER_R];
	avgRED = average[BAYER_R];

//	printf("RED average: %d, last gain: %f, action: ", avgRED, lastGlobalGain);
	// tune gain up
	if ((avgRED < (AVG_RED_TARGET-TARGET_MARGIN)) && (lastGlobalGain < 0x3F))
	{
//		printf("+++\n");
		step = (AVG_RED_TARGET - avgRED) >> 4;
		if (step == 0)
			step = 1;
		if ((lastGlobalGain + step) < 0x3F)
			lastGlobalGain += step;
		else
			lastGlobalGain = 0x3F;
		ioctl(fd_csi2c, IOCTL_SET_GAIN, lastGlobalGain);
		return;
	}
	// tune gain down
	if ((avgRED > (AVG_RED_TARGET+TARGET_MARGIN)) && (lastGlobalGain > 0))
	{
//		printf("---\n");
		step = (avgRED - AVG_RED_TARGET) >> 4;
		if (step == 0)
			step = 1;
		if ((lastGlobalGain - step) > 0)
			lastGlobalGain -= step;
		else
			lastGlobalGain = 0;
		ioctl(fd_csi2c, IOCTL_SET_GAIN, lastGlobalGain);
		return;
	}
//	printf("FREEZE\n");
}

//#define MAX_VF		0x1100
#define MAX_VF		0xFFFF
#define MIN_VF		0x0300

// The following routine control the virtual frame size to active the targeted average RED pixel intensity.
// Virtual frame size will effectively alter the exposure time.
//void AutoVFsizeCtrl(BSTAT * _bstat)
void AutoVFsizeCtrl()
{
	int avgRED, step;
	
	avgRED = average[BAYER_R];

	// tune Virtual Frame size up
	if ((avgRED < (AVG_RED_TARGET-TARGET_MARGIN)) && (lastVFsize < MAX_VF))
	{
		step = (AVG_RED_TARGET - avgRED) << 3;
		if (step == 0)
			step = 4;
		if ((lastVFsize + step) < MAX_VF)
			lastVFsize += step;
		else
			lastVFsize = MAX_VF;
		ioctl(fd_csi2c, IOCTL_SET_VF_WIDTH, lastVFsize);
		return;
	}
	// tune Virtual Frame size down
	if ((avgRED > (AVG_RED_TARGET+TARGET_MARGIN)) && (lastVFsize > MIN_VF))
	{
		step = (avgRED - AVG_RED_TARGET) << 3;
		if (step == 0)
			step = 4;
		if ((lastVFsize - step) > MIN_VF)
			lastVFsize -= step;
		else
			lastVFsize = MIN_VF;
		ioctl(fd_csi2c, IOCTL_SET_VF_WIDTH, lastVFsize);
	}
}

#define AEC_PER_FRAME_COUNT 4
void viewfinderCapture()
{
	if (read(fd_csi2c, rawData, VF_IMAGE_SIZE))
	{
		printcmsg("CSI data read succeed !\n");
	}
	else
		printf("*** CSI data read failed ! ***\n");
	printcmsg("CSI data capture done !\n");

	//
	//	AEC acts as the primary control
	//	to archieve the target mean pixel value
	//
	if ((frameCount > 0) && ((frameCount % AEC_PER_FRAME_COUNT) == 0))
	{
		printcmsg("Calculate statistics\n");
		generateHistogram(rawData, VF_WIDTH, VF_HEIGHT, 2);
		takeAverage(VF_IMAGE_SIZE);

		if (gAECmode == AEC_WITH_GAIN)
			AutoGainCtrl();
		else		
			AutoVFsizeCtrl();
	}

	printcmsg("White balance processing\n");
	white_balance(VF_WIDTH, VF_HEIGHT);
	
	printcmsg("Color interpolation\n");
	ci_bilinear_GRBg(rawData, VF_WIDTH, VF_HEIGHT, redData, greenData, blueData);
	deltaGreen2(LCD_SATURATION, VF_IMAGE_SIZE, redData, greenData, blueData);	

	frameCount++;
	printcmsg("Color interpolation done\n");
	return;
}

//----------------------------------------------------------------------
//
//	Bitmap 24-bit to 16-bit (5-6-5) convertor
//	(with window in frame support)
//
//	This is for 16-bit LCD panel display.
//	The 24-bit bitmap is down-sampled to 16-bit resolution by
//	truncating the least significant bits.  The output is 
//	then stored in a 16-bit storage type
//
//	In order to fully ultilize the bus bandwidth, data is fetched
//	in and out as 32-bit words.  i.e. for R,G,B buffer, each time
//	4 pixels will be taken in.  After processing, 2 16-bit pixel
//	will be packed to a 32-bit word and then write back to memory
//
//	Assume system endian = LITTLE
//	       LCDC endian   = BIG/LITTLE
//
//	Input	: _R		: Pointer to Red color
//			  _G		: Pointer to Green color
//			  _B		: Pointer to Blue color
//			  imgSize	: No. of pixel
//			  _out		: Pointer to the output array
//			  system_endian	: endian of target system
//			  lcdc_endian	: endian of LCD controller
//
//	Output	: void
//
//	Instruction count on ARM9 simulator : 2.98M (VGA image)
//
//----------------------------------------------------------------------
//#define OUTPUT_IN_BE
void bmp24to16(U8 * _R, U8 * _G, U8 * _B, U32 imgSize, U16 * _out)
{
	//
	//	Algorithm (example : All Big Endian)
	//	=========
	//
	//	1. Filter MSbs (truncation)
	//
	//	            k       k+1      k+2      k+3
	//		R : [rrrrrrrr rrrrrrrr rrrrrrrr rrrrrrrr] & [11111000 00000000 00000000 00000000] => [rrrrr000 00000000 00000000 00000000]