gps.cpp

这个程序包含了wince串口、GPS信号处理

// Gps.cpp: implementation of the CGps class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "gmark.h"
#include "define.h"
#include "Gps.h"
#include "GmarkDoc.h"
#include "GmarkView.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

FILE *fpRinex;

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CGps::CGps()
{
	m_portA.SetGps( this );
	m_portA.m_bNoRead = FALSE;
	m_portB.m_bNoRead = TRUE;

	// Initialize the local variable
	for(int i=0; i<GPS_MAX_RECNUM; i++)
	{
		m_tCommData[i] = NULL;
	}

	m_CurrIndex = 0;

	m_bGpsMsgBusy = FALSE;
}

CGps::~CGps()
{
	// Release the memory
	for(int i=0; i<GPS_MAX_RECNUM; i++)
	{
		if(m_tCommData[i] != NULL) delete m_tCommData[i];
	}
}

void CGps::OnReceived(unsigned char *buf, int nBytes)
{
	m_pLog->Write((const char *)buf);

/*	char sTemp[256];
	memset(sTemp,0,sizeof(sTemp));
	sprintf(sTemp,"nBytes=%d, len=%d, buf[0]=%2X", nBytes, strlen((const char*)buf), buf[0]);
	m_pLog->Write((const char *)sTemp);*/

	::SendMessage( m_hGmarkView, WM_GPSRECEIVED, (WORD)nBytes, (LONG)buf );
}

void CGps::AddChecksum(LPSTR cmd, LPSTR chksum)
{
	int nIndex;
	int nLength;
	unsigned char cChkSum;
	unsigned char str[4];

	if(cmd == NULL || chksum == NULL) return;

	nLength =  strlen(cmd);
	if( nLength < 1 ) return;					//empty command not need to add chksum

	memset( str, 0, sizeof(str) );

	cChkSum = 0;
	nIndex = 1;
	while( nIndex < nLength )
	{
		cChkSum = cChkSum^cmd[nIndex];
		nIndex++;
	}

	sprintf((char *)str, "*%02X", cChkSum);

	strcpy( chksum, cmd );
	chksum[nLength] = str[0];
	chksum[nLength+1] = str[1];
	chksum[nLength+2] = str[2];
}

void CGps::GetToken(LPSTR str, int n, LPSTR token)
{
	int nLength =  strlen(str);
	int nCount = 0;
	int nTokenCount = 0;
	LPSTR szResult = new char[nLength+1];

	memset( szResult, 0,  nLength+1);

	while( nCount < nLength)
	{
		if( str[nCount] == ',' || str[nCount] == '*' )
		{
			if( nTokenCount == n )
			{
				break;
			}
			else
			{
				nTokenCount++;
				memset( szResult, 0,  nLength+1);
			}
		}
		else 
		{
			szResult[strlen(szResult)] = str[nCount]; 
		}

		nCount++;
	}

	strcpy( token, szResult );

	delete szResult;
}

void CGps::CompleteMsg(unsigned char *buf, int nBytes)
{
	int nLen = nBytes;
	char *p = (char *)buf;
	char *CompMsg = new char[nLen + 1];
	int index = m_CurrIndex;
	int nIsCompleted;
	int nIsFoundPlace = 0;
	int nOrgLen = 0;
	int nCompLen= 0;

	nOrgLen = nBytes; 

	do
	{
		memset( CompMsg, 0, nLen + 1 );
		nIsCompleted = GetCompleteMsg( p, nOrgLen, CompMsg, nCompLen );

		//reset the offset in case anything left
		if( nIsCompleted == RET_OK) 
		{
			p = p + nCompLen + 2;
			nOrgLen = nOrgLen - nCompLen - 2;
		}
	
		// Find an index not completed to place the message
		index = m_CurrIndex;
		nIsFoundPlace = 0;
		do 
		{
			if( m_tCommData[index] == NULL ) 
			{
				m_tCommData[index] = new COMM_DATA;
				m_tCommData[index]->CompleteFlg = 0;
				memset( m_tCommData[index]->Message, 
						0, 
						sizeof(m_tCommData[index]->Message) );

				nIsFoundPlace = 1;
				break;
			}
			else 
			{
				if( m_tCommData[index]->CompleteFlg == 1 )
				{
					index = IndexIncrease( index );
					continue;
				}

				nIsFoundPlace = 1;
				break;
			}
		} while(index != m_CurrIndex);	//search only one round

		// append the message
		if(nIsFoundPlace == 1)
		{
			memcpy( m_tCommData[index]->Message + strlen(m_tCommData[index]->Message), 
					(const char *)CompMsg, nCompLen );
		}
		else 
		{
			memset( m_tCommData[index]->Message, 
					0, 
					sizeof(m_tCommData[index]->Message) );
			memcpy( m_tCommData[index]->Message, 
					(const char *)CompMsg, nCompLen );
		}

		// set the flag
		if(nIsCompleted == RET_OK)
		{
			m_tCommData[index]->CompleteFlg = 1;

			// refresh the current index
			m_CurrIndex = IndexIncrease( index );
		}

	}while( (nIsCompleted == RET_OK) && (strlen(p) > 0) );


	delete CompMsg;
}

void CGps::CreateGPSData()
{
	int index;
	char szCmdType[GPS_MAX_NEMASIZE+1];
	int nIsUpdate;

	index = m_CurrIndex;
	nIsUpdate = 0;
	do {
		if( m_tCommData[index] != NULL &&
			m_tCommData[index]->CompleteFlg == 1 ) 
		{
			// process message only when the checksum is right
			if( IsChecksumCorrect( m_tCommData[index]->Message) )
			{
				memset( szCmdType, 0, sizeof(szCmdType) );

				//according the command type do corresponding deal
				GetToken( m_tCommData[index]->Message, 0, szCmdType );

				if(strcmp(szCmdType, "$GPGGA") == 0)
				{
					ParseGGA( m_tCommData[index]->Message );
					
					// need to update gps data in the view
					nIsUpdate = 1;
				}
				else if(strcmp(szCmdType, "$GPGLL") == 0)
				{
					ParseGLL( m_tCommData[index]->Message );
					nIsUpdate = 1;
				}
				else if(strcmp(szCmdType, "$GPGSA") == 0)
				{
					ParseGSA( m_tCommData[index]->Message );
					nIsUpdate = 1;
				}
				else if(strcmp(szCmdType, "$PASHR") == 0)
				{
					GetToken( m_tCommData[index]->Message, 1, szCmdType );
					
					if(strcmp(szCmdType, "PBN") == 0)	// PBN data
					{
						ParsePBN( m_tCommData[index]->Message );
					}
					if(strcmp(szCmdType, "MCA") == 0)	// MCA data
					{
						ParseMCA( m_tCommData[index]->Message );
					}
					if(strcmp(szCmdType, "SNV") == 0)	// SNV data
					{
						ParseSNV( m_tCommData[index]->Message );
					}
					else if(strcmp(szCmdType, "CRT") == 0)
					{
						ParseCRT( m_tCommData[index]->Message );
					}
				}
			}

			// release the record memory
			delete m_tCommData[index];
			m_tCommData[index] = NULL;
		}
		
		index = IndexIncrease( index );
	}
	while( index != m_CurrIndex );

	if( nIsUpdate == 1 )
	{
		// Notify the view to refresh the gps data
		::PostMessage( m_hGmarkView, WM_GPSUPDATE, 0, 0 );
	}
}

int CGps::IndexIncrease(int index)
{
	if( index >= GPS_MAX_RECNUM - 1 ) index = 0;
	else index++;

	return index;
}

int CGps::IndexDecrease(int index)
{
	if( index <= 0 ) index = GPS_MAX_RECNUM - 1;
	else index--;

	return index;
}

void CGps::ParseGGA(char *Message)
{
	char szToken[GPS_MAX_NEMASIZE+1];
	double c = 111352.0044;	// = r*pai/180 = 6380000(m)*3.1415926/180

	GetToken( Message, 6, szToken );
	m_tGpsData.Status = atoi(szToken);

	GetToken( Message, 7, szToken );
	m_tGpsData.SatCount = atoi(szToken);

	GetToken( Message, 4, szToken );
	m_tGpsData.Longitude = DegToVal(szToken);	//Jing du

	m_tGpsData.LonDeg = (int)m_tGpsData.Longitude;
	m_tGpsData.LonMin = (int)((m_tGpsData.Longitude - m_tGpsData.LonDeg)*60);
	m_tGpsData.LonSec = ((m_tGpsData.Longitude - m_tGpsData.LonDeg)*60 - m_tGpsData.LonMin)*60;

	GetToken( Message, 2, szToken );
	m_tGpsData.Latitude = DegToVal(szToken);	//Wei du

	m_tGpsData.LatDeg = (int)m_tGpsData.Latitude;
	m_tGpsData.LatMin = (int)((m_tGpsData.Latitude - m_tGpsData.LatDeg)*60);
	m_tGpsData.LatSec = ((m_tGpsData.Latitude - m_tGpsData.LatDeg)*60 - m_tGpsData.LatMin)*60;

	// chang (longitude,latitude) to (x,y)
//	m_tGpsData.X =(m_tGpsData.Longitude - m_orig_lon)*c;
//	m_tGpsData.Y =(m_tGpsData.Latitude - m_orig_lat)*c;
}

void CGps::ParseGLL(char *Message)
{
	char szToken[GPS_MAX_NEMASIZE+1];

	GetToken( Message, 1, szToken );
	m_tGpsData.Latitude = DegToVal(szToken);

	GetToken( Message, 3, szToken );
	m_tGpsData.Longitude = DegToVal(szToken);

	GetToken( Message, 6, szToken );
//	m_tGpsData.Status = atoi(szToken);
}

void CGps::ParseGSA(char *Message)
{
	char szToken[GPS_MAX_NEMASIZE+1];

	GetToken( Message, 15, szToken );
	m_tGpsData.PDOP = atof(szToken);

	GetToken( Message, 16, szToken );
	m_tGpsData.HDOP = atof(szToken);

	GetToken( Message, 17, szToken );
	m_tGpsData.VDOP = atof(szToken);
}

void CGps::ParseCRT(char *Message)
{
	char szToken[GPS_MAX_NEMASIZE+1];

	GetToken( Message, 2, szToken );
	m_tGpsData.Status = atoi(szToken);

	GetToken( Message, 3, szToken );
	m_tGpsData.SatCount = atoi(szToken);

	GetToken( Message, 5, szToken );
	m_tGpsData.X = atof(szToken);

	GetToken( Message, 6, szToken );
	m_tGpsData.Y = atof(szToken);

	GetToken( Message, 13, szToken );
	m_tGpsData.PDOP = atof(szToken);

	GetToken( Message, 14, szToken );
	m_tGpsData.HDOP = atof(szToken);

	GetToken( Message, 15, szToken );
	m_tGpsData.VDOP = atof(szToken);
}

void CGps::ParsePBN(char *Message)
{
#define PBN_DATA_SIZE 56
	char PbnData[PBN_DATA_SIZE];

	memcpy( PbnData, Message + 11, PBN_DATA_SIZE );

	if( !BinCheckSum(PbnData, PBN_DATA_SIZE) ) return;

	PBN_DATA *pbnData;

	// get PBN_DATA structure
	pbnData = (PBN_DATA *)PbnData;

	// write PBN_DATA to RINEX file
	fwrite(pbnData, 1, sizeof(PBN_DATA), fpRinex);
}

void CGps::ParseMCA(char *Message)
{
#define MCA_DATA_SIZE 37
	char McaData[MCA_DATA_SIZE];

	memcpy( McaData, Message + 11, MCA_DATA_SIZE );

	if( !BinCheckSum(McaData, MCA_DATA_SIZE, 1) ) return;

	MCA_DATA *mcaData;

	// get MCA_DATA structure
	mcaData = (MCA_DATA *)McaData;

	// write MCA_DATA to RINEX file
	fwrite(mcaData, 1, sizeof(MCA_DATA), fpRinex);
}

void CGps::ParseSNV(char *Message)
{
#define SNV_DATA_SIZE 132
	char SnvData[SNV_DATA_SIZE];

	memcpy( SnvData, Message + 11, SNV_DATA_SIZE );

	if( !BinCheckSum(SnvData, SNV_DATA_SIZE) ) return;

	SNV_DATA *snvData;

	// get SNV_DATA structure
	snvData = (SNV_DATA *)SnvData;

	// write SNV_DATA to RINEX file
	fwrite(snvData, 1, sizeof(SNV_DATA), fpRinex);
}

double CGps::DegToVal(char *deg)
{
	double result;
	char *p=NULL;
	char d[4];
	char m[9];

	memset( d, 0, sizeof(d) );
	memset( m, 0, sizeof(m) );

	p = strstr( deg, "." );
	if(p)
	{
		int n = p-deg;
		strncpy( d, deg, n-2 );
		strncpy( m, deg+n-2, 8 );
	}
	else
	{
		strncpy( d, deg, sizeof(d)-1 );
		strcpy( m, "" );
	}

	result = atoi(d) + atof(m)/60;
	return result;
}

int CGps::SendCmd(LPSTR cmd)
{
	LPSTR pSendCmd;
	char szCrLf[3] = {0x0d, 0x0a, 0};
	int nLength;
	int nRet;

	nLength =  strlen(cmd);
	pSendCmd = new char[nLength + 6];
	memset( pSendCmd, 0, nLength + 6 );

	// add checksum to command
	AddChecksum(cmd, pSendCmd);	

	// add <cr>+<lf>
	strcpy( pSendCmd + strlen(pSendCmd), szCrLf );
	
	nRet = m_portA.SendData(pSendCmd, strlen(pSendCmd));

	if( (unsigned int)nRet != strlen(pSendCmd) ) nRet = RET_NG;
	else nRet = RET_OK;

	delete pSendCmd;

	return nRet;
}

int CGps::Initialize(int baudrate)
{
	int nRet = RET_OK;

	//set the GGA message to be output automatically every 1 second.
	SendCmd("$PASHS,NME,GGA,A,ON,1");

	//Initialize the COM6 for port B of GPS module

	return nRet;
}

int CGps::Close()
{
	int nRet = RET_OK;

	if(!m_portA.Close()) nRet = RET_NG;
	if(!m_portB.Close()) nRet = RET_NG;

	//close the raw data file
	fclose(fpRinex);

	return nRet;
}

void CGps::SetViewHandle(HWND hView)
{
	m_hGmarkView = hView;
}

int CGps::Connect()
{
	int nRet = RET_OK;

	//Initialize the COM2 for port A of GPS module

	while(1)
	{
		//the default bps of port A is 4800
#ifndef _WIN32_WCE_CEPC
		if( m_portA.Open(2, 115200) == FALSE )
#else
		if( m_portA.Open(1, 115200) == FALSE )
#endif
		{
			nRet = RET_NG;
			break;
		}

		//the default bps of port B is 4800
#ifndef _WIN32_WCE_CEPC
		if( m_portB.Open(6, 57600) == FALSE )
#else
		if( m_portB.Open(0, 57600) == FALSE )
#endif
		{
			nRet = RET_NG;
			break;
		}

		break;
	}

	//open file to write raw data
	char filename[MAX_PATH];
	
	memset( filename, 0, sizeof(filename) );
	this->m_GmarkView->CreateFilename(filename, FN_RAW);

	fpRinex = fopen( filename, "wb" );

	return nRet;
}

HWND CGps::GetViewHandle()
{
	return m_hGmarkView;
}

int CGps::GetCompleteMsg(LPSTR OrgMsg, int nOrgLen, LPSTR CompMsg, int & nCompLen)
{
	int nRet = RET_NG;
	char * p = NULL;

	for(int i = 0; i < nOrgLen-1; i++)
	{
		if( OrgMsg[i] == 0x0d &&
			OrgMsg[i+1] == 0x0a )
		{
			p = OrgMsg + i;
			break;
		}
	}

	if( p == NULL )
	{
		memcpy( CompMsg, OrgMsg, nOrgLen );
		nCompLen = nOrgLen;
		nRet = RET_NG;
	}
	else
	{
		nCompLen = p - OrgMsg;
		memcpy( CompMsg, OrgMsg, nCompLen );
		nRet = RET_OK;
	}

	return nRet;	
}

void CGps::ClearGpsData()
{
	m_tGpsData.Latitude = -1;
	m_tGpsData.Longitude = -1;
	m_tGpsData.SatCount = -1;
	m_tGpsData.Status = -1;

	// Notify the view to refresh the gps data
	::PostMessage( m_hGmarkView, WM_GPSUPDATE, 0, 0 );
}

void CGps::WriteRTCM(char *rtcm)
{
	m_portB.SendData( rtcm, strlen(rtcm) );
}

void CGps::SetOrigPosition(double orig_lon, double orig_lat)
{
	m_orig_lon = orig_lon;
	m_orig_lat = orig_lat;
}

BOOL CGps::IsChecksumCorrect(char *message)
{
	char szCmd[GPS_MAX_NEMASIZE+1];
	char szChecksum[GPS_MAX_NEMASIZE+1];
	char *p;

	memset(szCmd, 0, sizeof(szCmd));
	memset(szChecksum, 0, sizeof(szChecksum));

	if(strncmp(message, "$PASHR,PBN,", 11)==0)
	{
		return TRUE;
	}

	p = strstr( message, "*" );
	if(!p)
	{
		return FALSE;
	}

	strncpy( szCmd, message, p-message );
	
	AddChecksum( szCmd, szChecksum );

	if( strcmp( message, szChecksum ) != 0 )
	{
		return FALSE;
	}

	return TRUE;
}

void CGps::SetViewWnd(CWnd *pWnd)
{
	m_GmarkView = (CGmarkView *)pWnd;
}

BOOL CGps::BinCheckSum(char *buf, int len, int BinType)
{
	if(BinType == 0)
	{
		unsigned short chsum, checksum;
		unsigned short * p;

		p = (unsigned short *)buf;
		chsum = 0;

		for(int i=0; i<len-2; i+=2)
		{
			chsum += *p;
			p++;
		}

		checksum = *p;

		if(chsum == checksum) return TRUE;
		else return FALSE;
	}
	else
	{
		unsigned char chsum, checksum;
		unsigned char * p;

		p = (unsigned char *)buf;
		chsum = 0;

		for(int i=0; i<len-1; i++)
		{
			chsum = (*p)^chsum;
			p++;
		}

		checksum = *p;

		if(chsum == checksum) return TRUE;
		else return FALSE;
	}
}