navigationdlg.cpp

VC开发

// navigationDlg.cpp : implementation file
//

#include "stdafx.h"
#include "navigation.h"
#include "navigationDlg.h"
#include "time.h"
#include "math.h"

#include "stdio.h"
#include "stdlib.h"
#include "math.h"
#include "time.h" 
#define new DEBUG_NEW
#define PI 3.14159265
#define r 6378393.0
#define w 7.2921158e-5
#define e 0.003367
#define ep 0.000001
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();
	
      // Dialog Data
	//{{AFX_DATA(CAboutDlg)
	enum { IDD = IDD_ABOUTBOX };
	//}}AFX_DATA

	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CAboutDlg)
	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support
	//}}AFX_VIRTUAL

// Implementation
protected:
	//{{AFX_MSG(CAboutDlg)
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
	//{{AFX_DATA_INIT(CAboutDlg)
	//}}AFX_DATA_INIT
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CAboutDlg)
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
	//{{AFX_MSG_MAP(CAboutDlg)
		// No message handlers
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CNavigationDlg dialog

CNavigationDlg::CNavigationDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CNavigationDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CNavigationDlg)
	m_Text = _T("");
	m_Text2 = _T("");
	m_Text3 = _T("");
	m_the = _T("");
	m_gam = _T("");
	m_psi = _T("");
	m_Time = _T("");
	mt=_T("");
		mt2=_T("");
		mt3=_T("");
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CNavigationDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CNavigationDlg)
	DDX_Control(pDX, IDCLOSE, m_Save);
	DDX_Control(pDX, IDC_BUTTON1, m_Begin);
	DDX_Text(pDX, IDC_EDIT1, m_Text);
	DDX_Text(pDX, IDC_EDIT2, m_Text2);
	DDX_Text(pDX, IDC_EDIT3, m_Text3);
	DDX_Text(pDX, IDC_EDIT4, m_the);
	DDX_Text(pDX, IDC_EDIT5, m_gam);
	DDX_Text(pDX, IDC_EDIT6, m_psi);
	DDX_Control(pDX, IDC_MSCOMM1, m_Com);
	DDX_Control(pDX, IDC_MSCOMM2, m_Com2);
	DDX_Control(pDX, IDC_MSCOMM3, m_Com3);
	DDX_Text(pDX, IDC_EDIT7, m_Time);
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CNavigationDlg, CDialog)
	//{{AFX_MSG_MAP(CNavigationDlg)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDC_BUTTON1, OnButton1)
	ON_BN_CLICKED(IDCLOSE, OnClose)
	ON_BN_CLICKED(IDOK, OnOpen)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

BEGIN_EVENTSINK_MAP(CNavigationDlg, CDialog)
    //{{AFX_EVENTSINK_MAP(CNavigationDlg)
	ON_EVENT(CNavigationDlg, IDC_MSCOMM1, 1 /* OnComm */, OnOnCommMscomm1, VTS_NONE)
	ON_EVENT(CNavigationDlg, IDC_MSCOMM2, 1 /* OnComm */, OnOnCommMscomm2, VTS_NONE)
	ON_EVENT(CNavigationDlg, IDC_MSCOMM3, 1 /* OnComm */, OnOnCommMscomm3, VTS_NONE)
	//}}AFX_EVENTSINK_MAP
END_EVENTSINK_MAP()


/////////////////////////////////////////////////////////////////////////////
// CNavigationDlg message handlers
int CNavigationDlg::sign(double x)
{
if(x>=0)
        return 1;
	  else
  	    return -1;
}

/*void CNavigationDlg::lvbo(double gyor[N])
{//AR2建模
double a[2][2];
double b[2][2];
double q[2][2];
double p[2][2];
double pk[2][2];
double x[2];
double xk[2];
double fai[2];
double c[2];
double k[2];
double dm=0.1;
double da;
double lz[N];//滤波后值
int i,j,t;

double y[N-2],xx[N-2][2],xxt[2][N-2];
double y2[2],aa[2][2],cc[2][N-2],ani[2][2];
double yy1,y3;
double hlsa;
double sum=0;



	for(i=0;i<N-2;i++)
		y[i]=gx[i+2];
	
	for (i=0;i<N-2;i++)
		{
		xx[i][0]=gx[i+1];
	xx[i][1]=gx[i];
	}

	for (i=0;i<N-2;i++)
		for(j=0;j<2;j++)
			xxt[j][i]=xx[i][j];	

		for(i=0;i<2;i++)
		for(j=0;j<2;j++)
		{
			aa[i][j]=0;
			for(t=0;t<N-2;t++)
				aa[i][j]+=xxt[i][t]*xx[t][j];
		}

	hlsa=aa[0][0]*aa[1][1]-aa[0][1]*aa[1][0];
	if(hlsa!=0)
	{
	ani[0][0]=aa[1][1]/hlsa;
	ani[0][1]=-aa[0][1]/hlsa;
	ani[1][0]=-aa[1][0]/hlsa;
	ani[1][1]=aa[0][0]/hlsa;

		for(i=0;i<2;i++)
			for(j=0;j<N-2;j++)
		{
			cc[i][j]=0;
			for(t=0;t<2;t++)
				cc[i][j]+=ani[i][t]*xxt[t][j];
		}
	
		for(i=0;i<2;i++)
			
		{
			fai[i]=0;
			for(t=0;t<N-2;t++)
				fai[i]+=cc[i][t]*y[t];
		}

	for(i=0;i<2;i++)
	yy1=0;
	for(t=0;t<N-2;t++)
		yy1+=y[t]*y[t];
		for(j=0;j<2;j++)
		{
			y2[j]=0;
			for(t=0;t<N-2;t++)
				y2[j]+=y[t]*xx[t][j];
		}
	y3=0;
	for(t=0;t<2;t++)
		y3+=y2[t]*fai[t];
 	da=(yy1-y3)/(N-2);
 
	//Kalman滤波

	a[0][0]=fai[0];
	a[0][1]=fai[1];
	a[1][0]=1;
	a[1][1]=0;
	b[0][0]=1;
	b[0][1]=0;
	b[1][0]=0;
	b[1][1]=0;
	c[0]=1;
	c[1]=0;
	p[0][0]=1;
	p[0][1]=0;
	p[1][0]=0;
	p[1][1]=1;
	q[0][0]=da;
	q[0][1]=0;
	q[1][0]=0;
	q[1][1]=da;
	x[0]=0;
	x[1]=0;
	sum=0;
	for(i=0;i<N;i++)
	{
		xk[0]=fai[0]*x[0]+fai[1]*x[1];
		xk[1]=x[0];
		pk[0][0]=fai[0]*fai[0]*p[0][0]+fai[0]*fai[1]*(p[1][0]+p[0][1])
			+fai[1]*fai[1]*p[1][1]+b[0][0]*b[0][0]*da
			+b[0][1]*b[0][1]*da;
		pk[0][1]=fai[0]*p[0][0]+fai[1]*p[1][0]+b[0][0]*b[1][0]*da
			+b[0][1]*b[1][1]*da;
		pk[1][0]=fai[0]*p[0][0]+fai[1]*p[0][1]
			+b[0][0]*b[1][0]*da
			+b[0][1]*b[1][1]*da;
		pk[1][1]=p[0][0]+b[1][0]*b[1][0]*da+b[1][1]*b[1][1]*da;
		k[0]=pk[0][0]/(pk[0][0]+dm*dm);
		k[1]=pk[1][0]/(pk[0][0]+dm*dm);
		p[0][0]=pk[0][0]*(1-k[0]);
		p[0][1]=pk[0][1]*(1-k[0]);
		p[1][0]=-k[1]*pk[0][0]+pk[1][0];
		p[1][1]=-k[1]*pk[0][1]+pk[1][1];
		x[0]=xk[0]+k[0]*(gy[i]-xk[0]);
		x[1]=xk[1]+k[1]*(gy[i]-xk[0]);
		lz[i]=x[0];  //滤波后数据
		gsx[i]=lz[i];     //gsy[i]为y轴陀螺滤波后数据
		sum=sum+gsx[i];  
		}
	avex=sum/N;            //滤波后的均值
	}
}

*/
BOOL CNavigationDlg::OnInitDialog()
{
	CDialog::OnInitDialog();
CNavigationDlg T;
	// Add "About..." menu item to system menu.

	// IDM_ABOUTBOX must be in the system command range.
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);

	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != NULL)
	{
		CString strAboutMenu;
		strAboutMenu.LoadString(IDS_ABOUTBOX);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}

	// Set the icon for this dialog.  The framework does this automatically
	//  when the application's main window is not a dialog
	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon
	I=0;
	the=0*PI/180;  psi=0*PI/180;  gam=0*PI/180;
	alp=0;    phi=39*PI/180;    

	xt=0.005; 
	/*地球速率*/
	wpiex=w*sin(alp)*cos(phi);
	wpiey=w*cos(alp)*cos(phi);
	wpiez=w*sin(phi);
				 /*初始捷联矩阵*/
	BT11=cos(gam)*cos(psi)-sin(gam)*sin(the)*sin(psi);
    BT12=-cos(the)*sin(psi);
	BT13=sin(gam)*cos(psi)+cos(gam)*sin(the)*sin(psi);
	BT21=  cos(gam)*sin(psi)+sin(gam)*sin(the)*cos(psi);
	BT22=cos(the)*cos(psi);
	BT23=sin(gam)*sin(psi)-cos(gam)*sin(the)*cos(psi);
	BT31=-sin(gam)*cos(the);
	BT32=sin(the);
	BT33=cos(gam)*cos(the);

	/*-------------------- 初始四元数归一化   ------------------------*/

	q1=0.5*sqrt(1.0+BT11-BT22-BT33);
	q2=0.5*sqrt(1.0-BT11+BT22-BT33);
	q3=0.5*sqrt(1.0-BT11-BT22+BT33);
	q0=sqrt(1.0-q1*q1-q2*q2-q3*q3);

	q0=q0;
    q1=q1*sign(BT32-BT23);
	q2=q2*sign(BT13-BT31);
	q3=q3*sign(BT21-BT11);

	/*---------------------机体相对于地球角速率----------------------------*/

	
	wbipx=BT11*wpiex+BT21*wpiey+BT31*wpiez;
	wbipy=BT12*wpiex+BT22*wpiey+BT32*wpiez;
	wbipz=BT13*wpiex+BT23*wpiey+BT33*wpiez;
		
	// TODO: Add extra initialization here
	
	return TRUE;  // return TRUE  unless you set the focus to a control
}


void CNavigationDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialog::OnSysCommand(nID, lParam);
	}
}

// If you add a minimize button to your dialog, you will need the code below
//  to draw the icon.  For MFC applications using the document/view model,
//  this is automatically done for you by the framework.

void CNavigationDlg::OnPaint() 
{
	if (IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);

		// Center icon in client rectangle
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
	}
}

// The system calls this to obtain the cursor to display while the user drags
//  the minimized window.
HCURSOR CNavigationDlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}

void CNavigationDlg::OnButton1() 
{
	OnOK();

}

void CNavigationDlg::OnCancel() 
{
	// TODO: Add extra cleanup here
	
	CDialog::OnCancel();
}

void CNavigationDlg::OnClose() 
{
	CString str;
	
 m_Com.SetPortOpen(FALSE);
m_Com2.SetPortOpen(FALSE);
m_Com3.SetPortOpen(FALSE);

end=clock();
str.Format("%f",(end-start)/1000); 

m_Time=str;
	UpdateData(FALSE);
	
}

void CNavigationDlg::OnOpen() 
{
double ak0,ak1,ak2,ak3,bk0,bk1,bk2,bk3,ck0,ck1,ck2,ck3,dk0,dk1,dk2,dk3;
double q0a,q1a,q2a,q3a,q0b,q1b,q2b,q3b,q0c,q1c,q2c,q3c;
double q00,q11,q22,q33,q12,q03,q13,q02,q23,q01;	
CString tt;
f1=0; f2=0;f3=0;

m_Com.SetCommPort(3);//选择com1
	m_Com.SetInBufferSize(1024);
		m_Com.SetOutBufferSize(512);
	m_Com.SetSettings("38400,n,8,1");
	if(!m_Com.GetPortOpen())
			m_Com.SetPortOpen(TRUE);
	m_Com.SetInputMode(1);
	m_Com.SetRThreshold(1);
	m_Com.SetInputLen(1);
//m_Com.SetInputLen(0);
//	m_Com.GetInput();//先预读缓冲区以清除全部数据


		m_Com2.SetCommPort(4);//选择com1
	m_Com2.SetInBufferSize(1024);
		m_Com2.SetOutBufferSize(512);
	m_Com2.SetSettings("38400,n,8,1");
	if(!m_Com2.GetPortOpen())
			m_Com2.SetPortOpen(TRUE);
	m_Com2.SetInputMode(1);
	m_Com2.SetRThreshold(1);
			m_Com2.SetInputLen(1);
 //	m_Com2.SetInputLen(0);