parse.c

一个很不错的GPS接收机源程序

/**********************************************
*   File: Parse.C
*   Description: 处理GPS数据结构体 
*   Created Date:  2007-10-01
*   Last Modified: 2007-10-01
*   Notes: None
**********************************************/

#include <string.h>
#include <stdio.h>

#include "myformat.h"
#include "parse.h"
#include "context.h"
#include "math.h"
#include "units.h"

#include <assert.h>



/**********************************************
*   Function: nmea_pack_type(const char *buff, int buff_sz)
*   Input Variables: const char *buff, int buff_sz
*   Return Variables: Int
*   Usage:识别缓存区内的GPS数据类型
**********************************************/
int nmea_pack_type(const char *buff, int buff_sz)
{
    static const char *pheads[] = {					// 识别帧类型字符串
        "GPGGA",
        "GPGSA",
        "GPGSV",
        "GPRMC",
        "GPVTG",
    };

    NMEA_ASSERT(buff);		

    if(buff_sz < 5)								// 如果buff长度小于5	
        return GPNON;							// 未知类型
    else if(0 == memcmp(buff, pheads[0], 5))			// 如果和GPGGA相同
        return GPGGA;							//  return
    else if(0 == memcmp(buff, pheads[1], 5))			// 如果和GPGSA相同
        return GPGSA;							//  return
    else if(0 == memcmp(buff, pheads[2], 5))			// 如果和GPGSV相同
        return GPGSV;							//  return
    else if(0 == memcmp(buff, pheads[3], 5))			// 如果和GPRMC相同
        return GPRMC;							//  return
    else if(0 == memcmp(buff, pheads[4], 5))			// 如果和GPVTG相同
        return GPVTG;

    return GPNON;								// 返回未知类型
}



/**********************************************
*   Function: nmea_find_tail(const char *buff, int buff_sz, int *res_crc)
*   Input Variables: const char *buff, int buff_sz, int *res_crc
*   Return Variables: Int
*   Usage:判断帧完整性并作CRC校验
**********************************************/
int nmea_find_tail(const char *buff, int buff_sz, int *res_crc)
{
    static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */;

    const char *end_buff = buff + buff_sz;
    int nread = 0;
    int crc = 0;

    NMEA_ASSERT(buff && res_crc);

    *res_crc = -1;

    for(;buff < end_buff; ++buff, ++nread)				// 循环
    {
        if(('$' == *buff) && nread)					// 帧头
        {
            buff = 0;
            break;
        }
        else if('*' == *buff)						// 开始CRC校验部分
        {
            if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4])
            {
                *res_crc = nmea_atoi(buff + 1, 2, 16);
                nread = buff_sz - (int)(end_buff - (buff + tail_sz));
                if(*res_crc != crc)
                {
                    *res_crc = -1;
                    buff = 0;
                }
            }

            break;
        }
        else if(nread)
            crc ^= (int)*buff;
    }

    if(*res_crc < 0 && buff)
        nread = 0;

    return nread;								// 返回
}



/**********************************************
*   Function: nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack)
*   Input Variables: const char *buff, int buff_sz, nmeaGPGGA *pack
*   Return Variables: Int
*   Usage:解析GPGGA类型帧数据
**********************************************/
int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack)
{
    NMEA_ASSERT(buff && pack);

    memset(pack, 0, sizeof(nmeaGPGGA));

    nmea_trace_buff(buff, buff_sz);
	// 解析GPGGA数据
    if(17 != nmea_scanf(buff, buff_sz,
        "$GPGGA,%2d%2d%2d.%d,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*",
        &(pack->utc_time.hour), &(pack->utc_time.min), &(pack->utc_time.sec), &(pack->utc_time.hsec),
        &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
        &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units),
        &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid)))
    {
        nmea_error("GPGGA parse error!");
        return 0;
    }

    return 1;
}




/**********************************************
*   Function: nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack)
*   Input Variables: const char *buff, int buff_sz, nmeaGPGSA *pack
*   Return Variables: Int
*   Usage:解析GPGSA类型帧数据
**********************************************/
int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack)
{
    NMEA_ASSERT(buff && pack);

    memset(pack, 0, sizeof(nmeaGPGSA));

    nmea_trace_buff(buff, buff_sz);
	// 解析GPGSA数据
    if(17 != nmea_scanf(buff, buff_sz,
        "$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*",
        &(pack->fix_mode), &(pack->fix_type),
        &(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]),
        &(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]),
        &(pack->PDOP), &(pack->HDOP), &(pack->VDOP)))
    {
        nmea_error("GPGSA parse error!");
        return 0;
    }

    return 1;
}


/**********************************************
*   Function: nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack)
*   Input Variables: const char *buff, int buff_sz, nmeaGPGSV *pack
*   Return Variables: Int
*   Usage:解析GPGSV类型帧数据
**********************************************/
int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack)
{
    int nsen, nsat;

    NMEA_ASSERT(buff && pack);

    memset(pack, 0, sizeof(nmeaGPGSV));

    nmea_trace_buff(buff, buff_sz);
	// 解析GPGSV数据
    nsen = nmea_scanf(buff, buff_sz,
        "$GPGSV,%d,%d,%d,"
        "%d,%d,%d,%d,"
        "%d,%d,%d,%d,"
        "%d,%d,%d,%d,"
        "%d,%d,%d,%d*",
        &(pack->pack_count), &(pack->pack_index), &(pack->sat_count),
        &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig),
        &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig),
        &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig),
        &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig));

    nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
    nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
    nsat = nsat * 4 + 3 /* first three sentence`s */;

    if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3))
    {
        nmea_error("GPGSV parse error!");
        return 0;
    }

    return 1;
}


/**********************************************
*   Function: nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack)
*   Input Variables: const char *buff, int buff_sz, nmeaGPRMC *pack
*   Return Variables: Int
*   Usage:解析GPRMC类型帧数据
**********************************************/
int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack)
{
    int nsen;

    NMEA_ASSERT(buff && pack);

    memset(pack, 0, sizeof(nmeaGPRMC));

    nmea_trace_buff(buff, buff_sz);
	// 解析GPRMC类型数据
    nsen = nmea_scanf(buff, buff_sz,
        "$GPRMC,%2d%2d%2d.%d,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*",
        &(pack->utc.hour), &(pack->utc.min), &(pack->utc.sec), &(pack->utc.hsec),
        &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
        &(pack->speed), &(pack->direction),
        &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year),
        &(pack->declination), &(pack->declin_ew), &(pack->mode));

    if(nsen != 16 && nsen != 17)
    {
        nmea_error("GPRMC parse error!");
        return 0;
    }

    if(pack->utc.year < 90)
        pack->utc.year += 100;
    pack->utc.mon -= 1;

    return 1;
}


/**********************************************
*   Function: nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack)
*   Input Variables: const char *buff, int buff_sz, nmeaGPVTG *pack 
*   Return Variables: Int
*   Usage:解析GPVTG类型帧数据
**********************************************/
int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack)
{
    NMEA_ASSERT(buff && pack);

    memset(pack, 0, sizeof(nmeaGPVTG));

    nmea_trace_buff(buff, buff_sz);
	//　解析GPVTG类型数据
    if(8 != nmea_scanf(buff, buff_sz,
        "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*",
        &(pack->dir), &(pack->dir_t),
        &(pack->dec), &(pack->dec_m),
        &(pack->spn), &(pack->spn_n),
        &(pack->spk), &(pack->spk_k)))
    {
        nmea_error("GPVTG parse error!");
        return 0;
    }

    if( pack->dir_t != 'T' ||
        pack->dec_m != 'M' ||
        pack->spn_n != 'N' ||
        pack->spk_k != 'K')
    {
        nmea_error("GPVTG parse error (format error)!");
        return 0;
    }

    return 1;
}



/**********************************************
*   Function: nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info)
*   Input Variables: nmeaGPGGA *pack, nmeaINFO *info 
*   Return Variables: none
*   Usage:从GPGGA结构体中获得定位相关信息
**********************************************/
void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info)
{
    NMEA_ASSERT(pack && info);

    info->utc.hour = pack->utc_time.hour;
    info->utc.min = pack->utc_time.min;
    info->utc.sec = pack->utc_time.sec;
    info->utc.hsec = pack->utc_time.hsec;
    info->sig = pack->sig;
    info->HDOP = pack->HDOP;
    info->elv = pack->elv;
    info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
    info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
    info->smask |= GPGGA;
}



/**********************************************
*   Function: nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info)
*   Input Variables: nmeaGPGSA *pack, nmeaINFO *info
*   Return Variables: none
*   Usage:从GPGSA结构体中获得定位相关信息
**********************************************/
void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info)
{
    int i, j, nuse = 0;

    NMEA_ASSERT(pack && info);

    info->fix = pack->fix_type;
    info->PDOP = pack->PDOP;
    info->HDOP = pack->HDOP;
    info->VDOP = pack->VDOP;

    for(i = 0; i < NMEA_MAXSAT; ++i)
    {
        for(j = 0; j < info->satinfo.inview; ++j)
        {
            if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id)
            {
                info->satinfo.sat[j].in_use = 1;
                nuse++;
            }
        }
    }

    info->satinfo.inuse = nuse;
    info->smask |= GPGSA;
}


/**********************************************
*   Function: nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info)
*   Input Variables: nmeaGPGSV *pack, nmeaINFO *info
*   Return Variables: none
*   Usage:从GPGSV结构体中获得定位相关信息
**********************************************/
void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info)
{
    int isat, isi, nsat;

    NMEA_ASSERT(pack && info);

    if(pack->pack_index > pack->pack_count ||
        pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT)
        return;

    if(pack->pack_index < 1)
        pack->pack_index = 1;

    info->satinfo.inview = pack->sat_count;

    nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
    nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;

    for(isat = 0; isat < nsat; ++isat)
    {
        isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat;
        info->satinfo.sat[isi].id = pack->sat_data[isat].id;
        info->satinfo.sat[isi].elv = pack->sat_data[isat].elv;
        info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth;
        info->satinfo.sat[isi].sig = pack->sat_data[isat].sig;
    }

    info->smask |= GPGSV;
}




/**********************************************
*   Function: nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info)
*   Input Variables: nmeaGPRMC *pack, nmeaINFO *info
*   Return Variables: none
*   Usage:从GPGSV结构体中获得定位相关信息
**********************************************/
void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info)
{
    NMEA_ASSERT(pack && info);

    if('A' == pack->status)
    {
        if(NMEA_SIG_BAD == info->sig)
            info->sig = NMEA_SIG_MID;
        if(NMEA_FIX_BAD == info->fix)
            info->fix = NMEA_FIX_2D;
    }
    else if('V' == pack->status)
    {
        info->sig = NMEA_SIG_BAD;
        info->fix = NMEA_FIX_BAD;
    }

    info->utc = pack->utc;
    info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
    info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
    info->speed = pack->speed * NMEA_TUD_KNOTS;
    info->direction = pack->direction;
    info->smask |= GPRMC;
}




/**********************************************
*   Function: nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info)
*   Input Variables: nmeaGPVTG *pack, nmeaINFO *info
*   Return Variables: none
*   Usage:从GPGSV结构体中获得定位相关信息
**********************************************/
void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info)
{
    NMEA_ASSERT(pack && info);

    info->direction = pack->dir;
    info->declination = pack->dec;
    info->speed = pack->spk;
    info->smask |= GPVTG;
}