tvtrack.cpp

这是图像处理在实际系统的应用源程序

// TVTrack.cpp: implementation of the CTVTrack class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Module.h"
#include "TVTrack.h"
#include "math.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CTVTrack::CTVTrack()
{
	AA=1.0,EE=1.0;   //EE=e_integ; AA=a_integ;(单位：密位)

}

CTVTrack::~CTVTrack()
{

}


/****************************************************************************
                电视跟踪模块
****************************************************************************/
//输入量：a_AT,e_ET,D (D暂时为目标的距离DT，以后用激光测出的距离DL); 输出量：a_integ，e_integ
void CTVTrack::Television(double a_AT,double e_ET,double D,double &a_integ,double &e_integ)   
{

		static double w_a[2]={0,0},w_e[2]={0,0},ax0[2]={0,0},ey0[2]={0,0},
		us1[2]={0,0}, ah[2]={0,0}, us2[2]={0,0}, eh[2]={0,0};
		double j1[2]={0,0.99005}, j2[2]={9.204,-8.21};
		ah[0]=a_integ; eh[0]=e_integ;
	
		/////////////////// 求再生角速度wa,we ///////////////////////////////////////
	
		double x,y,z,x_2,y_2,z_2,vx_2,vy_2,vz_2,wa,we;
		static double x_0,y_0,z_0,x_1,y_1,z_1,vx_1,vy_1,vz_1;
		double k1=0.02532,k2=0.01229;
		static int i_tv=97,i=0;

		x=D*cos(EE)*cos(AA);
		y=D*cos(EE)*sin(AA);
		z=D*sin(EE);
		if (i_tv>97)
		{
			switch(i_tv)
			{
	
			case 98:  x_0=x; y_0=y; z_0=z;
				break;
			case 99:  x_1=x_0+k1*(x-x_0);  y_1=y_0+k1*(y-y_0);  z_1=z_0+k1*(z-z_0);
				vx_1=(x_1-x_0)*50.0;   vy_1=(y_1-y_0)*50.0;   vz_1=(z_1-z_0)*50.0;
				break;
			default: 
				x_2=x_1+k1*(x-x_1);  y_2=y_1+k1*(y-y_1);  z_2=z_1+k1*(z-z_1);
				vx_2=(x_2-x_1)*50.0;   vy_2=(y_2-y_1)*50.0;   vz_2=(z_2-z_1)*50.0;
				x_1=x_2;				y_1=y_2;		 	 z_1=z_2;
				vx_1=vx_1+k2*(vx_2-vx_1);
				vy_1=vy_1+k2*(vy_2-vy_1);
				vz_1=vz_1+k2*(vz_2-vz_1);
			
				wa=(vy_1*cos(AA)-vx_1*sin(AA))/(D*cos(EE));
				we=(vz_1*cos(EE)-vx_1*sin(EE)*cos(AA)-vy_1*sin(EE)*sin(AA))/D;
				if ((D>5000)||(D==5000))    //不同距离时,加入的再生角速度是不同的
				{
					wa=0.0; we=0.0;
				}
				else if(D<5000&&D>1000)
				{
					wa=(0.00025*(5000-D))*wa;
					we=(0.00025*(5000-D))*we;
				}
			}
		}
	
		wa=wa*ARC_MIL; we=we*ARC_MIL;
	/////////////////////////////////////////////////////////////////////////////////////////////	
	
		for(int m=1;m>0;m--)
		{
			w_a[m]=w_a[m-1]; w_e[m]=w_e[m-1]; ax0[m]=ax0[m-1];  ey0[m]=ey0[m-1];//校正
			ah[m]=ah[m-1];  eh[m]=eh[m-1]; us1[m]=us1[m-1]; us2[m]=us2[m-1]; //积分
		}
/////////////////////////////校正环节 ///////////////////////////////////
		ax0[0]=a_AT;  ey0[0]=e_ET;  w_a[0]=w_e[0]=0.0;        
	                                                  
		for(m=0;m<2;m++)                             //将跟踪误差x0，y0变为角速度w_a，w_e
		{
			w_a[0]=w_a[0]+j1[m]*w_a[m]+j2[m]*ax0[m];  //j1和j2为参数
			w_e[0]=w_e[0]+j1[m]*w_e[m]+j2[m]*ey0[m];
		}

/////////////////////////////积分环节///////////////////////////////////////
	
		double t=0.02;
		us1[0]=w_a[0];  us2[0]=w_e[0];  ah[0]=eh[0]=0.0;

		a_integ=ah[0]=ah[1]+0.5*t*(us1[1]+us1[0]);  
		e_integ=eh[0]=eh[1]+0.5*t*(us2[1]+us2[0]);
	
		if(a_integ<0)     a_integ=a_integ+6000;
		if(a_integ>6000)  a_integ=a_integ-6000;
	
	//由于各种原因形成的扰动误差（以后添加）
	

	///////////////////////////////////////////////////////////////////////
		i_tv++;
		EE=e_integ*MIL_ARC; 
		AA=a_integ*MIL_ARC;
	

}