latuscontroller.cpp

可以接入GPS等三个传感器,并且封装在一个组件里.上层只需拷贝几个文件,就可直接调用,组件的更新不影响上层使用

#include "StdAfx.h"
#include "LatusController.h"
#include "PDI.h"
LatusController::LatusController(void)
{
	m_bCnxReady = FALSE;
	m_nReceptor = 1;
	m_nMarker = 1;
	m_nBaudRate = PI_BR_115200;
	m_nParity = PI_PARITY_NONE;
	m_nPort = 1;


    m_pdiMDat.Empty();
	m_pdiMDat.Append( PDI_MODATA_FRAMECOUNT );
    m_pdiMDat.Append( PDI_MODATA_POS );
    m_pdiMDat.Append( PDI_MODATA_ORI );

//	m_pdiSer.SetPort(m_nPort);
	m_nPort = m_pdiSer.GetPort();
	m_pdiSer.SetBaudRate(m_nBaudRate); 
	m_pdiSer.SetParity(m_nParity);
	


	m_pdiLatus.SetPnoBuffer( g_pMotionBuf, BUFFER_SIZE );
}

LatusController::~LatusController(void)
{
	Clear();
}

void LatusController:: AddMsg(string &csMsg)
{
	cout << csMsg ;
}

void LatusController::AddResultMsg(LPCSTR szCmd)
{
	string msg;
	msg = string(szCmd) + " result: " + string( m_pdiLatus.GetLastResultStr() ) + "\r\n";
	AddMsg( msg );
}

void LatusController::SetupDevice(void)
{
	// Set output data list
	m_pdiLatus.SetSDataList( -1, m_pdiMDat );
	AddResultMsg("SetSDataList");

	// Read AutoLaunch
	BOOL m_bAutoLaunch;
	m_pdiLatus.GetAutoLaunchMode( m_bAutoLaunch );
	AddResultMsg("GetAutoLaunchMode");

	// Read WhoAmI
	DoWhoAmI();

	// Read Receptor Map
	DoReceptorMap();

	// Read Active Marker Map
	DoMarkerMap();

	// Read Active Marker IDs
	DoMarkerIDs();
}

void LatusController::DoWhoAmI(void)
{
	LPCSTR szWho;

	if (!(m_pdiLatus.WhoAmI( szWho )))
	{
		AddResultMsg("WhoAmI");
	}
	else
	{
		CHAR szMsg[1000];
		sprintf(szMsg, "WhoAmI: %d receptors, %d markers supported.\r\n%s \r\n\r\n",
			m_pdiLatus.MaxReceptors(), m_pdiLatus.MaxMarkers(), szWho );
		AddMsg( string(szMsg) );
	}
}

void LatusController::DoMarkerMap(void)
{
	CHAR szMsg[100];
	DWORD dwMarkerMap;

	m_pdiLatus.GetActiveMarkerMap( dwMarkerMap );
	AddResultMsg("GetActiveMarkerMap");

	sprintf(szMsg, "ActiveMarkerMap: %#x\r\n", dwMarkerMap );
	AddMsg( string(szMsg) );
}

void LatusController::DoMarkerIDs(void)
{
	DWORD dwMarkerMap;

	m_pdiLatus.GetActiveMarkerMap( dwMarkerMap );

	if (dwMarkerMap == 0)
	{
		return;
	}

	string msg = "Active Markers: ";

	for (INT i=0; i<LATUS_MAX_MARKERS; i++)
	{
		if (dwMarkerMap & (1 << i))
		{
			LPCSTR szID;
			CHAR szMsg[100];

			if (!(m_pdiLatus.WhoAmIMarker( i+1, szID )))
			{
				sprintf(szMsg, "  %d - Error,", i+1);
			}
			else
			{
				sprintf(szMsg, "  %d - %s,", i+1, szID );
			}
			msg += string(szMsg);

		}
	}

	if (msg.at(msg.length() - 1) == ',')
	{
		msg.erase(msg.end() - 1);
	}
	msg += string("\r\n");
	AddMsg(msg);
}

void LatusController::DoReceptorMap(void)
{
	CHAR szMsg[100];

	DWORD dwRcpMap;
	DWORD dwRcpAlignMap;

	m_pdiLatus.GetRcpMap( dwRcpMap );
	AddResultMsg("GetRcpMap");

	m_pdiLatus.GetRcpAlignMap( dwRcpAlignMap );
	AddResultMsg("GetRcpAlignMap");

	sprintf(szMsg, "ActiveStationMap: %#x\r\n", dwRcpMap );
	AddMsg( string(szMsg) );

	sprintf(szMsg, "AlignedReceptorMap: %#x\r\n", dwRcpAlignMap );
	AddMsg( string(szMsg) );
}

bool LatusController::Launch(int receptorID)
{
	BOOL ret = m_pdiLatus.MarkerLaunch( receptorID );

	CHAR szMsg[100];
	sprintf(szMsg, "MarkerLaunch %d", receptorID );
	AddResultMsg( string(szMsg).c_str() );

	DoMarkerMap();
	DoMarkerIDs();

	return ret != 0;
}

void LatusController::Unlaunch(int markerID)
{
	m_pdiLatus.MarkerUnlaunch( markerID );

	CHAR szMsg[100];
	sprintf(szMsg, "MarkerUnlaunch %d", markerID );
	AddResultMsg( string(szMsg).c_str() );

	DoMarkerMap();
	DoMarkerIDs();
}

bool LatusController::Init(int receptorID)
{

	m_nReceptor = receptorID;
	ePiErrCode error;

	string msg;
	
	if (!(m_pdiLatus.CnxReady()))
	{
		ePiCommType eType = m_pdiLatus.DiscoverCnx();
		
		switch (eType)
		{
		case PI_CNX_USB:
			msg = "USB Connection: " + string( m_pdiLatus.GetLastResultStr() ) + "\r\n";
			break;
		case PI_CNX_SERIAL:
			msg = "Serial Connection: " + string( m_pdiLatus.GetLastResultStr() ) + "\r\n";
			break;
		default:
			{error = m_pdiLatus.GetLastResult();
			msg = "DiscoverCnx result: " + string( m_pdiLatus.GetLastResultStr() ) + "\r\n";}
			break;
		}
		m_bCnxReady = m_pdiLatus.CnxReady();
		AddMsg( msg );

		if (m_bCnxReady)
		{
			SetupDevice();
		}
	}
	else
	{
		msg = "Already connected\r\n";
		m_bCnxReady = TRUE;
		AddMsg( msg );
	}

	if (m_bCnxReady)
	{
		m_pdiLatus.StartContPno( 0 );
		AddResultMsg("StartContPno");
	}

	return m_bCnxReady != 0;
}

void LatusController::Clear(void)
{
    if (!(m_pdiLatus.CnxReady()))
    {
		string msg = "Already disconnected\r\n";
        m_bCnxReady = FALSE;
		AddMsg( msg );
    }
    else
    {
		m_pdiLatus.StopContPno();
		AddResultMsg("StopContPno");

        m_pdiLatus.Disconnect();
        m_bCnxReady = m_pdiLatus.CnxReady();

		CHAR szMsg[100];
		sprintf(szMsg, "Disconnect result: %s\r\n", m_pdiLatus.GetLastResultStr());
		AddMsg( string(szMsg) );
    }
}

bool LatusController::GetFrame(double& x, double& y, double& z, double& az, double& el, double& ro)
{
	PBYTE pBuf;
	DWORD dwSize;

	if (!(m_pdiLatus.LastPnoPtr( pBuf, dwSize )))
	{
		AddResultMsg("LastPnoPtr");
		return false;
	}

	//string msg;
    INT	nItemCount = m_pdiMDat.NumItems();
    DWORD i=0;

    while ( i<dwSize)
    {
		BINHDR * pHdr = (BINHDR *)&(pBuf[i]);

        // skip rest of header
        i += sizeof(BINHDR);

		if (pHdr->length)
		{
			DWORD fc, sync, ts;

			//CHAR szMsg[100];
			//sprintf(szMsg, "Marker %d: ", pHdr->station );
			//msg = string(szMsg);

			for (INT j=0; j<nItemCount; j++)
			{
				switch(m_pdiMDat.ItemAt(j))
				{
				case PDI_MODATA_POS:
					x = *((PFLOAT)&pBuf[i]);
					i += sizeof(FLOAT);
					y = *((PFLOAT)&pBuf[i]);
					i += sizeof(FLOAT);
					z = *((PFLOAT)&pBuf[i]);
					i += sizeof(FLOAT);

					//sprintf(szMsg, "%f %f %f ", x, y, z );
					break;
				case PDI_MODATA_ORI:
					az = *((PFLOAT)&pBuf[i]);
					i += sizeof(FLOAT);
					el = *((PFLOAT)&pBuf[i]);
					i += sizeof(FLOAT);
					ro = *((PFLOAT)&pBuf[i]);
					i += sizeof(FLOAT);

					//sprintf(szMsg, "%f %f %f ", az, el, ro );
					break;
				case PDI_MODATA_TIMESTAMP:
					ts = *((DWORD*)&pBuf[i]);
					i += sizeof(DWORD);

					//sprintf(szMsg, "%d ", ts );
					break;
				case PDI_MODATA_FRAMECOUNT:
					fc = *((DWORD*)&pBuf[i]);
					i += sizeof(DWORD);

					//sprintf(szMsg, "%d ", fc);
					break;
				case PDI_MODATA_EXTSYNC:
					sync = *((DWORD*)&pBuf[i]);
					i += sizeof(DWORD);

					//sprintf(szMsg, "%d ", sync);
					break;
				case PDI_MODATA_CRLF:
					i += 2;
					break;
				case PDI_ODATA_SPACE:
					i++;
					break;
				default:
					break;
				}

				//msg += string(szMsg);
			}
			//msg += "\r\n";
		}
		else
		{
			//msg = "Empty Frame\r\n";
			return false;
		}
		//AddMsg(msg);
    }

	return true;
}