navigationdlg.cpp

VC开发

//		m_Com2.GetInput();//先预读缓冲区以清除全部数据m_Com.SetCommPort(1);//选择com1
	

	m_Com3.SetCommPort(5);//选择com1
		m_Com3.SetInBufferSize(1024);
		m_Com3.SetOutBufferSize(512);
	m_Com3.SetSettings("38400,n,8,1");
	if(!m_Com3.GetPortOpen())
			m_Com3.SetPortOpen(TRUE);
	m_Com3.SetInputMode(1);
		m_Com3.SetRThreshold(1);
			m_Com3.SetInputLen(1);
 //	m_Com3.SetInputLen(0);
//		m_Com3.GetInput();//先预读缓冲区以清除全部数据
	 
start=clock();
	
m_Time="0";	
middle=clock();
tt.Format("%d",(middle-start)/1000); 
m_Time=tt;
	UpdateData(FALSE);
	wbibx1=0;
	wbibx2=0;
	wbibx3=0;
	while(((middle-start)%10)==0)
{

        wbibx1=wbibx3;
		wbiby1=wbiby3;
		wbibz1=wbibz3;
	
	//	buchang();
    	wbibx3=gyo1*180/PI;//figx;/*每个10毫秒读入陀螺的角速度*/
	    wbiby3=gyo2*180/PI;
    	wbibz3=gyo3*180/PI;
     	wbibx2=(wbibx1+wbibx3)/2;
		wbiby2=(wbiby1+wbiby3)/2;
	    wbibz2=(wbibz1+wbibz3)/2;
     	wbpbx1=wbibx1-wbipx;
		wbpby1=wbiby1-wbipy;
		wbpbz1=wbibz1-wbipz;
		wbpbx2=wbibx2-wbipx;
		wbpby2=wbiby2-wbipy;
		wbpbz2=wbibz2-wbipz;
		wbpbx3=wbibx3-wbipx;
		wbpby3=wbiby3-wbipy;
		wbpbz3=wbibz3-wbipz;
 
		/*---------------------龙格库塔法解方程-----------------------*/
		ak0=xt*(-wbpbx1*q1-wbpby1*q2-wbpbz1*q3);
		ak1=xt*(wbpbx1*q0+wbpbz1*q2-wbpby1*q3);
		ak2=xt*(wbpby1*q0-wbpbz1*q1+wbpbx1*q3);
		ak3=xt*(wbpbz1*q0+wbpby1*q1-wbpbx1*q2);
		q0a=q0+0.5*ak0;
		q1a=q1+0.5*ak1;
		q2a=q2+0.5*ak2;
		q3a=q3+0.5*ak3;
		bk0=xt*(-wbpbx2*q1a-wbpby2*q2a-wbpbz2*q3a);
		bk1=xt*(wbpbx2*q0a+wbpbz2*q2a-wbpby2*q3a);
		bk2=xt*(wbpby2*q0a-wbpbz2*q1a+wbpbx2*q3a);
		bk3=xt*(wbpbz2*q0a+wbpby2*q1a-wbpbx2*q2a);
		q0b=q0+0.5*bk0;
		q1b=q1+0.5*bk1;
		q2b=q2+0.5*bk2;
		q3b=q3+0.5*bk3;
		ck0=xt*(-wbpbx2*q1b-wbpby2*q2b-wbpbz2*q3b);
		ck1=xt*(wbpbx2*q0b+wbpbz2*q2b-wbpby2*q3b);
		ck2=xt*(wbpby2*q0b-wbpbz2*q1b+wbpbx2*q3b);
		ck3=xt*(wbpbz2*q0b+wbpby2*q1b-wbpbx2*q2b);
		q0c=q0+ck0;
		q1c=q1+ck1;
		q2c=q2+ck2;
		q3c=q3+ck3;
		dk0=xt*(-wbpbx3*q1c-wbpby3*q2c-wbpbz3*q3c);
		dk1=xt*(wbpbx3*q0c+wbpbz3*q2c-wbpby3*q3c);
		dk2=xt*(wbpby3*q0c-wbpbz3*q1c+wbpbx3*q3c);
		dk3=xt*(wbpbz3*q0c+wbpby3*q1c-wbpbx3*q2c);
		q0=q0+1/6.0*(ak0+bk0+bk0+ck0+ck0+dk0);
		q1=q1+1/6.0*(ak1+bk1+bk1+ck1+ck1+dk1);
		q2=q2+1/6.0*(ak2+bk2+bk2+ck2+ck2+dk2);
		q3=q3+1/6.0*(ak3+bk3+bk3+ck3+ck3+dk3);
				qn=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
			q0=q0/qn;
			q1=q1/qn;
			q2=q2/qn;
			q3=q3/qn;

		q00=q0*q0;       /*-------------------四元数计算姿态矩阵---------------------*/
		q11=q1*q1;
		q22=q2*q2;
		q33=q3*q3;
		q12=q1*q2;
		q03=q0*q3;
		q13=q1*q3;
		q02=q0*q2;
		q23=q2*q3;
		q01=q0*q1;
		BT11=q00+q11-q22-q33;
		BT21=q12+q12+q03+q03;
		BT31=q13+q13-q02-q02;
		BT12=q12+q12-q03-q03;
		BT22=q00-q11+q22-q33;
		BT32=q23+q23+q01+q01;
		BT13=q13+q13-q02+q02;
    	BT32=q23+q23-q01-q01;
		BT33=q00-q11-q22+q33;
	
	     
	/*---------------------------------计算姿态角------------------------------*/
	if(fabs(BT32)<(1-ep))
		{
			the=atan(BT32/sqrt(1-BT32*BT32));
			if(fabs(BT22)>ep)
			{	
				psi=atan(-BT12/BT22);
				if(BT22>0)
				{	
					if(fabs(BT12)>ep) 
					{
						if(-BT12>0) 
						{
							psi=psi;
						} else
						{
							psi=psi+2*PI;
						}
					} else
					{
						psi=0;
					}
				} else
				{
					psi=psi+PI;
				}
			} else 
				if (-BT12>0)
				{
					psi=PI/2;
				} else
				{
					psi=PI*3/2;
				}

			if(fabs(BT33)>ep)
			{	
				gam=atan(-BT31/BT33);
				if(BT33>0) 
				{
					gam=gam;
				} else
					if (fabs(BT31)>ep)
					{
						if(-BT31<0) 
						{
							gam=gam-PI;
						} else
						{
							gam=gam+PI;
						}
					} else 
					{
						gam=PI;
					}
			} else
				if (-BT31>0)
				{
					gam=PI/2;
				} else
				{
					gam=-PI/2;
				}
		} else
		{
			if(BT32>0)
			{
				the=PI/2;
			} else
			{
				the=-PI/2;
			}
			gam=0;
			if (fabs(BT11)>ep)
			{
				psi=atan(BT21/BT11);
				if(BT11>0) 
				{
					if (fabs(BT21)>ep)
					{
						if (BT21>0)
						{
							psi=psi;
						} else
						{
							psi=psi+2*PI;
						}
					} else
					{
						psi=0;
					}
				} else
				{
					psi=psi+PI;
				}
			} else
				if (BT31>0)
				{
					psi=PI/2;
				} else
				{
					psi=PI*3/2;
				}
		}
       /*这里为相对于地球表面的输出*/
		printf("the=%f\t",the*180/PI);
		  printf("gam=%f\t",gam*180/PI);
				             printf("psi=%f\n",psi*180/PI);
   /*地球速率*/
        
	        
			wbipx=BT11*wpiex+BT21*wpiey+BT31*wpiez;
			wbipy=BT12*wpiex+BT22*wpiey+BT32*wpiez;
			wbipz=BT13*wpiex+BT23*wpiey+BT33*wpiez;

	I++;
m_the.Format("%f",the);
	UpdateData(FALSE);


m_gam.Format("%f",gam);
	UpdateData(FALSE);


m_psi.Format("%f",psi);
	UpdateData(FALSE);



	}

}


void CNavigationDlg::OnOnCommMscomm1() 
{
	

	VARIANT vResponse;
	COleSafeArray m_Input2;
	int nEvent;
	long i,length,kk;
    CString str,tt;
	BYTE data[42];
	if(f1==0)
	{
		kk=m_Com.GetInBufferCount();
	if(kk>40)
	{
			vResponse=m_Com.GetInput();
		str=(char*)(unsigned char*)vResponse.parray->pvData;
		if(*str=='.')
		{
			m_Com.SetInputLen(10);
			m_Com.GetInput();

m_Com.SetRThreshold(21);
	m_Com.SetInputLen(21);
f1=1;
		}

	}
		
		
	}
  else if(f1==1) {
	  m_Text="";
		nEvent=m_Com.GetCommEvent();
		switch(nEvent)
		{
	case 2:
		{
			kk=m_Com.GetInBufferCount();
if(kk>20)
{
	vResponse=m_Com.GetInput();//read
			m_Input2=vResponse;
			length=m_Input2.GetOneDimSize();
			for(i=0;i<length;i++)
				m_Input2.GetElement(&i,data+i);
	for(i=0;i<length;i++)//减10是为了不显示校验位和空格位
	{ char a=*(char*)(data+i);

	str.Format("%c",a);

	m_Text+=str;
  

	}
	UpdateData(FALSE);

  
gyo1=atof(m_Text); 
UpdateData(true);
}
break;
		}

		case 3: break;	AfxMessageBox("3");
		case 4: break;	AfxMessageBox("4");
		case 5: break;	AfxMessageBox("5");
		case 6: break;	AfxMessageBox("6");

		}
	
}
}

void CNavigationDlg::OnOnCommMscomm2() 
{

	VARIANT vResponse;
	COleSafeArray m_Input2;
	
	int nEvent;
	long i,length,kk;
    CString str,tt;
	BYTE data[42];
if(f2==0)
	{
		kk=m_Com2.GetInBufferCount();
		if(kk>40){
			vResponse=m_Com2.GetInput();
		str=(char*)(unsigned char*)vResponse.parray->pvData;
		if(*str=='.')
		{
			m_Com2.SetInputLen(10);
			m_Com2.GetInput();
f2=1;
		m_Com2.SetRThreshold(21);
	m_Com2.SetInputLen(21);
		}

	}
		
	}
else if(f2==1)
{	nEvent=m_Com2.GetCommEvent();
	m_Text2="";
		switch(nEvent)
		{
	case 2:
		{
			kk=m_Com2.GetInBufferCount();
if(kk>20)
{
	vResponse=m_Com2.GetInput();//read
			m_Input2=vResponse;
			length=m_Input2.GetOneDimSize();
			for(i=0;i<length;i++)
				m_Input2.GetElement(&i,data+i);
	for(i=0;i<length;i++)//减10是为了不显示校验位和空格位
	{ char a=*(char*)(data+i);

	str.Format("%c",a);

m_Text2+=str;
  

	}
	UpdateData(FALSE);
 
gyo2=atof(m_Text2); 
	UpdateData(true);


}
break;
		}

		case 3: break;	AfxMessageBox("3");
		case 4: break;	AfxMessageBox("4");
		case 5: break;	AfxMessageBox("5");
		case 6: break;	AfxMessageBox("6");
		}
			
}
}

void CNavigationDlg::OnOnCommMscomm3() 
{

	VARIANT vResponse;
	COleSafeArray m_Input2;
	
	int nEvent;
	long i,length,kk;
    CString str,tt;

	BYTE data[42];
if(f3==0)
	{
		kk=m_Com3.GetInBufferCount();
		if(kk>40){
			vResponse=m_Com3.GetInput();
		str=(char*)(unsigned char*)vResponse.parray->pvData;
		if(*str=='.')
		{
			m_Com3.SetInputLen(10);
			m_Com3.GetInput();
         f3=1;
		m_Com3.SetRThreshold(21);
	m_Com3.SetInputLen(21);
		}
	
	}
	
	}
else if(f3==1) 
{	nEvent=m_Com3.GetCommEvent();
		m_Text3="";
		switch(nEvent)
		{
	case 2:
		{
			kk=m_Com3.GetInBufferCount();
if(kk>20)
{
	vResponse=m_Com3.GetInput();//read
			m_Input2=vResponse;
			length=m_Input2.GetOneDimSize();
			for(i=0;i<length;i++)
				m_Input2.GetElement(&i,data+i);
	for(i=0;i<length;i++)//减10是为了不显示校验位和空格位
	{ char a=*(char*)(data+i);

	str.Format("%c",a);

m_Text3+=str;
  

	}
	UpdateData(FALSE);
 
gyo3=atof(m_Text3); 
	UpdateData(true);

}
break;
		}

		case 3: break;	AfxMessageBox("3");
		case 4: break;	AfxMessageBox("4");
		case 5: break;	AfxMessageBox("5");
		case 6: break;	AfxMessageBox("6");
		}
			
}
}