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()
{
	m_ov.Offset = 0;
	m_ov.OffsetHigh = 0;
	m_ov.hEvent = NULL;

}
	
void CHmdMouse :: SetHmdMouse(char *com,int baud,short hmdaddr,short mouseaddr)
{
    DCB    dcb;
    DWORD  dwError,count;
    BOOL   fSuccess;
	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 */
            FILE_FLAG_OVERLAPPED,//0,    /* 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; 
  // dcb.fRtsControl =RTS_CONTROL_ENABLE; 

   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;

}

  
  
////////////////////////////////////////////////////////////////////////////
//  bird_hemisphere     -   Set the Birds Hemisphere                      //
//  Return Value:       TRUE if successful                                //
//			            FALSE if unsuccessful                             //
//			            ESC_SEL if ESC selected                           //
//  Remarks:            prompt the user for the Bird hemisphere and send  //
//			            down coresponding hemisphere command to the Bird  //
////////////////////////////////////////////////////////////////////////////

int CHmdMouse ::bird_hemisphere(void)
{
	
	BOOL result;
	DWORD dwError, count;
	static char * hemisphere_menuptr[] = 
				  {"Hemisphere Options:",  // menu options 
				   "Forward",
				   "Aft",
				   "Upper",
				   "Lower",
				   "Left",
				   "Right"};
    static unsigned char hemisphere_cdata[] = {'L',0,0};  // command string to BIRD

    /*/	Send the Menu to the User 
    switch (sendmenu(hemisphere_menuptr,6))
    {

	 //   Setup the Command string to the Bird as a function of the
	 //  User menu selection.....
	 //  .....2 data bytes must be set for HEMI_AXIS and HEMI_SIGN
	
	case 0: // Forward 
	    hemisphere_cdata[1] = 0;       //set XYZ character 
	    hemisphere_cdata[2] = 0;       //set Sine character 
	    break;

	case 1: // Aft 
	    hemisphere_cdata[1] = 0;       // set XYZ character
	    hemisphere_cdata[2] = 1;       // set Sine character 
	    break;

	case 2: // Upper 
	    hemisphere_cdata[1] = 0xc;     // set XYZ character 
	    hemisphere_cdata[2] = 1;       // set Sine character 
	    break;

	case 3: // Lower 
	    hemisphere_cdata[1] = 0xc;     // set XYZ character 
	    hemisphere_cdata[2] = 0;       // set Sine character 
	    break;

	case 4: // Left 
	    hemisphere_cdata[1] = 6;       // set XYZ character 
	    hemisphere_cdata[2] = 1;       // set Sine character 
	    break;

	case 5: // Right 
	    hemisphere_cdata[1] = 6;       // set XYZ character 
	    hemisphere_cdata[2] = 0;       // set Sine character 
	    break;

	case ESC_SEL:
	    return(ESC_SEL);
    }*/
   hemisphere_cdata[1] = 0xc;
   hemisphere_cdata[2] = 1;

//	Send the Command
    result = WriteFile(m_hCom, hemisphere_cdata, 3, &count, &m_ov);
	if(!result)
	{
		
		switch(dwError = GetLastError())
		{
		case ERROR_IO_PENDING:
			result = GetOverlappedResult(m_hCom,	// Handle to COMM port 
									  &m_ov,		// Overlapped structure
									  &count,		// Stores number of bytes sent
									  TRUE); 			// Wait flag
			if(!result)
				return -1;
			break;
		default:
			return -1;
			break;
		}
	}
	else
	{
		if(count == 3)
			return 1; 
		else
		{
		   //printf("Failed to set hemisphere...\n");
		   return -1;
		}
	}
//    printf("Hemisphere Data Sent to the Bird\n\r");
//    printf("press any key to display data...\n");
//	_getch();

    return 1;
}



//////////////////////////////////////////////////////
//		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)
{
	/*m_ov.Offset = 0;
	m_ov.OffsetHigh = 0;
	m_ov.hEvent = NULL;*/
	unsigned char posorientcmd[2];
	char          rs232tofbbcmd[2];
	DWORD         count, dwError;
	BOOL result;
	
	switch (DataMode)
	{