video.c

基于TIc6205DSP上开发的MeanShift结合Kalman滤波的代码

#include "6205.h"
#include "glbvar.h"
#include "math.h"
#include "stdlib.h"


void MutualityH( int *pTplt, int *pArea, int bytesTplt, int wideTplt, int wideArea, uint *XY, uint *YY );
int SquareX( int *pTemplate, int nTemplateLen );
//void PixLineFilter( int* pSrcLine, int *pDstLine );
//void  PixStat( int *pAddr, int nStartLine, int Height, int *pX, int *pY );
//int ImageSub(int *SrcImgAddr1,  int *SrcImgAddr2,  int *DstImgAddr, int ImgLen );
//int	MemCopy(  int *DstImgAddr,  int *SrcImgAddr, int Len );
//int test(int *,int *,int *); 

//=================== Timer functions ======================
//定时程序，主要用以测试某程序段的执行时间
//记时有限制：记时时间不能超过40秒,记时单位：us
 void StartTimer( TIMER *timer )
{
	*timer = TMR0CNT;   
}

 int ReadTimer( TIMER timer )              //？？？？？
{
	register int Tmp = (int)(TMR0CNT) - timer;  
	if( Tmp  < 0 )
		Tmp += 0x7FFFFFFF; 
	Tmp /= 50;
	return( Tmp );
}


//================== DMA transfer functions ==================
//采用DMA方式搬移大量数据或者填充大量数据

//**********************************************************
//  MemSet:  以字节为单元填充数据
//**********************************************************
int MemSet( int *DestAddr, unsigned int Data, int LenInBytes )
{
	if( (PRICTL1 & 0xC) != 0)
		return 0;
	PRICTL1 = 0x00002240;
	SECCTL1 = 0x0000A080;
	SRC1 = (unsigned int)&Data;
	DST1 = (unsigned int)DestAddr;
    XFRCNT1 = LenInBytes;		
	PRICTL1 = PRICTL1 | 0x00000001;
	for(;;)
		if ( (PRICTL1 & 0xC) == 0x00 )
			break;
	PRICTL1 = PRICTL1 & 0xfffffffe;
	return 1;
}


//**********************************************************
//  MemCopy: 以32位字为单元传输数据，若长度Len不能
//           被4整除，则余数个字节不能被传输
//**********************************************************
int	MemCopy(  int *DstAddr,  int *SrcAddr, int Len )
{
	int	StartWordDMA( int *DestAddr, int *SourceAddr, int TransLen );
	int GetUserDMAState();
	void StopUserDMA();
	int k;
	StartWordDMA( DstAddr, SrcAddr, Len/4 );
	for(;;)
	{
		k = GetUserDMAState();
	  	if( k == 0x00 )
	       break;
	}
	StopUserDMA();
	return 0;
}
//**********************************************************
//  ByteMemCopy: 以字节为单元传输数据，长度Len无需保证能
//               被4整除，但传输速率不及MemCopy
//**********************************************************
int	ByteMemCopy(  int *DstAddr,  int *SrcAddr, int Len )
{
	int	StartByteDMA( int *DestAddr, int *SourceAddr, int TransLen );
	int GetUserDMAState();
	void StopUserDMA();
	
	int k;
	StartByteDMA( DstAddr, SrcAddr, Len );
	for(;;)
	{
		k = GetUserDMAState();
	  	if( k == 0x00 )
	       break;
	}
	StopUserDMA();
	return 0;
}

int StartByteDMA( int *DestAddr, int *SourceAddr, int TransLen )
{
	if( (PRICTL1 & 0xC) != 0)
		return 0;
	if( TransLen < 1 )
		return 0;
	PRICTL1 = 0x00002250;
	SECCTL1 = 0x0000A080;
	SRC1 = (unsigned int)SourceAddr;
	DST1 = (unsigned int)DestAddr;
    XFRCNT1 = TransLen;		
	PRICTL1 = PRICTL1 | 0x00000001;
	return 1;
}

int	StartWordDMA( int *DestAddr, int *SourceAddr, int TransLen )
{
	if( (PRICTL1 & 0xC) != 0)
		return 0;
	if( TransLen < 1 )
		return 0;
	PRICTL1 = 0x00002050;
	SECCTL1 = 0x0000A080;
	SRC1 = (unsigned int)SourceAddr;
	DST1 = (unsigned int)DestAddr;
    XFRCNT1 = TransLen;		
	PRICTL1 = PRICTL1 | 0x00000001;
	return 1;
}

int GetUserDMAState()
{
	return ( PRICTL1 & 0xC );
}

void StopUserDMA()
{
	PRICTL1 = PRICTL1 & 0xFFFFFFFE;
}


//===================  Led lighten and darken  ========================
// LED 指示灯控制程序
void LightenLed( unsigned char Flag )
{
	register unsigned int Data = 0xFFFFFFFF;
	if( Flag&0x01 )
		Data = Data & LED_GREEN_ON;
	if( Flag&0x02 )
		Data = Data & LED_RED_ON; 
	PCR1 = PCR1 & Data;
}
void DarkenLed( unsigned char Flag )
{
	register unsigned int Data = 0;
	if( Flag&0x01 )
		Data = Data | LED_GREEN_OFF;
	if( Flag&0x02 )
		Data = Data | LED_RED_OFF; 
	PCR1 = PCR1 | Data;
}

//================== delay with busy waiting, in 'ms' =================
//      NOTE: if system clock is other than 200MHz, the accuracy 
//            of the time delayed is not guaranteed!
void DelayMs(unsigned short int msTime)
{
	register int i,j;
	for(i=0; i<msTime; i++)
		for(j=0; j<11800; j++)asm(" nop  9");
}
//================== delay with busy waiting, in 'us' =================
//      NOTE: if system clock is other than 200MHz, the accuracy 
//            of the time delayed is not guaranteed!
void DelayUs(unsigned short int usTime)
{
	register int i,j;
	for(i=0; i<usTime; i++)
		for(j=0; j<12; j++)asm(" nop  9");
}


//======================== IIC bus operation ==========================
void Reset7113()
{
	PCR1 = PCR1 & 0xFFFFFFDF;    //通用I/O口DX1管脚控制SAA7113的复位
	DelayMs( 2 );
	PCR1 = PCR1 | 0x20;
	DelayMs( 2 );
}

void StartIIC()
{
	Set_SCL_Low();
	Set_SDA_Out();
	Set_SDA_High();
	Set_SCL_High();
	DelayUs( 10 );
	Set_SDA_Low();
	DelayUs( 10 );
	Set_SCL_Low();			//IIC Parked on SCL-low
	DelayUs( 10 );
}
void StopIIC()
{
	Set_SCL_High();
	Set_SDA_Out();
	Set_SDA_Low();
	DelayUs( 10 );
	Set_SDA_High();
	DelayUs( 10 );
	Set_SCL_Low();			//IIC Parked on SCL-low
	DelayUs( 10 );
}

char SendByteToIIC( uchar Data )
{
	uchar Tmp=0x80;
	int i;
	Set_SCL_Low();
	Set_SDA_Out();
	for(i=0; i<8; i++, Tmp = Tmp>>1)
	{
		if( Data & Tmp )
			Set_SDA_High();
		else
			Set_SDA_Low();
		DelayUs( 10 );
		Set_SCL_High();
		DelayUs( 10 );
		Set_SCL_Low();
		DelayUs( 10 );			
	}
	Set_SDA_In();
	Set_SCL_High();
	DelayUs( 10 );
	Tmp = Get_SDA();
	Set_SCL_Low();
	DelayUs( 10 );
	if( Tmp )
		return( 0 );   // NACK from 7113 detected!
	return 1;
}
uchar ReadByteFromIIC()
{
	uchar Data = 0, Tmp = 0x80;
	int i;
	Set_SDA_In();
	for(i=0; i<8; i++,Tmp = Tmp>>1)
	{
		Set_SCL_High();
		DelayUs( 10 );
		if( Get_SDA() )
			Data = Data | Tmp;
		Set_SCL_Low();
		DelayUs( 10 );
	}
	Set_SDA_Out();		// Send ACK signal back to 7113
	Set_SDA_Low();      // 响应信号时SDA为低
	DelayUs( 10 );		
	Set_SCL_High();
	DelayUs( 10 );
	Set_SCL_Low();
	DelayUs( 10 );
	return( Data );
}

char Write7113(uchar *pBuf, uchar StartAddress, uchar Len)  
{
	int i;
	StartIIC();	
	if( !SendByteToIIC(0x4A) )
	{
		StopIIC();
		return( ERR_IIC_BUS );
	}
	if( !SendByteToIIC(StartAddress) )
	{
		StopIIC();
		return( ERR_IIC_BUS );
	}
	for(i=0; i<Len; i++)
		if( !SendByteToIIC( *pBuf++ ) )
		{
			StopIIC();
			return( ERR_IIC_BUS );
		}
	StopIIC();
	return( 1 );
}

uchar Read7113( uchar *pBuf, uchar StartAddress, uchar Len )
{
	uchar *p = pBuf;
	int i;
	StartIIC();
	if( !SendByteToIIC(0x4A) )
	{
		StopIIC();
		return( ERR_IIC_BUS );
	}
	if( !SendByteToIIC(StartAddress) )
	{
		StopIIC();
		return( ERR_IIC_BUS );
	}	
	StartIIC();
	if( !SendByteToIIC(0x4B) )
	{
		StopIIC();
		return( ERR_IIC_BUS );
	}
	for( i=0; i<Len; i++ )
		*p++ = ReadByteFromIIC();
	StopIIC();
	return( 1 );
}

uchar DefaultConfig7113()
{
	uchar *p, Start, Len;
	if( FLAG_CVBS == 0 )
		p = (uchar*)SAA7113_CVBS_Config_Data;
		
	else
	{
		FLAG_CVBS = 1;
		p = (uchar*)SAA7113_SVIDEO_Config_Data;
	}
	for(;;)
	{
		Start = *p++;
		if( Start == 0xff )break; 
		Len = *p++;			
		Write7113(p, Start, Len);
		p += Len;
	}
	return 1;
}


interrupt void VideoAcquistion( void )
{

}

interrupt void FieldDetected( void )
{
	PCR1 = PCR1 ^ LED_GREEN_OFF;
	IER = IER & 0xffffff9c;           //禁止INT5,INT6中断

	FieldStatus++;
	FieldStatus = FieldStatus & 0x7;
							
	switch( FieldStatus )
	{
		case 0:		
				DST0 = 0x00400000;
			    XFRCNT0 = 0x00016540;		       //每块1帧，每帧25920单元
				PRICTL0 = PRICTL0 | 0x00000001;  
				break;
		case 1:
				FieldReady = 0;                    //存储在第一块区域
				++ReadyNumber;                     //传好图数+1
				PRICTL0 = PRICTL0 & 0xFFFFFFFE;
				break;
		case 2:		
				DST0 = 0x00419500;
			    XFRCNT0 = 0x00016540;		
				PRICTL0 = PRICTL0 | 0x00000001;
				break;
		case 3:
				FieldReady = 1;
				++ReadyNumber;
				PRICTL0 = PRICTL0 & 0xFFFFFFFE;
				break;
		case 4:		
				DST0 = 0x00432A00;
			    XFRCNT0 = 0x00016540;		
				PRICTL0 = PRICTL0 | 0x00000001;
				break;
		case 5:
				FieldReady = 2;
				++ReadyNumber;
				PRICTL0 = PRICTL0 & 0xFFFFFFFE;
				break;
		case 6:		
				DST0 = 0x0044BF00;
			    XFRCNT0 = 0x00016540;		
				PRICTL0 = PRICTL0 | 0x00000001;
				break;
		case 7:
				FieldReady = 3;
				++ReadyNumber;
				PRICTL0 = PRICTL0 & 0xFFFFFFFE;
				break;

	}
	asm(" nop ");
	IER = IER | 0x43;        //INT6使能
}


void StartVideoAcquistion()
{
	//////////// Global control variables	/////////////
	FieldStatus = 0;
	ImageDataCounter = 0;
	FieldReady = 0xff;
	ReadyNumber = 0;
	PCI_LOCK = 0xF;
	DSP_LOCK = 0;
	if( FLAG_FILTER != 0 && FLAG_FILTER != 1)
		FLAG_FILTER = 0;
	if( FLAG_MARK != 0 && FLAG_MARK != 1)
		FLAG_MARK = 1;
	if( BIN_THRESHOLD < 5 || BIN_THRESHOLD > 100 )
		BIN_THRESHOLD = 50;
	
	ObjPos1 = ObjPos2 = OBJ_POS = 0xffffffff;
	I11=I21=I31=I41=I51=I61=I71=I1=I2=I3=I4=I5=I6=I7=0xffffffff;
	
	DetectManner = 0;
	OBJ_LEFTTOP = 0;
	OBJ_RIGHTBOT = 0;
	FLAG_CRADLE = 0;
	dividor_image=256/number_bin;//每区四个灰度级
	Flag_pro_zero=0;//候选目标概率密度为0标志
	Flag_zero=0;//计算新位置分母(或分子)为0标志
	//////////// DMA configuration /////////////
	PRICTL0 = 0x0b014040;                     //INT5作为同步事件
	SECCTL0 = 0x0010A080;
	SRC0 = 0x03000000;
	///////////////////// Interrupt	/////////////////////
	EXTPOL = EXTPOL | 0x4;      // Falling edge triger INT6	
	CSR = CSR | 0x1;            // 可屏蔽中断使能
    ICR = 0xffff;
	IER = IER | 0x43;			// En INT5(Data Ready);  INT6(Field complete)

}

void StopVideoAcquistion()
{
	PRICTL0 = PRICTL0 & 0xFFFFFFFE;
	IER = IER & 0xffffff9c;     //禁止INT5,INT6
}


//================ Initialize DSP and its peripherals =================
int InitDSPs()
{
	//////////////// Check System States ////////////////
	SPCR1 = SPCR1 & 0xfffefffe;  //XRST=RRST=0;接收，发送复位
	PCR1 = PCR1 | 0x00003c00;    //XIOEN=RIOEN=1,FSXM=FSRM=1串口设置为通用I/O模式
	SystemState = PCR1 & 0x40;   //CLKS_STAT
	////////////////////////////////////////////////////
	if( SystemState == SYS_IN_HOST )
	{
		//PCR1 = PCR1 | 0x2020;
		SPCR0 = SPCR0 & 0xfffefffe;
		PCR0 = PCR0 | 0x3400;	//XIOEN=RIOEN=1;FSRM=1, SCL output;
		Reset7113();
		DefaultConfig7113();
		Reset7113();
		DefaultConfig7113();
	}
	///////////////// EMIF configuration ////////////////
	CE3CTL = 0xFFF20322;    // CE3-->CPLD
	CE0CTL = 0xfFFF3F33;	// CE0-->SDRAM
	SDCTL = 0x7113000;
	
	///////////////////// TIMER setup /////////////////////
	TMR0CTL = 0x300;        //方波形式
	TMR0PRD = 0x7FFFFFFF;
	TMR0CNT = 0;
	TMR0CTL = 0x3C0;
		
	return 0;	
}