hmdmouse.cpp

GPS的串口读取数据过程,并且进行了坐标转换,转化为通用的坐标系

//////////////////////////////////////////////////////////////
// hmdmouse.cpp : implementation of the CHmdmouse class		//
//////////////////////////////////////////////////////////////

#include "hmdmouse.h"
#include "math.h"

//////////////////////////////////////////////////////////////////////////////
// Convert data from bird to Pos and(or) Orientation according to DataMode	//
// DataMode  : the mode of data read from bird 								//
// *data     : point to input data buffer									//
// *floatdata: point to output data buffer									//
//////////////////////////////////////////////////////////////////////////////
void ByteToData(short DataMode,short *data ,float *floatdata)
{
	int   i;
	short datasize;   //data's size

	switch (DataMode) {
	case POS:
	case ANGLE:
		datasize = 3;
		break;
	case MATRIX:
		datasize = 9;
		break;
	case QUATER:
		datasize = 4;
		break;
	case POSANGLE:
		datasize = 6;
		break;
	case POSMATRIX:
		datasize = 12;
		break;
	case POSQUATER:
		datasize = 7;
		break;
	}

   	/*
	   MOTOROLA and some RISC CPUs place
	   the high order byte of a 16 bit word in the lower address of
	   memory whereas INTEL like CPUs place the high order bytes in the
	   higher addresses...therefore this operation must be CPU
		   independent
	*/
	for (i = 0;i < datasize;i++){
	       *data++ = (short)((((short)(*(unsigned char *) data) & 0x7F) |
				(short)(*((unsigned char *) data+1)) << 7)) << 2;
        }
	
	data -= datasize;

	/*
			data  change
	*/
	switch (DataMode)
	{
	case POS:
		for (i = 0;i < 3;i++) floatdata[i] = (float)(*data++*POSK36);
		break;
	case ANGLE:
		for (i=0;i<3;i++) floatdata[i] = (float)(*data++*ANGK);
		break;
	case MATRIX:
		for (i=0;i<9;i++) floatdata[i] = (float)(*data++*WTF);
		break;
	case QUATER:
		for (i=0;i<4;i++) floatdata[i] = (float)(*data++*WTF);
		break;
	case POSANGLE:
		for (i=0;i<3;i++) floatdata[i] = (float)(*data++*POSK36);
		for (i=3;i<6;i++) floatdata[i] = (float)(*data++*ANGK);
		break;
	case POSMATRIX:
		for (i=0;i<3;i++) floatdata[i] = (float)(*data++*POSK36);
		for (i=3;i<12;i++) floatdata[i] = (float)(*data++*WTF);
		break;
	case POSQUATER:
		for (i=0;i<3;i++) floatdata[i] = (float)(*data++*POSK36);
		for (i=3;i<7;i++) floatdata[i] = (float)(*data++*WTF);
		break;
	}
}

//////////////////////////////////////
//    Set FBB Address				//
//     hmdaddr:		HMD address		//
//     mouseaddr:	3DMouse address //
//////////////////////////////////////
void CHmdMouse:: SetHmdMouseAddr(short hmdaddr,short mouseaddr)
{
	m_HmdAddr   = hmdaddr;
	m_MouseAddr = mouseaddr;
}

////////////////////////////////////////////////////////////
//           Class CHmdMouse's Constructor                //
//     com:       serial port							  //
//     baud:      current baud rate                       //
//     hmdaddr:   HMD address							  //
//     mouseaddr: 3D mouse address						  //
////////////////////////////////////////////////////////////

CHmdMouse :: CHmdMouse(char *com,int baud,short hmdaddr,short mouseaddr)
{
	SetHmdMouse(com,baud,hmdaddr,mouseaddr);
}
	
void CHmdMouse :: SetHmdMouse(char *com,int baud,short hmdaddr,short mouseaddr)
{
    DCB    dcb;
    DWORD  dwError,count;
    BOOL   fSuccess;
	char   birdchangevaluecmd[] = {'P',0,0,0,0};
	short  flocksize = 2;

	m_hCom = CreateFile(com,
            GENERIC_READ | GENERIC_WRITE,
            0,    /* comm devices must be opened w/exclusive-access */
            NULL, /* no security attrs */
            OPEN_EXISTING, /* comm devices must use OPEN_EXISTING */
            0,    /* not overlapped I/O */
            NULL  /* hTemplate must be NULL for comm devices */
           );

    if (m_hCom == INVALID_HANDLE_VALUE) {
         dwError = GetLastError();
		     /* handle error */
		 return ;
    }

	fSuccess = SetupComm(m_hCom,128,128);
	if (!fSuccess) {
		return;
	}

/*
 * Omit the call to SetupComm to use the default queue sizes.
 * Get the current configuration.
 */

    fSuccess = GetCommState(m_hCom, &dcb);
	if (!fSuccess) {
    /* Handle the error. */
		return ;
    }

	fSuccess = GetCommState(m_hCom,&m_SaveDCB);
	if (!fSuccess) {
    /* Handle the error. */
		return ;
    }

/* Fill in the DCB: baud=9600, 8 data bits, no parity, 1 stop bit. */

   dcb.BaudRate = baud;
   dcb.ByteSize = 8;
   dcb.Parity = NOPARITY;
   dcb.StopBits = ONESTOPBIT;
   dcb.fRtsControl =RTS_CONTROL_DISABLE; 

   fSuccess = SetCommState(m_hCom, &dcb);

   Sleep(1000);

   if (!fSuccess) {
    /* Handle the error. */
	   return ;
   }

   COMMTIMEOUTS CommTimeouts={25,150,150,150,150} ;
   
   fSuccess = SetCommTimeouts( m_hCom,	// handle of communications device
                              &CommTimeouts	// address of communications time-out structure
							 );

   	m_HmdAddr   = hmdaddr;
	m_MouseAddr = mouseaddr;
/*
		set bird's address
*/
	birdchangevaluecmd[1] = 50;
	birdchangevaluecmd[2] = (unsigned char)flocksize;
	fSuccess = WriteFile(m_hCom, birdchangevaluecmd,3,&count,NULL);
	Sleep(DELAYTIME + 20);
}

//////////////////////////////////////////////////////
//		CHmdMouse		destruction					//
//////////////////////////////////////////////////////

CHmdMouse::~CHmdMouse()
{
	DCB   dcb;
	float RestoreAngle[3];
	int i;
    BOOL fSuccess;

	for (i=0;i<3;i++)		RestoreAngle[i] = 0.0f;
	
	/*
			restore bird's status
	*/
	BirdAngleAlign(RestoreAngle,m_HmdAddr);  
	BirdAngleAlign(RestoreAngle,m_MouseAddr);

	fSuccess = SetCommState(m_hCom, &m_SaveDCB);
	if (fSuccess) {
		GetCommState(m_hCom, &dcb);
		dcb.fRtsControl = RTS_CONTROL_ENABLE; 
		SetCommState(m_hCom, &dcb);
		Sleep(1000);
		CloseHandle(m_hCom);
	}
}

//////////////////////////////////////////////////////////////
///           CheckSystem:Check Bird is FLY or not			//
//////////////////////////////////////////////////////////////
BOOL CHmdMouse::CheckSystem()
{
	unsigned char birdgetaddrmodecmd[] = {'O',0};
    unsigned char parameter[2];
    DWORD         count;
	BOOL          fsuccess;

    fsuccess=WriteFile(m_hCom, birdgetaddrmodecmd,2,&count,NULL);	
	Sleep(DELAYTIME);
	if ((!fsuccess) || (count != 2)) return FALSE;

	fsuccess=ReadFile(m_hCom, parameter,2,&count,NULL);
	Sleep(DELAYTIME);
	if ((!fsuccess) || (count != 2)) return FALSE;

    return TRUE;
}

//////////////////////////////////////////
//   Initial  FBB: Angle Align			//
//////////////////////////////////////////
void CHmdMouse:: InitFbb()
{
	float mousePos[3],hmdPos[3],mouseOrient[9],hmdOrient[9],MouseSum[3],HmdSum[3];
	short button,i,j;
	
	GetDoubleBirdData(ANGLE ,mousePos,mouseOrient,hmdPos,hmdOrient,&button);
	GetDoubleBirdData(ANGLE ,mousePos,mouseOrient,hmdPos,hmdOrient,&button);
	GetDoubleBirdData(ANGLE ,mousePos,mouseOrient,hmdPos,hmdOrient,&button);

	for (i=0;i<3;i++)
	{
		MouseSum[i] = 0.0f;
		HmdSum[i] = 0.0f;
	}

    for (i=0;i<10;i++){
		GetDoubleBirdData(ANGLE ,mousePos,mouseOrient,hmdPos,hmdOrient,&button);
		for (j = 0;j < 3;j++){
		    MouseSum[j] += mouseOrient[j];
			HmdSum[j] += hmdOrient[j];
		}
	}

	for (j=0;j<3;j++){
        MouseSum[j] /= 10.0f;
		HmdSum[j] /= 10.0f;
	}

	BirdAngleAlign(MouseSum,m_MouseAddr);                //Hmd Align
	BirdAngleAlign(HmdSum,m_HmdAddr);                  //3Dmouse Align
	
	GetDoubleBirdData(ANGLE ,mousePos,mouseOrient,hmdPos,hmdOrient,&button);
	GetDoubleBirdData(ANGLE ,mousePos,mouseOrient,hmdPos,hmdOrient,&button);
}

////////////////////////////////////////////////////////////////////
//    Reset Fbb: Restore the original serial port configuration	  //
////////////////////////////////////////////////////////////////////
void CHmdMouse::ResetFbb()
{
	int   i;
	float RestoreAngle[3];

	for (i=0;i<3;i++){
		RestoreAngle[i] = 0.0f;
	}

    BirdAngleAlign(RestoreAngle,m_HmdAddr);
	BirdAngleAlign(RestoreAngle,m_MouseAddr);
	
	InitFbb();
}

////////////////////////////////////////////////////////////
//           ReSet   FBB   Configuration                  //
//     com:       serial port							  //
//     baud:      current baud rate                       //
//     hmdaddr:   HMD address							  //
//     mouseaddr: 3D mouse address						  //
////////////////////////////////////////////////////////////

BOOL CHmdMouse::SetComFbb(char *com,int baud,short hmdaddr,short mouseaddr)
{
	DCB    dcb;
    DWORD  dwError,count;
    BOOL   fSuccess;
	char   birdchangevaluecmd[] = {'P',0,0,0,0};
	short  flocksize = 2;

	m_hCom = CreateFile(com,
            GENERIC_READ | GENERIC_WRITE,
            0,    /* comm devices must be opened w/exclusive-access */
            NULL, /* no security attrs */
            OPEN_EXISTING, /* comm devices must use OPEN_EXISTING */
            0,    /* not overlapped I/O */
            NULL  /* hTemplate must be NULL for comm devices */
           );

    if (m_hCom = INVALID_HANDLE_VALUE) {
         dwError = GetLastError();
		     /* handle error */
		 return(FALSE);
    }

/*
 * Omit the call to SetupComm to use the default queue sizes.
 * Get the current configuration.
 */

    fSuccess = GetCommState(m_hCom, &dcb);

    if (!fSuccess) {
    /* Handle the error. */
		return(FALSE);
    }

/* Fill in the DCB: baud=9600, 8 data bits, no parity, 1 stop bit. */

   dcb.BaudRate = baud;
   dcb.ByteSize = 8;
   dcb.Parity = NOPARITY;
   dcb.StopBits = ONESTOPBIT;
   dcb.fRtsControl =RTS_CONTROL_DISABLE; 

   fSuccess = SetCommState(m_hCom, &dcb);

   Sleep(2000);

   if (!fSuccess) {
    /* Handle the error. */
	   return(FALSE);
   }
   
   m_HmdAddr   = hmdaddr;
   m_MouseAddr = mouseaddr;
   //      Set bird count
   birdchangevaluecmd[1] = 50;
   birdchangevaluecmd[2] = (unsigned char)flocksize;
   WriteFile(m_hCom, birdchangevaluecmd,3,&count,NULL);
   Sleep(DELAYTIME + 10);

   return (TRUE);
}

//////////////////////////////////////////////
//     SetMouseMode:   set mouse output		//
//    mouseaddr:     mouse address			//
//    buttonmode:    0-----cancel			//
//                   1-----enable			//
//////////////////////////////////////////////
BOOL CHmdMouse::SetMouseMode(short mouseaddr,short buttonmode)
{
	char   button_cdata[] = {'M',0};
	char   rs232tofbbcmd[2];
	DWORD  count;
		
	rs232tofbbcmd[0] = (char)(0xF0|mouseaddr); //Set Current Bird's Address
    WriteFile(m_hCom,rs232tofbbcmd,1 ,&count,NULL);
	Sleep(DELAYTIME);
		
	button_cdata[1] = (char)buttonmode;             //Set 3Dmouse Button mode
    WriteFile(m_hCom,button_cdata,2,&count,NULL);
	Sleep(DELAYTIME);

	return (TRUE);
}

//////////////////////////////////////////////////////////
//		SetDataMode:  Set data output mode				//
//			DataMode: specify data mode					//
//                    POS,ANGLE,MATRIX,QUATER,POSANGLE,	//
//						POSMATRIX,POSQUATER				//
//			birdaddr: bird's address					//
//////////////////////////////////////////////////////////
short  CHmdMouse::SetDataMode(short DataMode,short birdaddr)
{
	unsigned char posorientcmd[2];
	char          rs232tofbbcmd[2];