RTL271_1/app/nmea.c

#include "nmea.h"

#ifndef NULL
	#define NULL '\0'
#endif

NMEA_DEF nmeaInfo={
	.isGpsValid=0
};
/*
str串以\r\n或\r或\n结尾，以此为标志截断
*/
void turncateStr(char *str){
	int i;
	for(i=0;i<strlen(str);i++){
		if(str[i]=='\r' || str[i]=='\n'){
			str[i]=0;
			return;
		}
	}
}

/*
nmea为原始数据，所有字段串在一起
这里预处理一下，提取target字段
*/
char * preNmea(char *nmea, char *target){
	int i;
	char *ret;

	if(nmea==NULL) return NULL;
	ret=strstr(nmea,target);
	if(NULL!=ret) turncateStr(ret);
	return ret;
}

//#define GPS_TYPE_ZKW
#define GPS_TYPE_UBLOX
#define GPRMC_UTC1_TOKEN_INDEX						0
#define GPRMC_STATUS_TOKEN_INDEX					1
#define GPRMC_LATITUE_TOKEN_INDEX					2
#define GPRMC_NORTH_TOKEN_INDEX						3
#define GPRMC_longitue_TOKEN_INDEX					4
#define GPRMC_EAST_TOKEN_INDEX						5
#define GPRMC_SPEED_TOKEN_INDEX						6
#define GPRMC_ASPECT_TOKEN_INDEX					7
#define GPRMC_UTC2_TOKEN_INDEX						8

/*
解析RMC字段并获取相关值
*/
void nmeaRmc(char *rmc)
{
	unsigned int size=strlen(rmc);
	char tmp_char[16] = {0};
	char  bcd_tmp[8] = {0};
	char *token[16] = {NULL};
	char size_each_token[16] = {0};
    unsigned char hh,hl;
	unsigned char token_index = 0;
	char ch;
	int i = 0;
	int index = 0;
	int dig_bit_num = 0;
	int ptr_each = 0;
	unsigned short speed;
	unsigned short aspect;

	double speed_jie = 0.0;
	
	token[token_index] = rmc;	
	while(index < size )
	{
		if(rmc[index] == ',')
		{
			size_each_token[token_index] = ptr_each;
			if(ptr_each == 0)
				token[token_index] = NULL;
			ptr_each = 0;
			token_index++;
			token[token_index] = rmc+index+1;  //skip this ',' 
			if(*(token[token_index]+1) == '*')
			{
				size_each_token[token_index] = 1;	     //cut of *hh\r\n
			}        
		}
		else
		{
			ptr_each++;    
		}
		index++;
	}
   
	//UTC time:hms    
	//014216 = 01:42:16
#if 0 //»ñÈ¡GPSʱ¼ä 
	if(token[GPRMC_UTC1_TOKEN_INDEX])   
	{ //,040831.00,  Ò»°ã³¤¶ÈΪ9
		if(size_each_token[0]<6 && size_each_token[0]>10){
			//printf("GPS Error1 %d\r\n",size_each_token[0]);
			return;
		}
		strncpy((char*)tmp_char,(char*)token[0],size_each_token[0]);
		translate_digitAscii_to_bcd(tmp_char,6,bcd_tmp);  //??.xx
	  nmeaInfo.hour=bcd_tmp[0];
		nmeaInfo.minu = bcd_tmp[1];
		nmeaInfo.sec = bcd_tmp[2];
	}  	

		//UTC2
	//250219 = 19-02-25
	if(token[GPRMC_UTC2_TOKEN_INDEX])   
	{
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		memset(bcd_tmp , 0 ,sizeof(bcd_tmp));    	   
		if(size_each_token[GPRMC_UTC2_TOKEN_INDEX]!=6){
			//printf("GPS Error2\r\n");
			return;
		}
		strncpy((char*)tmp_char,(char*)token[GPRMC_UTC2_TOKEN_INDEX],size_each_token[GPRMC_UTC2_TOKEN_INDEX]);
		translate_digitAscii_to_bcd(tmp_char,6,bcd_tmp);  
		
		nmeaInfo.day = bcd_tmp[0];
		nmeaInfo.month= bcd_tmp[1];
		nmeaInfo.year = bcd_tmp[2];
	}
#endif
		//Fix status
	if(token[GPRMC_STATUS_TOKEN_INDEX])   
	{
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		if(size_each_token[1]!=1){
			//wlog_error("GpsDataError!");
			return;
		}

		strncpy((char*)tmp_char,(char*)token[1],size_each_token[1]);
		if(*tmp_char == 'A'){
			nmeaInfo.isGpsValid = GPS_ACTIVE;
		}else{
			nmeaInfo.isGpsValid = GPS_INVALID;
			nmeaInfo.Altitude=0;
			nmeaInfo.Latitude=0;
			nmeaInfo.Longitude=0;
			nmeaInfo.Aspect=0;
			nmeaInfo.Speed=0;
			return;
		}
	}

    //latitude ??
	/*=====================================
	UTC time:hms   status       latitue         north?    longitue          east?   ??(?)   ???  UTC time      ???      ??   ??      checksum
	$GPRMC    hhmmss.ss      A/V      ddmm.mmmm   N/S   dddmm.mmmm  E/W      speed             aspect        ddmmyy      000-180            E/W        A/D/E/N     *hh
	For example: $GPRMC,024813.640,A,3158.4608,N,11848.3737,E,010.5,324.7,150706,,,A*50
	*/
	if(token[GPRMC_LATITUE_TOKEN_INDEX])   
	{//ddmm.mmmmm,N/S
		ch=*token[GPRMC_NORTH_TOKEN_INDEX];
		char *p=token[GPRMC_LATITUE_TOKEN_INDEX];
		//ddmm.mmmmm
		int ddmm=atoi(p);
		int mmmmm=atoi((p+5));
		int dd=ddmm/100;
		int mm=ddmm%100;
		//double fmm=(double)mm+(double)mmmmm/100000.0;
		//nmeaInfo.latitude=(double)dd+fmm/60.0;
		nmeaInfo.Latitude=dd*1000000;//¶È ·Å´ó1000000±¶£¬ÕûÊý²¿·Ö
		mm*=1000000;
		mmmmm*=10;
		mm+=mmmmm;//·Ö ·Å´ó1000000±¶
		dd=mm/60;//¶È ·Å´ó1000000±¶  СÊý²¿·Ö
		nmeaInfo.Latitude+=dd;//¶È ·Å´ó1000000±¶
		if(ch=='S'){
			nmeaInfo.South=1;
		}else{
			nmeaInfo.South=0;
		}
		//sprintf(nmeaInfo.vv1,"%d%d.%d\r\n",dd,mm,mmmm);

		
	}
	
	//longitude ??
	if(token[GPRMC_longitue_TOKEN_INDEX])   
	{//dddmm.mmmmm,E/W
#if 0
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		memset(bcd_tmp , 0 ,sizeof(bcd_tmp));    
		strncpy((char*)tmp_char,(char*)token[4],size_each_token[4]);
		for(i = 5; i < 8;i++)
			tmp_char[i] = tmp_char[i+1];
		tmp_char[8] = 0;
		translate_digitAscii_to_bcd(tmp_char,8,bcd_tmp);  //??.xx	
		//east or west    
		if(token[GPRMC_EAST_TOKEN_INDEX])
		{
			memset(tmp_char , 0 ,sizeof(tmp_char));    
			strncpy((char*)tmp_char,(char*)token[5],size_each_token[5]);
			nmeaInfo.EWFlag=*tmp_char;
			nmeaInfo.longitude = (bcd_tmp[3] | (bcd_tmp[2] << 8) |(bcd_tmp[1] << 16) |(bcd_tmp[0] << 24));   
		}
#else
		ch=*token[GPRMC_EAST_TOKEN_INDEX];
		if(ch=='W')nmeaInfo.West=1;
		else nmeaInfo.West=0;
		
		char *p=token[GPRMC_longitue_TOKEN_INDEX];
		int dddmm=atoi(p);
		int mmmmm=atoi((p+6));
		int ddd=dddmm/100;
		int mm=dddmm%100;
		//double fmm=(double)mm+(double)mmmmm/100000.0;
		//nmeaInfo.longitude=(double)ddd+fmm/60.0;
		
		nmeaInfo.Longitude=ddd*1000000;//¶È ·Å´ó1000000±¶  ÕûÊý²¿·Ö
		mm*=1000000;
		mmmmm*=10;
		mm+=mmmmm;//·Ö ·Å´ó1000000±¶
		ddd=mm/60;//¶È ·Å´ó1000000±¶  СÊý²¿·Ö
		nmeaInfo.Longitude+=ddd;
		
#endif
	}
	
	//Speed
	//0.094
	if(token[GPRMC_SPEED_TOKEN_INDEX])   
	{
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		strncpy((char*)tmp_char,(char*)token[GPRMC_SPEED_TOKEN_INDEX],size_each_token[GPRMC_SPEED_TOKEN_INDEX]);
		index = 0;
		while(index < size_each_token[GPRMC_SPEED_TOKEN_INDEX])
		{
			if(tmp_char[index] == '.')break;
				index++;
		}
		dig_bit_num = size_each_token[GPRMC_SPEED_TOKEN_INDEX]-index-1;
		speed_jie = atoi(tmp_char);
		if(dig_bit_num>0)speed_jie += (tmp_char[index+1]-0x30)*0.1;
		//nmeaInfo.Speed=(int)(speed_jie/0.1852);// 1/10 km
		nmeaInfo.Speed=(int)(speed_jie*18.52);// 1/10 km
		//1½ÚÒ²¾ÍÊÇ1.852¹«Àï/Сʱ.
	}

	//Aspect ??
	if(token[GPRMC_ASPECT_TOKEN_INDEX])   
	{
		memset(tmp_char , 0 ,sizeof(tmp_char));    
		if(size_each_token[GPRMC_ASPECT_TOKEN_INDEX]==0){
			nmeaInfo.Aspect=0;
		}else{
			strncpy((char*)tmp_char,(char*)token[GPRMC_ASPECT_TOKEN_INDEX],size_each_token[GPRMC_ASPECT_TOKEN_INDEX]);
			aspect=atoi(tmp_char);
			nmeaInfo.Aspect= aspect;
		}
		
	}else nmeaInfo.Aspect=0;
}

/*
解析GSA字段并获取相关值
*/
void nmeaGsa(char *gsa,int size){
	#if defined GPS_TYPE_UBLOX //---------------------------------UbloxоƬ·½°¸
/*
	UbloxÄ£¿é£º
$GNGSA,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>,<13>,<14>,<15>,<16
<1>ģʽ2:M=ÊÖ¶¯,A=×Ô¶¯
<2>ģʽ1:¶¨Î»ÐÍʽ1=δ¶¨Î»,2=¶þά¶¨Î»,3=Èýά¶¨Î»
<3>µÚ1ÐŵÀÕýÔÚʹÓõÄÎÀÐÇPRNÂë±àºÅ01ÖÁ32(Ç°µ¼Î»Êý²»×ãÔò²¹0,×î¶à¿É½ÓÊÕ12¿ÅÎÀÐÇÐÅÏ¢)
<4>µÚ2ÐŵÀÕýÔÚʹÓõÄÎÀÐÇPRNÂë±àºÅ01ÖÁ32(Ç°µ¼Î»Êý²»×ãÔò²¹0,×î¶à¿É½ÓÊÕ12¿ÅÎÀÐÇÐÅÏ¢)
	...
¶ÔÓÚ±±¶·£¬PRN±àÂë´Ó401¿ªÊ¼...
	¾ÙÀý£º
	$GNGSA,A,3,18,11,31,08,,,,,,,,,3.03,2.13,2.15*19
	ÕýÔÚʹÓÃÎÀÐÇ GPS±àºÅΪ18 11 31 08  ¹²4¿ÅÎÀÐÇ£¬ PDOPÒò×ÓΪ3.03
	$GNGSA,A,3,416,401,406,408,424,428,,,,,,,3.03,2.13,2.15*1E
	ÕýÔÚʹÓÃÎÀÐÇ BD±àºÅΪ416 401 406 408 424 428 ¹²6¿ÅÎÀÐÇ £¬PDOPÒò×ÓΪ3.03

*/
	int flag=0;
	int i,j,k,BDNum,GPSNum,PDOP;
	char ch,ch1,ch2;
	nmeaInfo.GPSNum=0;nmeaInfo.BDNum=0;
	if(gsa[7]!='A' && gsa[0]!='M' )return;
//	if(gsa[9]=='1'){//δ¶¨Î»
//		nmeaInfo.GPSNum=0;
//		nmeaInfo.BDNum=0;
//		nmeaInfo.PDOP=0;
//		return;
//	}
//	if(gsa[9]!='2' && gsa[9]!='3' )return;
	i=11;j=0;k=0;BDNum=0;GPSNum=0;
	while(i<size && j<12){
		ch=gsa[i];
		if(ch==','){
			i++;
			j++;
			if(k==2)GPSNum++;
			else if(k==3)BDNum++;
			k=0;
		}else if(ch>='0' && ch<='9'){
			i++;
			k++;
		}else{
			break;
		}
	}
	PDOP=0;
	if(j==12){
		if(gsa[i]>='0' && gsa[i]<='9' && gsa[i+1]=='.' && gsa[i+2]>='0' &&  gsa[i+2]<='9'){
			PDOP=(gsa[i]-0x30);
			PDOP*=10;
			PDOP+=(gsa[i+2]-0x30);
		}
	}
	if(GPSNum>12)GPSNum=12;
	if(BDNum>12)BDNum=12;
	if(PDOP>63)PDOP=63;
	if(GPSNum>0)nmeaInfo.GPSNum=GPSNum;
	if(BDNum>0)nmeaInfo.BDNum=BDNum;
	nmeaInfo.PDOP=PDOP;
#elif defined GPS_TYPE_ZKW //---------------------------------ÖпÆ΢оƬ·½°¸
/*
	ÖпÆ΢ģ¿é£º
	$GPGSA,A,3,33,14,22,27,31,32,08,35,21,,,,1.6,1.1,1.2*38
	$BDGSA,A,3,01,02,03,04,08,09,13,06,,,,,1.6,1.1,1.2*26
*/
	int flag=0;
	int i,j,num;
	char ch,ch1,ch2;
	if(gsa[7]!='A' && gsa[0]!='M' )return;
	if(gsa[9]=='1'){//δ¶¨Î»
		nmeaInfo.GPSNum=0;
		nmeaInfo.BDNum=0;
		return;
	}
	if(gsa[9]!='2' && gsa[9]!='3' )return;
	i=11;j=0;num=0;
	while(i<size && j<12){
		ch=gsa[i];
		if(ch==','){
			i++;
			j++;
		}else if(ch>='0' && ch<='9'){
			i+=2;
			num++;
		}else{//·Ç·¨
			break;
		}
	}
	PDOP=0;
	if(j==12){
		if(gsa[i]>='0' && gsa[i]<='9' && gsa[i+1]=='.' && gsa[i+2]>='0' &&  buf[i+2]<='9'){
			PDOP=(gsa[i]-0x30);
			PDOP*=10;
			PDOP+=(gsa[i+2]-0x30);
		}
	}
	
	if(GPSNum>12)GPSNum=12;
	if(BDNum>12)BDNum=12;
	if(PDOP>63)PDOP=63;
	if(flag==1){
		nmeaInfo.GPSNum=num;
	}else if(flag==2){
		nmeaInfo.BDNum=num;
	}
	nmeaInfo.PDOP=PDOP;
#endif
}