nmeastring.cpp

Gpsapi 2.0(标准mobile接口)

/**************************************************************************/
/*                                                                        */
/* Copyright (c) 2000-2008  YULONG Company             　　　　　　　     */
/*                 宇龙计算机通信科技（深圳）有限公司  版权所有 2000-2008 */
/*                                                                        */
/* PROPRIETARY RIGHTS of YULONG Company are involved in the  　　　　　　 */
/* subject matter of this material.  All manufacturing, reproduction, use,*/
/* and sales rights pertaining to this subject matter are governed by the */
/* license agreement.  The recipient of this software implicitly accepts  */ 
/* the terms of the license.                                              */
/* 本软件文档资料是宇龙公司的资产,任何人士阅读和使用本资料必须获得        */
/* 相应的书面授权,承担保密责任和接受相应的法律约束.                       */
/*                                                                        */
/**************************************************************************/
#include "stdafx.h"
#include "nmeastring.h"

#define  NMEA_GPRMC            "$GPRMC"
#define  NMEA_GPGGA            "$GPGGA"
#define  NMEA_GPGSV            "$GPGSV"
#define  NMEA_GPGSA            "$GPGSA"

#define  NMEA_FRAME_FLAGS_LEN   6

#define  NMEA_FRAME_END        "\r\n"

//GPRMC信息格式定义
#define  NMEA_GPRMC_TIME         1
#define  NMEA_GPRMC_STATUS       2
#define  NMEA_GPRMC_LAT          3
#define  NMEA_GPRMC_LONG         5
#define  NMEA_GPRMC_SPEED        7
#define  NMEA_GPRMC_ANGEL        8
#define  NMEA_GPRMC_DATE         9
#define  NMEA_GPRMC_SEG_NUM      12

//GPRMC信息格式定义
#define  NMEA_GPGGA_TIME         1
#define  NMEA_GPGGA_LAT          2
#define  NMEA_GPGGA_LONG         4
#define  NMEA_GPGGA_GPS_STATUS   6
#define  NMEA_GPGGA_STAR_NUM     7
#define  NMEA_GPGGA_HIGH         9
#define  NMEA_GPGGA_SEG_NUM      14

//GPGSV信息格式定义
#define  NMEA_GPGSV_SENTENCE_NUM         1
#define  NMEA_GPGSV_SENTENCE_NO          2
#define  NMEA_GPGSV_STAR_NUM             3

#define  NMEA_GPGSV_STAR_NO              1
#define  NMEA_GPGSV_STAR_ANGLE           2
#define  NMEA_GPGSV_STAR_ANGLE2          3
#define  NMEA_GPGSV_STAR_SAR             0

#define  NMEA_GPGSV_SEG_NUM              8

//GPGSA信息格式定义
#define  NMEA_GPGSA_MODE         1
#define  NMEA_GPGSA_TYPE         2
#define  NMEA_GPGSA_PRN          3
#define  NMEA_GPGSA_PDOP         15
#define  NMEA_GPGSA_HDOP         16
#define  NMEA_GPGSA_VDOP         17
#define  NMEA_GPGSA_SEG_NUM      17


#define  MAX_TIMEOUT       60
#define  MAX_SPEED        60

#define PREDICT_COUNT      10

#define PI    3.1415926535897932

NmeaString::NmeaString()
{
	m_pBuffer=NULL;
	m_size=0;
	m_lengh=0;

	memset(m_gpsData,0,GPSDATA_ARRAY_LEN*sizeof(GPSData));	

	m_pointer=-1;

	m_pos=-1;

	m_iStarDataLen=0;

	//m_iStarNum=0;	      
	memset(&m_gTime,0,sizeof(SYSTEMTIME));	
	m_iStarNumCon=0;
	
	m_bLogGpsData = 0;
	m_bReadFromFile = 0;

	InitKalman();
	
}

NmeaString::~NmeaString()
{
	if(m_pBuffer)
	{
		free(m_pBuffer);
	}
}

void NmeaString::FreeBuf()
{
	if(m_pBuffer)
	{
		free(m_pBuffer);
	}

	m_pBuffer=NULL;
	m_size=0;
	m_lengh=0;

	memset(m_gpsData,0,GPSDATA_ARRAY_LEN*sizeof(GPSData));	

	m_pointer=-1;

	m_pos=-1;

	m_iStarDataLen=0;

	//m_iStarNum=0;	      
	memset(&m_gTime,0,sizeof(SYSTEMTIME));	
	m_iStarNumCon=0;
	
//	m_bLogGpsData = 0;
//	m_bReadFromFile = 0;

	InitKalman();
}
char* NmeaString::StringTok(char* str, char seps)
{
	static char* p=NULL;
	int tmp;
	static int i=0;
	static int len=0;
	

	if(str)
	{
		i=0;
		len=strlen(str);
		p=str;
	}
	else
	{
		if(i>=len) return NULL;

		i++;
	}	

	tmp=i;	

	while(p[i] && p[i]!=seps) i++;	

	p[i]=0;
	return p+tmp;
}

double NmeaString::ComputeKGain()
{
	double dKGain=0.3;

	dKGain+=(m_gpsData[m_pointer].iStarNum-3)*0.1+m_gpsData[m_pointer].dSpeed/10*0.1;

	dKGain=dKGain>1?1:dKGain;

	return dKGain;
}

// 对GPS数据过滤处理，
void NmeaString::FiltData()
{
	int seconds;
	double len;
	double dltX,dltY;	
	GPSData m_state0;
	static int iCount=0;

#ifdef _COMPLEX_KALMAN_FILTER_
	double m_dData[2],*m_pData;
#endif
	if(m_pointer<0) 
	{
		return ;
	}

	if(m_bFirst && m_gpsData[m_pointer].iQuality)
	{
		if (m_gpsData[m_pointer].dLong < 262800 || m_gpsData[m_pointer].dLat < 57600 || m_gpsData[m_pointer].dLong > 493200||m_gpsData[m_pointer].dLat > 198000)
		{
			return;
		}

		m_gpsFiltedData[0]=m_gpsData[m_pointer];
		m_iFilterPointer=0;

		m_bFirst=FALSE;

		return ;
	}

	m_state0=m_gpsFiltedData[m_iFilterPointer];	

	m_iFilterPointer++;
	m_iFilterPointer%=GPSDATA_ARRAY_LEN;

	if(m_bFirst)
	{
		if (m_gpsData[m_pointer].dLong < 262800 || m_gpsData[m_pointer].dLat < 57600 || m_gpsData[m_pointer].dLong > 493200||m_gpsData[m_pointer].dLat > 198000)
		{
			return;
		}
		m_iFilterPointer=0;
		m_gpsFiltedData[m_iFilterPointer]=m_gpsData[m_pointer];	
		
		return ;
	}

	seconds=((m_gpsData[m_pointer].gTime.wHour-m_state0.gTime.wHour+24)%24)*3600+(m_gpsData[m_pointer].gTime.wMinute-m_state0.gTime.wMinute)*60+(m_gpsData[m_pointer].gTime.wSecond-m_state0.gTime.wSecond);

	if(!seconds)
	{
		m_gpsFiltedData[m_iFilterPointer]=m_gpsFiltedData[(m_iFilterPointer-1+GPSDATA_ARRAY_LEN)%GPSDATA_ARRAY_LEN];	
		return;
	}

	len=m_state0.dSpeed*seconds*1852.0/3600;	
	

	dltY=len*cos(m_state0.dAngle*2*PI/360);
	dltX=len*sin(m_state0.dAngle*2*PI/360);

	m_gpsFiltedData[m_iFilterPointer]       = m_gpsData[m_pointer];
	m_gpsFiltedData[m_iFilterPointer].dLat  = m_state0.dLat+dltY/24;
	m_gpsFiltedData[m_iFilterPointer].dLong = m_state0.dLong+dltX/24;	

	if(!m_gpsData[m_pointer].iQuality || Distance(m_gpsFiltedData[m_iFilterPointer].dLong*24,m_gpsFiltedData[m_iFilterPointer].dLat*24,m_gpsData[m_pointer].dLong*24,m_gpsData[m_pointer].dLat*24)/seconds>MAX_SPEED)
	{
		//超出范围处理	
		//iCount++;
		//if(iCount>PREDICT_COUNT)
		//{
			//m_iFilterPointer--;
			//m_iFilterPointer+=GPSDATA_ARRAY_LEN;
			//m_iFilterPointer%=GPSDATA_ARRAY_LEN;
			//m_gpsFiltedData[m_iFilterPointer].gTime=m_gpsData[m_pointer].gTime;
		//}
		if (m_gpsData[m_pointer].dSpeed <= 0)
		{
			m_gpsFiltedData[m_iFilterPointer] = m_state0;
			m_gpsFiltedData[m_iFilterPointer].gTime=m_gpsData[m_pointer].gTime;
		}
		else
		    m_gpsFiltedData[m_iFilterPointer].gTime=m_gpsData[m_pointer].gTime;
		return ;
	}

	if (m_gpsData[m_pointer].dSpeed == 0)
	{
		m_gpsFiltedData[m_iFilterPointer] = m_state0;
		m_gpsFiltedData[m_iFilterPointer].gTime = m_gpsData[m_pointer].gTime;
		return;
	}
	//iCount=0;
	if(seconds>MAX_TIMEOUT || seconds<0)
	{
		//超时处理
		if (seconds<0)
		{
			m_gpsFiltedData[m_iFilterPointer]=m_state0;
			m_gpsFiltedData[m_iFilterPointer].gTime=m_gpsData[m_pointer].gTime;
			return;
		}
		else	
		    m_gpsFiltedData[m_iFilterPointer]=m_gpsData[m_pointer];
		m_bFirst = TRUE;
		return ;
	}

	//m_gpsFiltedData[m_iFilterPointer]=m_gpsData[m_pointer];


	if(m_gpsData[m_pointer].dSpeed*1.852<=8)
	{
		m_kgain=0.5;
	}
	else
		m_kgain=1;

	m_gpsFiltedData[m_iFilterPointer].dLat=(1-m_kgain)*m_gpsFiltedData[m_iFilterPointer].dLat+m_kgain*m_gpsData[m_pointer].dLat;
	m_gpsFiltedData[m_iFilterPointer].dLong=(1-m_kgain)*m_gpsFiltedData[m_iFilterPointer].dLong+m_kgain*m_gpsData[m_pointer].dLong;
	m_gpsFiltedData[m_iFilterPointer].gTime=m_gpsData[m_pointer].gTime;

#ifdef  _COMPLEX_KALMAN_FILTER_		
	m_dData[0]=m_gpsFiltedData[m_pointer].dLat;
	m_dData[1]=m_gpsFiltedData[m_pointer].dLong;		

	m_pData = kalman.GetKalmanValue(m_dData);

	m_gpsFiltedData[m_iFilterPointer].dLat=m_pData[0];
	m_gpsFiltedData[m_iFilterPointer].dLong=m_pData[1];
#endif
}

GPSData* NmeaString::GetFiltedData()
{	
	if(m_iFilterPointer<0) 
		return NULL;

	return m_gpsFiltedData+m_iFilterPointer;
}

void NmeaString::InitKalman()
{
	m_kgain=0.995;

	m_bFirst=TRUE;

	m_iFilterPointer=-1;
#ifdef _COMPLEX_KALMAN_FILTER_
	kalman.InitKalman();
#endif
}

GPSData* NmeaString::GetGPSData()
{
	if(m_pointer<0) 
		return NULL;

	return m_gpsData+m_pointer;
}

GPSData* NmeaString::GetLastGPSData()
{
	if(m_pointer<0) 
		return NULL;

	return m_gpsData+((m_pointer-1+GPSDATA_ARRAY_LEN)%GPSDATA_ARRAY_LEN);
}

//#ifdef READ_DATA_FROM_FILE
void NmeaString::SetTestMessage(HWND hWnd,DWORD message)
{
	m_message=message;
	m_hWnd=hWnd;	
}
//#endif

int NmeaString::ParserRMC(int istart)
{
	int i,res,tmp,m_iQuality;
	char *p,*token ;
	double		dLong=0; // 经度 单位为秒
	double		dLat=0; // 纬度	
	double		dSpeed=0; // 速度 公里/小时
	double      dAngle=0;	
	char str[10];

	i=istart+1;
	while(m_pBuffer[i]!='\r' && m_pBuffer[i+1]!='\n' && m_pBuffer[i]!='$' && i<m_lengh) i++;

	if(i>=m_lengh) return -1;	

	res=i-istart+2;
	m_pBuffer[res-1+istart]=0;

	if(m_pBuffer[i]=='$') return res;

	//验证数据有效性
	tmp=0;
	i=istart;
	while(m_pBuffer[i])
	{
		if(m_pBuffer[i]==',') tmp++;

		i++;
	}
	if(tmp!=12) return res;

	//解析卫星数据
	i=0;
	p=m_pBuffer+istart;
	token=StringTok(p,',');	
	while(token && i<NMEA_GPRMC_SEG_NUM)
	{
		switch(i)
		{
		case NMEA_GPRMC_TIME:
			{
				if(strlen(token)>=6)
				{
					memset(str,0,5);
					memcpy(str,token,2);
					m_gTime.wHour=atoi(str)+8;

					memcpy(str,token+2,2);
					m_gTime.wMinute=atoi(str);

					memcpy(str,token+4,2);
					m_gTime.wSecond=atoi(str);
				}
			}
			break;
		case NMEA_GPRMC_STATUS:
			{
				if(!memcmp(token,"A",1))
					m_iQuality=1;
				else
					m_iQuality=0;
			}
			break;
		case NMEA_GPRMC_LAT:
			dLat=GetDecimalDegrees(Str2Double(token));
			break;
		case NMEA_GPRMC_LONG:
			dLong=GetDecimalDegrees(Str2Double(token));
			break;
		case NMEA_GPRMC_SPEED:
			dSpeed=Str2Double(token);
			break;	
		case NMEA_GPRMC_ANGEL:
			dAngle=Str2Double(token);
			break;
		case NMEA_GPRMC_DATE:
			{
				if(strlen(token)>=6)
				{
					memset(str,0,5);
					memcpy(str,token,2);
				//	m_gTime.iDay=atoi(str);
					m_gTime.wDay = atoi(str);

					memcpy(str,token+2,2);
				//	m_gTime.iMonth=atoi(str);
					m_gTime.wMonth=atoi(str);

					memcpy(str,token+4,2);
				//	m_gTime.iYear=atoi(str);
					m_gTime.wYear=atoi(str);
				}
			}
			break;
		}
		token=StringTok(NULL,',');
		i++;
	}	
	
	m_pointer++;
	m_pointer%=GPSDATA_ARRAY_LEN;

	//if(m_pointer < GPSDATA_ARRAY_LEN)
	{
		m_pointer = 0;
		memset(m_gpsData+m_pointer,0,sizeof(GPSData));
		
		m_gpsData[m_pointer].dLat=dLat;
		m_gpsData[m_pointer].dLong=dLong;
		m_gpsData[m_pointer].dSpeed=dSpeed;	
		m_gpsData[m_pointer].dAngle=dAngle;	

		m_gpsData[m_pointer].gTime=m_gTime;
		m_gpsData[m_pointer].iQuality=m_iQuality;
		m_gpsData[m_pointer].iStarNum=m_iStarNum;

	}
		

//	FiltData();
	return res;
}

double NmeaString::Str2Double(char* str)
{
#if 1
	double result=0;

	int intr,deci;
	int i=0,len;