parser.cpp

Read nmea file and send to com port. you can easly simulate the gps application

///////////////////////////////////////////////////////////////////////////////
// NMEAParser.cpp: 
// Desctiption:	Implementation of the CNMEAParser class.
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 1998-2002 VGPS
// All rights reserved.
//
// VGPS licenses this source code for use within your application in
// object form. This source code is not to be distributed in any way without
// prior written permission from VGPS.
//
// Visual Source Safe: $Revision: 8 $
///////////////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "Parser.h"
#include "NmeaParser.h"

#define MAXFIELD	25		// maximum field length

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CNMEAParser::CNMEAParser()
{
	m_nState = NP_STATE_SOM;
	m_dwCommandCount = 0;
	m_dwCommandEtc = 0;
	m_dwCrcCount= 0;
	m_dwErrorCount = 0;
	m_bSimYear = TRUE;
	m_wCmdIdx = 0;
	m_wDataIdx = 0;
	m_wIndex = 0;
	Reset();
}

CNMEAParser::~CNMEAParser()
{
}

///////////////////////////////////////////////////////////////////////////////
// ParseBuffer:	Parse the supplied buffer for NMEA sentence information. Since
//				the parser is a state machine, partial NMEA sentence data may
//				be supplied where the next time this method is called, the
//				rest of the partial NMEA sentence will complete	the sentence.
//
//				NOTE:
//
// Returned:	TRUE all the time....
///////////////////////////////////////////////////////////////////////////////
BOOL CNMEAParser::ParseBuffer(BYTE *pBuff, DWORD dwLen)
{
	GpsMsgErr = TRUE;
	for(DWORD i = 0; i < dwLen; i++)
	{
		ProcessNMEA(pBuff[i]);
	}
	if(GpsMsgErr)
		return TRUE;
	return FALSE;
}

///////////////////////////////////////////////////////////////////////////////
// ProcessNMEA: This method is the main state machine which processes individual
//				bytes from the buffer and parses a NMEA sentence. A typical
//				sentence is constructed as:
//
//					$CMD,DDDD,DDDD,....DD*CS<CR><LF>
//
//				Where:
//						'$'			HEX 24 Start of sentence
//						'CMD'		Address/NMEA command
//						',DDDD'		Zero or more data fields
//						'*CS'		Checksum field
//						<CR><LF>	Hex 0d 0A End of sentence
//
//				When a valid sentence is received, this function sends the
//				NMEA command and data to the ProcessCommand method for
//				individual field parsing.
//
//				NOTE:
//						
///////////////////////////////////////////////////////////////////////////////
void CNMEAParser::ProcessNMEA(BYTE btData)
{
	switch(m_nState)
	{
		///////////////////////////////////////////////////////////////////////
		// Search for start of message '$'
		case NP_STATE_SOM :
			if(btData == '$')
			{
				m_btChecksum = 0;			// reset checksum
				m_btNewChecksum = 0;
				m_wIndex = 0;				// reset index
				m_wCmdIdx = 0;
				m_nState = NP_STATE_CMD;
			}
			m_pCmdData[m_wCmdIdx++] = btData;
		break;

		///////////////////////////////////////////////////////////////////////
		// Retrieve command (NMEA Address)
		case NP_STATE_CMD :
			m_pCmdData[m_wCmdIdx++] = btData;
			if(btData != ',' && btData != '*')
			{
				m_pCommand[m_wIndex++] = btData;
				m_btChecksum ^= btData;

				// Check for command overflow
				if(m_wIndex >= NP_MAX_CMD_LEN)
				{
					m_nState = NP_STATE_SOM;
				}
			}
			else
			{
				m_pCommand[m_wIndex] = '\0';	// terminate command
				m_btChecksum ^= btData;
				m_wIndex = 0;
				m_nState = NP_STATE_DATA;		// goto get data state
				m_wDataIdx = m_wCmdIdx;
			}
		break;

		///////////////////////////////////////////////////////////////////////
		// Store data and check for end of sentence or checksum flag
		case NP_STATE_DATA :
			m_pCmdData[m_wCmdIdx++] = btData;
			if(btData == '*') // checksum flag?
			{
				m_pData[m_wIndex] = '\0';
				m_nState = NP_STATE_CHECKSUM_1;
			}
			else // no checksum flag, store data
			{
				//
				// Check for end of sentence with no checksum
				//
				if(btData == '\r')
				{
					m_pData[m_wIndex] = '\0';
					ProcessCommand(m_pCommand, m_pData);
					for ( int pi=0; pi<4; pi++ )
						m_pCommPort[pi].WriteComm((unsigned char *)m_pData, m_wIndex) ;
					m_nState = NP_STATE_SOM;
					return;
				}

				//
				// Store data and calculate checksum
				//
				m_btChecksum ^= btData;
				m_pData[m_wIndex] = btData;
				if(++m_wIndex >= NP_MAX_DATA_LEN) // Check for buffer overflow
				{
					m_nState = NP_STATE_SOM;
					m_dwErrorCount++;
					// cglee error (m_pCommand, m_pData)
					GpsMsgErr = FALSE;   //051101hj
					TRACE("\r\nBUF ERROR[%d][%s:%s]",m_dwErrorCount, m_pCommand, m_pData);
				}
			}
		break;

		///////////////////////////////////////////////////////////////////////
		case NP_STATE_CHECKSUM_1 :
			m_pCmdData[m_wCmdIdx++] = btData;
			if( (btData - '0') <= 9)
			{
				m_btReceivedChecksum = (btData - '0') << 4;
			}
			else
			{
				m_btReceivedChecksum = (btData - 'A' + 10) << 4;
			}

			m_nState = NP_STATE_CHECKSUM_2;

		break;

		///////////////////////////////////////////////////////////////////////
		case NP_STATE_CHECKSUM_2 :
			m_pCmdData[m_wCmdIdx++] = btData;
			if( (btData - '0') <= 9)
			{
				m_btReceivedChecksum |= (btData - '0');
			}
			else
			{
				m_btReceivedChecksum |= (btData - 'A' + 10);
			}

			if(m_btChecksum == m_btReceivedChecksum)
			{
				// ProcessCommand(m_pCommand, m_pData);

				ProcessCommand(m_pCommand, &m_pCmdData[m_wDataIdx]);

				{
					m_btNewChecksum = 0;
					for ( int ll=1;ll<m_wCmdIdx-3;ll++)
						m_btNewChecksum ^= m_pCmdData[ll];

					BYTE high, low;
					high = (m_btNewChecksum&0xF0)>>4;
					low = (m_btNewChecksum&0xF);

					if ( high > 10 ) high += 'A';
					else high += '0';

					if ( low > 10 ) low += 'A';
					else low += '0';

					m_pCmdData[m_wCmdIdx-2] = high;
					m_pCmdData[m_wCmdIdx-1] = low;
				}

				m_pCmdData[m_wCmdIdx++] = '\r';
				m_pCmdData[m_wCmdIdx++] = '\n';
				m_pCmdData[m_wCmdIdx+1] = 0;
				for ( int pi=0; pi<4; pi++ )
					m_pCommPort[pi].WriteComm((unsigned char *)m_pCmdData, m_wCmdIdx) ;
			}
			else {
				m_dwCrcCount++;
				GpsMsgErr = FALSE;   //051101hj
				TRACE("\r\nCRC ERROR[%d][%s:%s][%02x:%02x]", m_dwCrcCount, m_pCommand, m_pData,m_btChecksum,m_btReceivedChecksum);
			}


			m_nState = NP_STATE_SOM;
		break;

		///////////////////////////////////////////////////////////////////////
		default : m_nState = NP_STATE_SOM;
	}
}

///////////////////////////////////////////////////////////////////////////////
// Process NMEA sentence - Use the NMEA address (*pCommand) and call the
// appropriate sentense data prossor.
///////////////////////////////////////////////////////////////////////////////
BOOL CNMEAParser::ProcessCommand(BYTE *pCommand, BYTE *pData)
{
	//
	// GPGGA
	//
	if( strcmp((char *)pCommand, "GPGGA") == NULL )
	{
		ProcessGPGGA(pData);
	}

	//
	// GPGSA
	//
	else if( strcmp((char *)pCommand, "GPGSA") == NULL )
	{
		ProcessGPGSA(pData);
	}

	//
	// GPGSV
	//
	else if( strcmp((char *)pCommand, "GPGSV") == NULL )
	{
		ProcessGPGSV(pData);
	}

	//
	// GPRMB
	//
	else if( strcmp((char *)pCommand, "GPRMB") == NULL )
	{
		ProcessGPRMB(pData);
	}

	//
	// GPRMC
	//
	else if( strcmp((char *)pCommand, "GPRMC") == NULL )
	{
		ProcessGPRMC(pData);
	}

	//
	// GPZDA
	//
	else if( strcmp((char *)pCommand, "GPZDA") == NULL )
	{
		ProcessGPZDA(pData);
	}
	else {
		m_dwCommandEtc++;
	}

	m_dwCommandCount++;
	return TRUE;
}

///////////////////////////////////////////////////////////////////////////////
// Name:		GetField
//
// Description:	This function will get the specified field in a NMEA string.
//
// Entry:		BYTE *pData -		Pointer to NMEA string
//				BYTE *pField -		pointer to returned field
//				int nfieldNum -		Field offset to get
//				int nMaxFieldLen -	Maximum of bytes pFiled can handle
///////////////////////////////////////////////////////////////////////////////
INT CNMEAParser::GetField(BYTE *pData, BYTE *pField, int nFieldNum, int nMaxFieldLen)
{
	//
	// Validate params
	//

	INT rtn = 0;

	if(pData == NULL || pField == NULL || nMaxFieldLen <= 0)
	{
		return 0;
	}

	//
	// Go to the beginning of the selected field
	//
	int i = 0;
	int nField = 0;
	while(nField != nFieldNum && pData[i])
	{
		if(pData[i] == ',')
		{
			nField++;
		}

		i++;

		if(pData[i] == NULL)
		{
			pField[0] = '\0';
			return 0;
		}
	}

	if(pData[i] == ',' || pData[i] == '*')
	{
		pField[0] = '\0';
		return 0;
	}

	//
	// copy field from pData to Field
	//
	int i2 = 0;

	rtn = i+1;

	while(pData[i] != ',' && pData[i] != '*' && pData[i])
	{
		pField[i2] = pData[i];
		i2++; i++;

		//
		// check if field is too big to fit on passed parameter. If it is,
		// crop returned field to its max length.
		//
		if(i2 >= nMaxFieldLen)
		{
			i2 = nMaxFieldLen-1;
			break;
		}
	}
	pField[i2] = '\0';

	return rtn;
}

///////////////////////////////////////////////////////////////////////////////
// Reset: Reset all NMEA data to start-up default values.
///////////////////////////////////////////////////////////////////////////////
void CNMEAParser::Reset()
{
	int i;

	//
	// GPGGA Data
	//
	m_btGGAHour = 0;					//
	m_btGGAMinute = 0;					//
	m_btGGASecond = 0;					//
	m_dGGALatitude = 0.0;				// < 0 = South, > 0 = North 
	m_dGGALongitude = 0.0;				// < 0 = West, > 0 = East
	m_btGGAGPSQuality = 0;				// 0 = fix not available, 1 = GPS sps mode, 2 = Differential GPS, SPS mode, fix valid, 3 = GPS PPS mode, fix valid
	m_btGGANumOfSatsInUse = 0;			//
	m_dGGAHDOP = 0.0;					//
	m_dGGAAltitude = 0.0;				// Altitude: mean-sea-level (geoid) meters
	m_dwGGACount = 0;					//
	m_nGGAOldVSpeedSeconds = 0;			//
	m_dGGAOldVSpeedAlt = 0.0;			//
	m_dGGAVertSpeed = 0.0;				//

	//
	// GPGSA
	//
	m_btGSAMode = 'M';					// M = manual, A = automatic 2D/3D
	m_btGSAFixMode = 1;					// 1 = fix not available, 2 = 2D, 3 = 3D
	for(i = 0; i < NP_MAX_CHAN; i++)
	{
		m_wGSASatsInSolution[i] = 0;	// ID of sats in solution
	}
	m_dGSAPDOP = 0.0;					//
	m_dGSAHDOP = 0.0;					//
	m_dGSAVDOP = 0.0;					//
	m_dwGSACount = 0;					//

	//
	// GPGSV
	//
	m_btGSVTotalNumOfMsg = 0;			//
	m_wGSVTotalNumSatsInView = 0;		//
	for(i = 0; i < NP_MAX_CHAN; i++)
	{
		m_GSVSatInfo[i].m_wAzimuth = 0;
		m_GSVSatInfo[i].m_wElevation = 0;
		m_GSVSatInfo[i].m_wPRN = 0;
		m_GSVSatInfo[i].m_wSignalQuality = 0;
		m_GSVSatInfo[i].m_bUsedInSolution = FALSE;
	}
	m_dwGSVCount = 0;

	//
	// GPRMB
	//
	m_btRMBDataStatus = 'V';			// A = data valid, V = navigation receiver warning
	m_dRMBCrosstrackError = 0.0;		// nautical miles
	m_btRMBDirectionToSteer = '?';		// L/R
	m_lpszRMBOriginWaypoint[0] = '\0';	// Origin Waypoint ID
	m_lpszRMBDestWaypoint[0] = '\0';	// Destination waypoint ID
	m_dRMBDestLatitude = 0.0;			// destination waypoint latitude
	m_dRMBDestLongitude = 0.0;			// destination waypoint longitude
	m_dRMBRangeToDest = 0.0;			// Range to destination nautical mi
	m_dRMBBearingToDest = 0.0;			// Bearing to destination, degrees true
	m_dRMBDestClosingVelocity = 0.0;	// Destination closing velocity, knots
	m_btRMBArrivalStatus = 'V';			// A = arrival circle entered, V = not entered
	m_dwRMBCount = 0;					//

	//
	// GPRMC
	//
	m_btRMCHour = 0;					//
	m_btRMCMinute = 0;					//
	m_btRMCSecond = 0;					//
	m_btRMCDataValid = 'V';				// A = Data valid, V = navigation rx warning
	m_dRMCLatitude = 0.0;				// current latitude
	m_dRMCLongitude = 0.0;				// current longitude
	m_dRMCGroundSpeed = 0.0;			// speed over ground, knots
	m_dRMCCourse = 0.0;					// course over ground, degrees true
	m_btRMCDay = 1;						//
	m_btRMCMonth = 1;					//
	m_wRMCYear = 2000;					//
	m_dRMCMagVar = 0.0;					// magnitic variation, degrees East(+)/West(-)
	m_dwRMCCount = 0;					//

	//
	// GPZDA
	//
	m_btZDAHour = 0;					//
	m_btZDAMinute = 0;					//
	m_btZDASecond = 0;					//
	m_btZDADay = 1;						// 1 - 31
	m_btZDAMonth = 1;					// 1 - 12
	m_wZDAYear = 2000;					//
	m_btZDALocalZoneHour = 0;			// 0 to +/- 13
	m_btZDALocalZoneMinute = 0;			// 0 - 59
	m_dwZDACount = 0;					//

	m_bDOPCheck = FALSE;
	m_nInViewAverage = 0.0;
	m_nWorkAverage = 0.0;
	m_nUsedCount = 0;
	m_nGoodSat = 0;
}

///////////////////////////////////////////////////////////////////////////////
// Check to see if supplied satellite ID is used in the GPS solution.
// Retruned:	BOOL -	TRUE if satellate ID is used in solution
//						FALSE if not used in solution.
///////////////////////////////////////////////////////////////////////////////
BOOL CNMEAParser::IsSatUsedInSolution(WORD wSatID)
{
	if(wSatID == 0) return FALSE;
	for(int i = 0; i < 12; i++)
	{
		if(wSatID == m_wGSASatsInSolution[i])
		{
			return TRUE;
		}
	}

	return FALSE;
}


///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
void CNMEAParser::ProcessGPGGA(BYTE *pData)
{
	BYTE pField[MAXFIELD];
	CHAR pBuff[10];

	//
	// Time
	//
	if(GetField(pData, pField, 0, MAXFIELD))
	{
		// Hour
		pBuff[0] = pField[0];
		pBuff[1] = pField[1];
		pBuff[2] = '\0';
		m_btGGAHour = atoi(pBuff);

		// minute
		pBuff[0] = pField[2];
		pBuff[1] = pField[3];
		pBuff[2] = '\0';
		m_btGGAMinute = atoi(pBuff);

		// Second
		pBuff[0] = pField[4];
		pBuff[1] = pField[5];
		pBuff[2] = '\0';
		m_btGGASecond = atoi(pBuff);
	}

	//
	// Latitude
	//
	if(GetField(pData, pField, 1, MAXFIELD))
	{
		m_dGGALatitude = atof((CHAR *)pField+2) / 60.0;
		pField[2] = '\0';
		m_dGGALatitude += atof((CHAR *)pField);

	}
	if(GetField(pData, pField, 2, MAXFIELD))
	{
		if(pField[0] == 'S')
		{
			m_dGGALatitude = -m_dGGALatitude;
		}
	}

	//
	// Longitude
	//
	if(GetField(pData, pField, 3, MAXFIELD))
	{
		m_dGGALongitude = atof((CHAR *)pField+3) / 60.0;
		pField[3] = '\0';
		m_dGGALongitude += atof((CHAR *)pField);
	}
	if(GetField(pData, pField, 4, MAXFIELD))
	{
		if(pField[0] == 'W')
		{
			m_dGGALongitude = -m_dGGALongitude;
		}
	}

	//
	// GPS quality
	//
	if(GetField(pData, pField, 5, MAXFIELD))
	{
		m_btGGAGPSQuality = pField[0] - '0';
	}

	//
	// Satellites in use
	//
	if(GetField(pData, pField, 6, MAXFIELD))
	{
		pBuff[0] = pField[0];
		pBuff[1] = pField[1];
		pBuff[2] = '\0';
		m_btGGANumOfSatsInUse = atoi(pBuff);
	}

	//
	// HDOP
	//
	if(GetField(pData, pField, 7, MAXFIELD))
	{
		m_dGGAHDOP = atof((CHAR *)pField);
	}
	
	//
	// Altitude
	//
	if(GetField(pData, pField, 8, MAXFIELD))
	{
		m_dGGAAltitude = atof((CHAR *)pField);
	}

	//
	// Durive vertical speed (bonus)
	//
	int nSeconds = (int)m_btGGAMinute * 60 + (int)m_btGGASecond;
	if(nSeconds > m_nGGAOldVSpeedSeconds)
	{
		double dDiff = (double)(m_nGGAOldVSpeedSeconds-nSeconds);