NMEA0183.c

#include "NMEA0183.h"

static short int from_hex(char a);
static unsigned char nmea_term_complete(gps_nmea* pnmea, gps_data* pdata);
static int sring_to_int(const char* pstr, unsigned char* b);
static int int_pow(int value, unsigned int count);
static double double_pow(double value, unsigned int count);
static unsigned char float_to_string(double value, char* pdest, unsigned int intgr, unsigned int dec);
static unsigned char int_to_string(int value, char* pdest, unsigned int intgr) ;
static double string_to_float(const char* pstr, unsigned char* b); 
static int sring_to_int(const char* pstr, unsigned char* b); 
static int make_date_time(gps_data* pdata, unsigned int date, unsigned int time);

/* GPS数据解析 */ 
unsigned char nmea_decode(NMEA0183* nmea, char c)
{
    unsigned char valid_sentence = 0;
    
	if((void*)0==nmea) return 0;

    switch (c) 
	{
	    case ',':  /* 字段域分割符号 */ 
	        nmea->gpsParse.parity ^= c;
	        /* no break */
	    case '\r':
	    case '\n':
	    case '*':
	        if (nmea->gpsParse.term_offset < sizeof(nmea->gpsParse.term)) 
			{
	            nmea->gpsParse.term[nmea->gpsParse.term_offset] = 0;
	            valid_sentence = nmea_term_complete(&nmea->gpsParse, &nmea->gpsData);
	        }
	        ++nmea->gpsParse.term_number;
	        nmea->gpsParse.term_offset = 0;
	        nmea->gpsParse.is_checksum_term = (c == '*');   /* 后面两个字节为校验字段 */ 
	        return valid_sentence;
	
	    case '$': /* 字段信息开始 */ 
	        nmea->gpsParse.term_number = nmea->gpsParse.term_offset = 0;
	        nmea->gpsParse.parity = 0;
	        nmea->gpsParse.sentence_type = GPS_SENTENCE_OTHER;
	        nmea->gpsParse.is_checksum_term = 0;
	        nmea->gpsParse.gps_data_good = 0;
	        return valid_sentence;
    }

    /* 保存字段域数据 */
    if (nmea->gpsParse.term_offset < sizeof(nmea->gpsParse.term) - 1)
        nmea->gpsParse.term[nmea->gpsParse.term_offset++] = c;
    if (!nmea->gpsParse.is_checksum_term)
        nmea->gpsParse.parity ^= c;

    return valid_sentence;
}

/* 单个十六进制字符转换为十进制 */
static short int from_hex(char a)
{
    if (a >= 'A' && a <= 'F')
        return a - 'A' + 10;
    else if (a >= 'a' && a <= 'f')
        return a - 'a' + 10;
    else
        return a - '0';
}

/* 处理单个字段域数据 */
static unsigned char nmea_term_complete(gps_nmea* pnmea, gps_data* pdata)
{
	if((void*)0==pnmea || (void*)0==pdata) return 0;
    // handle the last term in a message
    if (pnmea->is_checksum_term)
	{
        unsigned char checksum = 16 * from_hex(pnmea->term[0]) + from_hex(pnmea->term[1]);
        
        /* 检验信息段是否完整无错，如果校验通过，则将解析得到的数据保存，并提供给用户调用*/
        if (checksum == pnmea->parity) 
		{
            if (pnmea->gps_data_good) 
			{
                unsigned int now = current_time_ms();
                
                switch (pnmea->sentence_type) 
				{
	                case GPS_SENTENCE_RMC:
	                    pnmea->last_RMC_ms = now;
	                    //time                        = _new_time;
	                    //date                        = _new_date;
	                    pdata->location.lat     = pnmea->new_latitude;
	                    pdata->location.lng     = pnmea->new_longitude;
	                    pdata->ground_speed     = pnmea->new_speed;
	                    pdata->ground_course    = pnmea->new_course;
						make_date_time(pdata, pnmea->new_date, pnmea->new_time);
	                    pdata->last_gps_time_ms = now;
	                    
	                    break;
	                case GPS_SENTENCE_GGA:
	                    pnmea->last_GGA_ms = now;
	                    pdata->location.alt  = pnmea->new_altitude;
	                    pdata->location.lat  = pnmea->new_latitude;
	                    pdata->location.lng  = pnmea->new_longitude;
	                    pdata->num_sats      = pnmea->new_satellite_count;
	                    pdata->hdop          = pnmea->new_hdop;

	                    switch(pnmea->new_quality_indicator) {
							case 0: // Fix not available or invalid
								pdata->status = NO_FIX;
								break;
							case 1: // GPS SPS Mode, fix valid
								pdata->status = GPS_OK_FIX_3D;
								break;
							case 2: // Differential GPS, SPS Mode, fix valid
								pdata->status = GPS_OK_FIX_3D_DGPS;
								break;
							case 3: // GPS PPS Mode, fix valid
								pdata->status = GPS_OK_FIX_3D;
								break;
							case 4: // Real Time Kinematic. System used in RTK mode with fixed integers
								pdata->status = GPS_OK_FIX_3D_RTK_FIXED;
								break;
							case 5: // Float RTK. Satellite system used in RTK mode, floating integers
								pdata->status = GPS_OK_FIX_3D_RTK_FLOAT;
								break;
							case 6: // Estimated (dead reckoning) Mode
								pdata->status = NO_FIX;
								break;
							default://to maintain compatibility with MAV_GPS_INPUT and others
								pdata->status = GPS_OK_FIX_3D;
								break;
						}
	                    break;
	                case GPS_SENTENCE_VTG:
	                    pnmea->last_VTG_ms = now;
	                    pdata->ground_speed  = pnmea->new_speed;
	                    pdata->ground_course = pnmea->new_course;

	                    // VTG has no fix indicator, can't change fix status
	                    break;
	                case GPS_SENTENCE_HDT:
						 pnmea->last_HDT_ms = now;
//						 pdata->gps_yaw = wrap_360(pnmea->new_gps_yaw*0.01f);
						 pdata->have_gps_yaw = 1;
						 break;
                }
            } else {
                switch (pnmea->sentence_type) {
                case GPS_SENTENCE_RMC:
                case GPS_SENTENCE_GGA:
                    // Only these sentences give us information about
                    // fix status.
                    pdata->status = NO_FIX;
                }
            }
            // see if we got a good message
            //return _have_new_message();
            return 1;
        }
        // we got a bad message, ignore it
        return 0;
    }

    // the first term determines the sentence type
    if (pnmea->term_number == 0) {
        /*
          The first two letters of the NMEA term are the talker
          ID. The most common is 'GP' but there are a bunch of others
          that are valid. We accept any two characters here.
         */
        if (pnmea->term[0] < 'A' || pnmea->term[0] > 'Z' ||
            pnmea->term[1] < 'A' || pnmea->term[1] > 'Z') {
            pnmea->sentence_type = GPS_SENTENCE_OTHER;
            return 0;
        }
        const char *term_type = &pnmea->term[2];
        if (strcmp(term_type, "RMC") == 0) {
            pnmea->sentence_type = GPS_SENTENCE_RMC;
        } else if (strcmp(term_type, "GGA") == 0) {;
            pnmea->sentence_type = GPS_SENTENCE_GGA;
        }else if (strcmp(term_type, "HDT") == 0) {
            pnmea->sentence_type = GPS_SENTENCE_HDT;
            // HDT doesn't have a data qualifier
            pnmea->gps_data_good = 1;
        }else if (strcmp(term_type, "VTG") == 0) {
            pnmea->sentence_type = GPS_SENTENCE_VTG;
            // VTG may not contain a data qualifier, presume the solution is good
            // unless it tells us otherwise.
            pnmea->gps_data_good = 1;
        } else {
            pnmea->sentence_type = GPS_SENTENCE_OTHER;
        }
        return 0;
    }

    // 32 = RMC, 64 = GGA, 96 = VTG
    if (pnmea->sentence_type != GPS_SENTENCE_OTHER && pnmea->term[0]) {
        switch (pnmea->sentence_type + pnmea->term_number) {
        // operational status
        //
        case GPS_SENTENCE_RMC + 2: // validity (RMC)
            pnmea->gps_data_good = pnmea->term[0] == 'A';
            break;
        case GPS_SENTENCE_GGA + 6: // Fix data (GGA)
            pnmea->gps_data_good = pnmea->term[0] > '0';
            pnmea->new_quality_indicator = pnmea->term[0] - '0';
            break;
        case GPS_SENTENCE_VTG + 9: // validity (VTG) (we may not see this field)
            pnmea->gps_data_good = pnmea->term[0] != 'N';
            break;
        case GPS_SENTENCE_GGA + 7: // satellite count (GGA)
            pnmea->new_satellite_count = (unsigned char)sring_to_int((const char*)pnmea->term, (void*)0);
            break;
        case GPS_SENTENCE_GGA + 8: // HDOP (GGA)
            pnmea->new_hdop = (unsigned short)(string_to_float((const char*)pnmea->term, (void*)0)*100);
            break;

        // time and date
        //
        case GPS_SENTENCE_RMC + 1: // Time (RMC)
        case GPS_SENTENCE_GGA + 1: // Time (GGA)
            pnmea->new_time = (unsigned int)(string_to_float((const char*)pnmea->term, (void*)0)*1000);  /* ms */ 
            break;
        case GPS_SENTENCE_RMC + 9: // Date (GPRMC)
            pnmea->new_date = (unsigned int)sring_to_int((const char*)pnmea->term, (void*)0);
            break;

        // location
        //
        case GPS_SENTENCE_RMC + 3: // Latitude
        case GPS_SENTENCE_GGA + 2:
            pnmea->new_latitude = string_to_float((const char*)pnmea->term, (void*)0)/100.0;   //dd.mmmm
			pnmea->new_latitude = (float)( ((unsigned int)pnmea->new_latitude) + (pnmea->new_latitude-((unsigned int)pnmea->new_latitude))/0.6 );  //deg
            break;
        case GPS_SENTENCE_RMC + 4: // N/S
        case GPS_SENTENCE_GGA + 3:
            if (pnmea->term[0] == 'S')
                pnmea->new_latitude = -pnmea->new_latitude;
            break;
        case GPS_SENTENCE_RMC + 5: // Longitude
        case GPS_SENTENCE_GGA + 4:
            pnmea->new_longitude = string_to_float((const char*)pnmea->term, (void*)0)/100.0;  //dd.mmmmmm   
			pnmea->new_longitude = (float)(( (unsigned int)pnmea->new_longitude) + (pnmea->new_longitude-((unsigned int)pnmea->new_longitude))/0.6);  //deg
            break;
        case GPS_SENTENCE_RMC + 6: // E/W
        case GPS_SENTENCE_GGA + 5:
            if (pnmea->term[0] == 'W')
                pnmea->new_longitude = -pnmea->new_longitude;
            break;
        case GPS_SENTENCE_GGA + 9: // Altitude (GPGGA)
            pnmea->new_altitude = (int)(string_to_float((const char*)pnmea->term, (void*)0)*100);  /* cm */ 
            break;

        // course and speed
        //
        //case GPS_SENTENCE_RMC + 7: // Speed (GPRMC)
        //case GPS_SENTENCE_VTG + 5: // Speed (VTG)
        //    pnmea->new_speed = (_parse_decimal_100(pnmea->term) * 514) / 1000;       // knots-> m/sec, approximiates * 0.514
        //    break;
        
        case GPS_SENTENCE_VTG + 7: // Speed (VTG)
        	pnmea->new_speed = string_to_float((const char*)pnmea->term, (void*)0);       //km/h
           break;
        
        case GPS_SENTENCE_HDT + 1: // Course (HDT)
			 pnmea->new_gps_yaw = string_to_float((const char*)pnmea->term, (void*)0);
			 break;
        case GPS_SENTENCE_RMC + 8: // Course (GPRMC)
        case GPS_SENTENCE_VTG + 1: // Course (VTG)
            pnmea->new_course = string_to_float((const char*)pnmea->term, (void*)0);
            break;
        }
    }

    return 0;
}

/* 多次方 */
static int int_pow(int value, unsigned int count)
{
	int v = 1;
	
	while(count--)
		v = v*value;
	return v;
} 

static double double_pow(double value, unsigned int count)
{
	double v = 1;
	
	while(count--)
		v = v*value;
	return v;
}

/* 检查字符串是否为数字字符串，是返回1，否则返回0 */ 
static unsigned char string_check_digital(const char* pc)
{
	const char* p;
	if((void*)0==pc || '\0'==*pc) return 0;	
	
	p = pc;
	while('\0'!=*p)
	{
		if(!IS_DIGITAL(*p)) return 0;
		++p;
	}
	return 1;	
} 

/* 数字字符串处理函数,识别符号并且不检查溢出
*  如果是小数，则只返回整数部分,如果转换出错，返回0 
*  传入参数： 原始字符串， 转换成功标志 0-错误  1-正确 
*/
static int sring_to_int(const char* pstr, unsigned char* b)
{
	char nega = 1;    /* 负数为1， 正数为1 */ 
	const char* psrc = (void*)0;
	int value = 0;
	
	if( (void*)0==pstr ) 
	{
		*b = 0; 
		return 0;
	}
	
	if( !string_check_digital(pstr) ) 
	{
		*b = 0; 
		return 0;
	}
	
	psrc = pstr;
	if('-'==*psrc)  /* 符号位 */ 
	{
		nega = -1;
		++psrc;
	}	
	
	while('\0'!=*psrc)
	{
		if('.'==*psrc || '-'==*psrc ) return value;
		value = 10*value + CHAR_TO_DIGITAL(*psrc);	
		++psrc;
	} 
	if( -1==nega )
		value = -value;
		
	return value;
} 

/* 字符串转换成小数 */
static double string_to_float(const char* pstr, unsigned char* b) 
{
	char nega = 1;    /* 负数为1， 正数为1 */ 
	const char* psrc = (void*)0;
	double value = 0.0;
	double lvalue = 0.0; /* 小数点后面数字 */ 
	unsigned char dotflag = 0;  /* 小数点标记  1为之前出现过小数点 */ 
	unsigned char j = 0;
	
	if( (void*)0==pstr ) 
	{
		*b = 0; 
		return 0;
	}
	
	psrc = pstr;
	if( !string_check_digital(psrc) ) 
	{
		*b = 0; 
		return 0;
	}
	
	if('-'==*psrc)  /* 符号位 */ 
	{
		nega = -1;
		++psrc;
	}	
	
	while('\0'!=*psrc)
	{
		if('-'==*psrc ) return value;
		if('.'==*psrc ) 
		{
			dotflag = 1;
			++psrc;
			continue;
		}
		
		if(!dotflag) 
		{
			value = 10*value + CHAR_TO_DIGITAL(*psrc);	
		}else
		{
			++j;
			lvalue = lvalue + CHAR_TO_DIGITAL(*psrc)*double_pow(0.1, j);
		}	
		++psrc;
	} 
	if( -1==nega )
		value = -(value+lvalue);
	else value = value+lvalue;
		
	return value;
}

/* 浮点数转换为字符串，包括整数转换为字符串 
*  intgr指定整数位个数，dec指定小数位个数 
*  自动去除前面的0，小数点后面的0不会舍去 
*/ 
static unsigned char float_to_string(double value, char* pdest, unsigned int intgr, unsigned int dec)
{
	char* pstr = (void*)0;
	double fvalue = 0.0;
	char c = 0;
	unsigned int tvalue = 0;
	unsigned char zeroflag = 0;
	unsigned int tm = 0;
	
	if( (void*)0==pdest || 0==intgr ) return 0;
	
	if(dec>9) dec = 9;
	
	if(1==intgr) zeroflag = 1;
	
	pstr = pdest;
	if(value<-0.000000000000000001)
	{
		*pstr = '-';
		++pstr;
		value = -value;
	}
	
	tvalue = (int)value%int_pow(10,intgr);
	while(intgr)
	{
		c = DIGITAL_TO_CHAR(tvalue/int_pow(10,intgr-1));
		
		if( !zeroflag  && '0'==c )
		{
			tvalue = tvalue%int_pow(10,intgr-1);
			--intgr;
			continue;
		}
		
		zeroflag = 1;
		
		*pstr = c;
		tvalue = tvalue%int_pow(10,intgr-1);
		--intgr;
		++pstr;
	}
	
	if( !zeroflag ) *pstr++ = '0';
		
		
	/* 如果小数位数为0，则返回整数部分 */
	if(0==dec)
	{
		*pstr = '\0';
		return 1;		
	}
	
	*pstr++ = '.';	
	tm = (unsigned int)int_pow(10,dec);
	tvalue = (unsigned int)( ( (unsigned int)((value-(unsigned int)value)*tm) )%tm );
	while(dec)
	{
		tm = int_pow(10,dec-1);
		*pstr = DIGITAL_TO_CHAR(tvalue/tm);
		tvalue = tvalue%tm;
		--dec;
		++pstr;
	}
	*pstr = '\0';
	return 1;	
} 


/* 整数转换成字符串，内部其实调用了 float_to_string */
static unsigned char int_to_string(int value, char* pdest, unsigned int intgr) 
{
	return float_to_string((double)value, pdest, intgr,0);
}

/* 时间处理  传入参数为日月年和毫秒*/ 
static int make_date_time(gps_data* pdata, unsigned int date, unsigned int time)
{
	
	unsigned char year, mon, day, hour, min, sec;
    unsigned short int msec;
    unsigned int v,ret;
    char rmon;
    
    if((void*)0==pdata) return 0;

    year = date % 100;
    mon  = (date / 100) % 100;
    day  = date / 10000;
    
    pdata->date_time.year = year;
    pdata->date_time.month = mon;
    pdata->date_time.day = day;

    v = time;
    msec = v % 1000; v /= 1000;
    sec  = v % 100; v /= 100;
    min  = v % 100; v /= 100;
    hour = v % 100; v /= 100;

    rmon = mon - 2;
    if (0 >= rmon) {    
        rmon += 12;
        year -= 1;
    }

    // get time in seconds since unix epoch
    ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day;
    ret += year*365 + 10501;
    ret = ret*24 + hour;
    ret = ret*60 + min;
    ret = ret*60 + sec;

    // convert to time since GPS epoch
    ret -= 272764785UL;

    // get GPS week and time
    pdata->date_time.week = ret / AP_SEC_PER_WEEK;
    
    return 1;
}