GPS(NMEA 0183)数据解析

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

#define _STR(x)    #x
#define STR(x)    _STR(x)

#define ARRAY_SIZE(array)	(sizeof(array)/sizeof(array[0]))

#define NMEA_UNIT_LEN 20
#define NMEA_INFO_LEN 16

#if 0
"$GPRMC,173843,A,3349.896,N,11808.521,W,000.0,360.0,230108,013.4,E*69\r\n",
"$GPGGA,111609.14,5001.27,N,3613.06,E,3,08,0.0,10.2,M,0.0,M,0.0,0000*70\r\n",
"$GPGSV,2,1,08,01,05,005,80,02,05,050,80,03,05,095,80,04,05,140,80*7f\r\n",
"$GPGSV,2,2,08,05,05,185,80,06,05,230,80,07,05,275,80,08,05,320,80*71\r\n",
"$GPGSA,A,3,01,02,03,04,05,06,07,08,00,00,00,00,0.0,0.0,0.0*3a\r\n",
"$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n",
"$GPVTG,217.5,T,208.8,M,000.00,N,000.01,K*4C\r\n"
#endif

char nmea_info[NMEA_UNIT_LEN][NMEA_INFO_LEN] = {0};

void nmea_init(char *info);
void nmea_parse(char *str, char *info);
void nmea_decode(char *info);
void showInfo(char *info);

int main(int argc, char *argv[])
{
	char *gps_str[]= {
		"$GPGGA,111609.14,5001.27,N,3613.06,E,3,08,0.0,10.2,M,0.0,M,0.0,0000*70\r\n",
        "$GPGSA,A,3,01,02,03,04,05,06,07,08,00,00,00,00,0.0,0.0,0.0*3a\r\n",
        "$GPGSV,2,1,08,01,05,005,80,02,05,050,80,03,05,095,80,04,05,140,80*7f\r\n",
        "$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n",
        "$GPVTG,217.5,T,208.8,M,000.00,N,000.01,K*4C\r\n",
        "$GPGLL,4250.5589,S,14718.5084,E,092204.999,A*2D\r\n",
	};
	
	for (int i = 0; i < 6; i ++)
	{
		nmea_init((char *)nmea_info);
		
		nmea_parse(gps_str[i], (char *)nmea_info);
	#if 0
		showInfo((char *)nmea_info);
	#endif
		nmea_decode((char *)nmea_info);
	}

	return 0;
}

void nmea_init(char *info)
{
	for (int i = 0; i < NMEA_UNIT_LEN; i ++)
	{
		for (int j = 0; j < NMEA_INFO_LEN; j ++)
		{
			info[i * NMEA_INFO_LEN + j] = '\0';
		}
	}
}

void nmea_parse(char *str, char *info)
{
	if (NULL == str || NULL == info)
	{
		printf("pointer is NULL!\n");

		return;
	}

	if ('$' != str[0])
	{
		printf("format is error!\n");

		return;
	}

	int i, j, k;

	i = 0;
	j = 0;
	k = 1;

	while (1)
	{
		if (('*' == str[k]) || ('\r' == str[k] && '\n' == str[k + 1]))
		{
			break;
		}

		if (',' == str[k])
		{
			i ++;
			j = 0;
		}
		else
		{
			info[i * NMEA_INFO_LEN + j] = str[k];

			j ++;
		}
		
		k ++;
	}
}

enum
{
	ID_N,	// none
	ID_C,	// char
	ID_D,	// double
	ID_I,	// int
};

#define MAX_ID_LEN      6
#define MAX_FLAG_LEN    19

struct NMEA2GNSS
{
    void (*parse)(char *info);
    unsigned char len;
	char id[MAX_ID_LEN];    // GPGGA/GPGSA/......
	char flag[MAX_FLAG_LEN];// <01>,<02>,...,<19>
};

void gpgga_parse(char *info);
void gpgsa_parse(char *info);
void gpgsv_parse(char *info);
void gprmc_parse(char *info);
void gpvtg_parse(char *info);
void gpgll_parse(char *info);

struct NMEA2GNSS nmea_tbl[] = {
	/*      parse,len,    <00>, <01>, <02>, <03>, <04>, <05>, <06>, <07>, <08>, <09>, <10>, <11>, <12>, <13>, <14>, <15>, <16>, <17>, <18>, <19> */
	{ gpgga_parse, 14, "GPGGA", ID_D, ID_D, ID_C, ID_D, ID_C, ID_I, ID_I, ID_D, ID_D, ID_C, ID_D, ID_C, ID_D, ID_I, ID_N, ID_N, ID_N, ID_N, ID_N },
	{ gpgsa_parse, 17, "GPGSA", ID_C, ID_I, ID_I, ID_I, ID_I, ID_I, ID_I, ID_I, ID_I, ID_I, ID_I, ID_I, ID_I, ID_I, ID_D, ID_D, ID_D, ID_N, ID_N },
	{ gpgsv_parse, 19, "GPGSV", ID_I, ID_I, ID_I, ID_I, ID_D, ID_D, ID_I, ID_I, ID_D, ID_D, ID_I, ID_I, ID_D, ID_D, ID_I, ID_I, ID_D, ID_D, ID_I },
	{ gprmc_parse, 11, "GPRMC", ID_D, ID_C, ID_D, ID_C, ID_D, ID_C, ID_D, ID_D, ID_D, ID_D, ID_C, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N },
	{ gpvtg_parse,  8, "GPVTG", ID_D, ID_C, ID_D, ID_C, ID_D, ID_C, ID_D, ID_C, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N },
	{ gpgll_parse,  6, "GPGLL", ID_D, ID_C, ID_D, ID_C, ID_D, ID_C, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N, ID_N },
};

void nmea_decode(char *info)
{
	if (NULL == info)
	{
		printf("pointer is NULL!\n");

		return;
	}
	
	for (int i = 0; i < ARRAY_SIZE(nmea_tbl); i ++)
	{
		if (0 == strcmp(nmea_tbl[i].id, &info[0]))
		{
        #if 1
            nmea_tbl[i].parse(&info[0]);
        #else
			printf("%s:", nmea_tbl[i].id);
			
			for (int j = 1; j <= nmea_tbl[i].len; j ++)
			{
				char *p = &info[j * NMEA_INFO_LEN];

				if (strlen(p))
				{
					switch (nmea_tbl[i].flag[j-1])
					{
					case ID_N:
						break;
					case ID_C:	// char
                        printf("%c", p[0]);
						break;
					case ID_D:	// double
                    {
                        double d = atof(p);
                        printf("%f", d);
						break;
                    }
					case ID_I:	// int
                    {
                        int val = atoi(p);
                        printf("%d", val);
						break;
                    }
					default:
						break;
					}

                    printf("|");
				}
                else
                {
                    printf(" ");
                }
			}

            printf("\n");
			
			break;
        #endif
		}
	}
}

void showInfo(char *info)
{
	printf("%s:Enter-------------\n", __FUNCTION__);
	
	for (int i = 0; i < NMEA_UNIT_LEN; i ++)
	{
		char *p = &info[i * NMEA_INFO_LEN];
		
		if (strlen(p))
		{
			printf("%s\n", p);
		}
	}

	printf("%s:Exit -------------\n", __FUNCTION__);
}

typedef unsigned int uint32_t;

enum
{
    INVALID = 0x00,
    VALID
};

struct gpgga
{
	uint32_t valid_utc;
	double utc;
	uint32_t valid_latitude;
	double latitude;
	uint32_t valid_longitude;
	double longitude;
	uint32_t valid_fix_quality;
	uint32_t fix_quality;
	uint32_t valid_number_of_satellites;
	uint32_t number_of_satellites;
	uint32_t valid_hdop;
	double hdop;
	uint32_t valid_altitude;
	double altitude;
	uint32_t valid_height_of_geoid;
	double height_of_geoid;
	uint32_t valid_time_since_dgps_update;
	double time_since_dgps_update;
	uint32_t valid_dgps_station_id;
	uint32_t dgps_station_id;
};

struct gpgga gpgga_data;

void gpgga_parse(char *info)
{
    printf("%s\n", __FUNCTION__);

    memset(&gpgga_data, sizeof(gpgga_data), 0);

    char *p1, *p2;
    
    // GNSS_GPGGA_UTC
    p1 = &info[1 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_utc = VALID;
        gpgga_data.utc = atof(p1);

        printf("%s:%f\n", STR(gpgga_data.utc), gpgga_data.utc);
    }
    else
    {
        gpgga_data.valid_utc = INVALID;
    }

    // GNSS_GPGGA_LATITUDE
    p1 = &info[2 * NMEA_INFO_LEN];
    p2 = &info[3 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_latitude = VALID;
        gpgga_data.latitude = atof(p1);

        char c = p2[0];

        if ('N' == c)
        {
            gpgga_data.latitude = gpgga_data.latitude;
        }
        else if ('S' == c)
        {
            gpgga_data.latitude =-gpgga_data.latitude;
        }
        else
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpgga_data.latitude), gpgga_data.latitude);
    }
    else
    {
        gpgga_data.valid_latitude = INVALID;
    }

    // GNSS_GPGGA_LONGITUDE
    p1 = &info[4 * NMEA_INFO_LEN];
    p2 = &info[5 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_longitude = VALID;
        gpgga_data.longitude = atof(p1);

        char c = p2[0];

        if ('N' == c)
        {
            gpgga_data.longitude = gpgga_data.longitude;
        }
        else if ('S' == c)
        {
            gpgga_data.longitude =-gpgga_data.longitude;
        }
        else
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpgga_data.longitude), gpgga_data.longitude);
    }
    else
    {
        gpgga_data.valid_longitude = INVALID;
    }

    // GNSS_GPGGA_FIX_QUALITY
    p1 = &info[6 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_fix_quality = VALID;
        gpgga_data.fix_quality = atoi(p1);

        printf("%s:%d\n", STR(gpgga_data.fix_quality), gpgga_data.fix_quality);
    }
    else
    {
        gpgga_data.valid_fix_quality = INVALID;
    }

    // GNSS_GPGGA_NUMBER_OF_SATELLITES
    p1 = &info[7 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_number_of_satellites = VALID;
        gpgga_data.number_of_satellites = atoi(p1);

        printf("%s:%d\n", STR(gpgga_data.number_of_satellites), gpgga_data.number_of_satellites);
    }
    else
    {
        gpgga_data.valid_number_of_satellites = INVALID;
    }

    // GNSS_GPGGA_HDOP
    p1 = &info[8 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_hdop = VALID;
        gpgga_data.hdop = atof(p1);

        printf("%s:%f\n", STR(gpgga_data.hdop), gpgga_data.hdop);
    }
    else
    {
        gpgga_data.valid_hdop = INVALID;
    }

    // GNSS_GPGGA_ALTITUDE
    p1 = &info[9 * NMEA_INFO_LEN];
    p2 = &info[10* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_altitude = VALID;
        gpgga_data.altitude = atof(p1);

        char c = p2[0];

        if ('M' == c)
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpgga_data.altitude), gpgga_data.altitude);
    }
    else
    {
        gpgga_data.valid_altitude = INVALID;
    }

    // GNSS_GPGGA_HEIGHT_OF_GEOID
    p1 = &info[11* NMEA_INFO_LEN];
    p2 = &info[12* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_height_of_geoid = VALID;
        gpgga_data.height_of_geoid = atof(p1);

        char c = p2[0];

        if ('M' == c)
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpgga_data.height_of_geoid), gpgga_data.height_of_geoid);
    }
    else
    {
        gpgga_data.valid_height_of_geoid = INVALID;
    }

    // GNSS_GPGGA_TIME_SINCE_DGPS_UPDATE
    p1 = &info[13* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_time_since_dgps_update = VALID;
        gpgga_data.time_since_dgps_update = atof(p1);

        printf("%s:%f\n", STR(gpgga_data.time_since_dgps_update), gpgga_data.time_since_dgps_update);
    }
    else
    {
        gpgga_data.valid_time_since_dgps_update = INVALID;
    }

    // GNSS_GPGGA_DGPS_STATION_ID
    p1 = &info[13* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgga_data.valid_dgps_station_id = VALID;
        gpgga_data.dgps_station_id = atoi(p1);

        printf("%s:%d\n", STR(gpgga_data.dgps_station_id), gpgga_data.dgps_station_id);
    }
    else
    {
        gpgga_data.valid_dgps_station_id = INVALID;
    }
}

struct gpgsa
{
    uint32_t valid_mode;
    uint32_t mode;
    uint32_t valid_type;
    uint32_t type;
    uint32_t valid_prn_1;
    uint32_t prn_1;
    uint32_t valid_prn_2;
    uint32_t prn_2;
    uint32_t valid_prn_3;
    uint32_t prn_3;
    uint32_t valid_prn_4;
    uint32_t prn_4;
    uint32_t valid_prn_5;
    uint32_t prn_5;
    uint32_t valid_prn_6;
    uint32_t prn_6;
    uint32_t valid_prn_7;
    uint32_t prn_7;
    uint32_t valid_prn_8;
    uint32_t prn_8;
    uint32_t valid_prn_9;
    uint32_t prn_9;
    uint32_t valid_prn_10;
    uint32_t prn_10;
    uint32_t valid_prn_11;
    uint32_t prn_11;
    uint32_t valid_prn_12;
    uint32_t prn_12;
    uint32_t valid_pdop;
    double pdop;
    uint32_t valid_hdop;
    double hdop;
    uint32_t valid_vdop;
    double vdop;
};

struct gpgsa gpgsa_data;

void gpgsa_parse(char *info)
{
    printf("%s\n", __FUNCTION__);

    memset(&gpgsa_data, sizeof(gpgsa_data), 0);

    char *p1, *p2;

    // GNSS_GPGSA_MODE
    p1 = &info[1 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_mode = VALID;
        gpgsa_data.mode = p1[0];

        printf("%s:%c\n", STR(gpgsa_data.mode), gpgsa_data.mode);
    }
    else
    {
        gpgsa_data.valid_mode = INVALID;
    }

    // GNSS_GPGSA_TYPE
    p1 = &info[2 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_type = VALID;
        gpgsa_data.type = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.type), gpgsa_data.type);
    }
    else
    {
        gpgsa_data.valid_type = INVALID;
    }

    // GNSS_GPGSA_PRN_1
    p1 = &info[3 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_1 = VALID;
        gpgsa_data.prn_1 = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_1), gpgsa_data.prn_1);
    }
    else
    {
        gpgsa_data.valid_prn_1 = INVALID;
    }

    // GNSS_GPGSA_PRN_2
    p1 = &info[4 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_2 = VALID;
        gpgsa_data.prn_2 = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_2), gpgsa_data.prn_2);
    }
    else
    {
        gpgsa_data.valid_prn_2 = INVALID;
    }

    // GNSS_GPGSA_PRN_3
    p1 = &info[5 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_3 = VALID;
        gpgsa_data.prn_3 = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_3), gpgsa_data.prn_3);
    }
    else
    {
        gpgsa_data.valid_prn_3 = INVALID;
    }

    // GNSS_GPGSA_PRN_4
    p1 = &info[6 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_4 = VALID;
        gpgsa_data.prn_4 = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_4), gpgsa_data.prn_4);
    }
    else
    {
        gpgsa_data.valid_prn_4 = INVALID;
    }

    // GNSS_GPGSA_PRN_5
    p1 = &info[7 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_5 = VALID;
        gpgsa_data.prn_5 = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_5), gpgsa_data.prn_5);
    }
    else
    {
        gpgsa_data.valid_prn_5 = INVALID;
    }

    // GNSS_GPGSA_PRN_6
    p1 = &info[8 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_6 = VALID;
        gpgsa_data.prn_6 = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_6), gpgsa_data.prn_6);
    }
    else
    {
        gpgsa_data.valid_prn_6 = INVALID;
    }

    // GNSS_GPGSA_PRN_7
    p1 = &info[9 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_7 = VALID;
        gpgsa_data.prn_7 = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_7), gpgsa_data.prn_7);
    }
    else
    {
        gpgsa_data.valid_prn_7 = INVALID;
    }

    // GNSS_GPGSA_PRN_8
    p1 = &info[10* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_8 = VALID;
        gpgsa_data.prn_8 = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_8), gpgsa_data.prn_8);
    }
    else
    {
        gpgsa_data.valid_prn_8 = INVALID;
    }

    // GNSS_GPGSA_PRN_9
    p1 = &info[11* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_9 = VALID;
        gpgsa_data.prn_9 = atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_9), gpgsa_data.prn_9);
    }
    else
    {
        gpgsa_data.valid_prn_9 = INVALID;
    }

    // GNSS_GPGSA_PRN_10
    p1 = &info[12* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_10= VALID;
        gpgsa_data.prn_10= atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_10), gpgsa_data.prn_10);
    }
    else
    {
        gpgsa_data.valid_prn_10= INVALID;
    }

    // GNSS_GPGSA_PRN_11
    p1 = &info[13* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_11= VALID;
        gpgsa_data.prn_11= atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_11), gpgsa_data.prn_11);
    }
    else
    {
        gpgsa_data.valid_prn_11= INVALID;
    }

    // GNSS_GPGSA_PRN_12
    p1 = &info[14* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_prn_12= VALID;
        gpgsa_data.prn_12= atoi(p1);

        printf("%s:%d\n", STR(gpgsa_data.prn_12), gpgsa_data.prn_12);
    }
    else
    {
        gpgsa_data.valid_prn_12= INVALID;
    }

    // GNSS_GPGSA_PDOP
    p1 = &info[15* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_pdop= VALID;
        gpgsa_data.pdop = atof(p1);

        printf("%s:%f\n", STR(gpgsa_data.pdop), gpgsa_data.pdop);
    }
    else
    {
        gpgsa_data.valid_pdop = INVALID;
    }

    // GNSS_GPGSA_HDOP
    p1 = &info[16* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_hdop= VALID;
        gpgsa_data.hdop = atof(p1);

        printf("%s:%f\n", STR(gpgsa_data.hdop), gpgsa_data.hdop);
    }
    else
    {
        gpgsa_data.valid_hdop = INVALID;
    }

    // GNSS_GPGSA_VDOP
    p1 = &info[17* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsa_data.valid_vdop= VALID;
        gpgsa_data.vdop = atof(p1);

        printf("%s:%f\n", STR(gpgsa_data.vdop), gpgsa_data.vdop);
    }
    else
    {
        gpgsa_data.valid_vdop = INVALID;
    }
}

struct gpgsv
{
    uint32_t valid_number_of_messages;
    uint32_t number_of_messages;
    uint32_t valid_message_number;
    uint32_t message_number;
    uint32_t valid_total_number_of_svs;
    uint32_t total_number_of_svs;

    uint32_t valid_sv1_prn;
    uint32_t sv1_prn;
    uint32_t valid_sv1_elevation;
    double sv1_elevation;
    uint32_t valid_sv1_azimuth;
    double sv1_azimuth;
    uint32_t valid_sv1_snr;
    uint32_t sv1_snr;

    uint32_t valid_sv2_prn;
    uint32_t sv2_prn;
    uint32_t valid_sv2_elevation;
    double sv2_elevation;
    uint32_t valid_sv2_azimuth;
    double sv2_azimuth;
    uint32_t valid_sv2_snr;
    uint32_t sv2_snr;

    uint32_t valid_sv3_prn;
    uint32_t sv3_prn;
    uint32_t valid_sv3_elevation;
    double sv3_elevation;
    uint32_t valid_sv3_azimuth;
    double sv3_azimuth;
    uint32_t valid_sv3_snr;
    uint32_t sv3_snr;

    uint32_t valid_sv4_prn;
    uint32_t sv4_prn;
    uint32_t valid_sv4_elevation;
    double sv4_elevation;
    uint32_t valid_sv4_azimuth;
    double sv4_azimuth;
    uint32_t valid_sv4_snr;
    uint32_t sv4_snr;
};

struct gpgsv gpgsv_data;

void gpgsv_parse(char *info)
{
    printf("%s\n", __FUNCTION__);

    memset(&gpgsv_data, sizeof(gpgsv_data), 0);

    char *p1, *p2;

    // GNSS_GPGSV_NUMBER_OF_MESSAGES
    p1 = &info[1 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_number_of_messages = VALID;
        gpgsv_data.number_of_messages = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.number_of_messages), gpgsv_data.number_of_messages);
    }
    else
    {
        gpgsv_data.valid_number_of_messages = INVALID;
    }

    // GNSS_GPGSV_MESSAGE_NUMBER
    p1 = &info[2 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_message_number = VALID;
        gpgsv_data.message_number = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.message_number), gpgsv_data.message_number);
    }
    else
    {
        gpgsv_data.valid_message_number = INVALID;
    }

    // GNSS_GPGSV_TOTAL_NUMBER_OF_SVS
    p1 = &info[3 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_total_number_of_svs = VALID;
        gpgsv_data.total_number_of_svs = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.total_number_of_svs), gpgsv_data.total_number_of_svs);
    }
    else
    {
        gpgsv_data.valid_total_number_of_svs = INVALID;
    }

    // GNSS_GPGSV_SV1_PRN
    p1 = &info[4 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv1_prn = VALID;
        gpgsv_data.sv1_prn = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.sv1_prn), gpgsv_data.sv1_prn);
    }
    else
    {
        gpgsv_data.valid_sv1_prn = INVALID;
    }

    // GNSS_GPGSV_SV1_ELEVATION
    p1 = &info[5 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv1_elevation = VALID;
        gpgsv_data.sv1_elevation = atof(p1);

        printf("%s:%f\n", STR(gpgsv_data.sv1_elevation), gpgsv_data.sv1_elevation);
    }
    else
    {
        gpgsv_data.valid_sv1_elevation = INVALID;
    }

    // GNSS_GPGSV_SV1_AZIMUTH
    p1 = &info[6 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv1_azimuth = VALID;
        gpgsv_data.sv1_azimuth = atof(p1);

        printf("%s:%f\n", STR(gpgsv_data.sv1_azimuth), gpgsv_data.sv1_azimuth);
    }
    else
    {
        gpgsv_data.valid_sv1_azimuth = INVALID;
    }

    // GNSS_GPGSV_SV1_SNR
    p1 = &info[7 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv1_snr = VALID;
        gpgsv_data.sv1_snr = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.sv1_snr), gpgsv_data.sv1_snr);
    }
    else
    {
        gpgsv_data.valid_sv1_snr = INVALID;
    }

    // GNSS_GPGSV_SV2_PRN
    p1 = &info[8 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv2_prn = VALID;
        gpgsv_data.sv2_prn = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.sv2_prn), gpgsv_data.sv2_prn);
    }
    else
    {
        gpgsv_data.valid_sv2_prn = INVALID;
    }

    // GNSS_GPGSV_SV2_ELEVATION
    p1 = &info[9 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv2_elevation = VALID;
        gpgsv_data.sv2_elevation = atof(p1);

        printf("%s:%f\n", STR(gpgsv_data.sv2_elevation), gpgsv_data.sv2_elevation);
    }
    else
    {
        gpgsv_data.valid_sv2_elevation = INVALID;
    }

    // GNSS_GPGSV_SV2_AZIMUTH
    p1 = &info[10* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv2_azimuth = VALID;
        gpgsv_data.sv2_azimuth = atof(p1);

        printf("%s:%f\n", STR(gpgsv_data.sv2_azimuth), gpgsv_data.sv2_azimuth);
    }
    else
    {
        gpgsv_data.valid_sv2_azimuth = INVALID;
    }

    // GNSS_GPGSV_SV2_SNR
    p1 = &info[11* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv2_snr = VALID;
        gpgsv_data.sv2_snr = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.sv2_snr), gpgsv_data.sv2_snr);
    }
    else
    {
        gpgsv_data.valid_sv2_snr = INVALID;
    }

    // GNSS_GPGSV_SV3_PRN
    p1 = &info[12* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv3_prn = VALID;
        gpgsv_data.sv3_prn = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.sv3_prn), gpgsv_data.sv3_prn);
    }
    else
    {
        gpgsv_data.valid_sv3_prn = INVALID;
    }

    // GNSS_GPGSV_SV3_ELEVATION
    p1 = &info[13* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv3_elevation = VALID;
        gpgsv_data.sv3_elevation = atof(p1);

        printf("%s:%f\n", STR(gpgsv_data.sv3_elevation), gpgsv_data.sv3_elevation);
    }
    else
    {
        gpgsv_data.valid_sv3_elevation = INVALID;
    }

    // GNSS_GPGSV_SV3_AZIMUTH
    p1 = &info[14* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv3_azimuth = VALID;
        gpgsv_data.sv3_azimuth = atof(p1);

        printf("%s:%f\n", STR(gpgsv_data.sv3_azimuth), gpgsv_data.sv3_azimuth);
    }
    else
    {
        gpgsv_data.valid_sv3_azimuth = INVALID;
    }

    // GNSS_GPGSV_SV3_SNR
    p1 = &info[15* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv3_snr = VALID;
        gpgsv_data.sv3_snr = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.sv3_snr), gpgsv_data.sv3_snr);
    }
    else
    {
        gpgsv_data.valid_sv3_snr = INVALID;
    }

    // GNSS_GPGSV_SV4_PRN
    p1 = &info[16* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv4_prn = VALID;
        gpgsv_data.sv4_prn = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.sv4_prn), gpgsv_data.sv4_prn);
    }
    else
    {
        gpgsv_data.valid_sv4_prn = INVALID;
    }

    // GNSS_GPGSV_SV4_ELEVATION
    p1 = &info[17* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv4_elevation = VALID;
        gpgsv_data.sv4_elevation = atof(p1);

        printf("%s:%f\n", STR(gpgsv_data.sv4_elevation), gpgsv_data.sv4_elevation);
    }
    else
    {
        gpgsv_data.valid_sv4_elevation = INVALID;
    }

    // GNSS_GPGSV_SV4_AZIMUTH
    p1 = &info[18* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv4_azimuth = VALID;
        gpgsv_data.sv4_azimuth = atof(p1);

        printf("%s:%f\n", STR(gpgsv_data.sv4_azimuth), gpgsv_data.sv4_azimuth);
    }
    else
    {
        gpgsv_data.valid_sv4_azimuth = INVALID;
    }

    // GNSS_GPGSV_SV4_SNR
    p1 = &info[19* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgsv_data.valid_sv4_snr = VALID;
        gpgsv_data.sv4_snr = atoi(p1);

        printf("%s:%d\n", STR(gpgsv_data.sv4_snr), gpgsv_data.sv4_snr);
    }
    else
    {
        gpgsv_data.valid_sv4_snr = INVALID;
    }
}

struct gprmc
{
    uint32_t valid_utc;
    double utc;
    uint32_t valid_fix_quality;
    uint32_t fix_quality;
    uint32_t valid_latitude;
    double latitude;
    uint32_t valid_longitude;
    double longitude;
    uint32_t valid_speed;
    double speed;
    uint32_t valid_azimuth;
    double azimuth;
    uint32_t valid_magnetic_declination;
    double magnetic_declination;
};

struct gprmc gprmc_data;

void gprmc_parse(char *info)
{
    printf("%s\n", __FUNCTION__);

    memset(&gprmc_data, sizeof(gprmc_data), 0);

    char *p1, *p2;

    // GNSS_GPRMC_UTC
    p1 = &info[1 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gprmc_data.valid_utc = VALID;
        gprmc_data.utc = atof(p1);

        printf("%s:%f\n", STR(gprmc_data.utc), gprmc_data.utc);
    }
    else
    {
        gprmc_data.valid_utc = INVALID;
    }

    // GNSS_GPRMC_FIX_QUALITY
    p1 = &info[2 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gprmc_data.valid_fix_quality = VALID;
        gprmc_data.fix_quality = p1[0];

        printf("%s:%c\n", STR(gprmc_data.fix_quality), gprmc_data.fix_quality);
    }
    else
    {
        gprmc_data.valid_fix_quality = INVALID;
    }

    // GNSS_GPRMC_LATITUDE
    p1 = &info[3 * NMEA_INFO_LEN];
    p2 = &info[4 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gprmc_data.valid_latitude = VALID;
        gprmc_data.latitude = atof(p1);

        char c = p2[0];

        if ('N' == c)
        {
            gprmc_data.latitude = gprmc_data.latitude;
        }
        else if ('S' == c)
        {
            gprmc_data.latitude =-gprmc_data.latitude;
        }
        else
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gprmc_data.latitude), gprmc_data.latitude);
    }
    else
    {
        gprmc_data.valid_latitude = INVALID;
    }

    // GNSS_GPRMC_LONGITUDE
    p1 = &info[5 * NMEA_INFO_LEN];
    p2 = &info[6 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gprmc_data.valid_longitude = VALID;
        gprmc_data.longitude = atof(p1);

        char c = p2[0];

        if ('E' == c)
        {
            gprmc_data.longitude = gprmc_data.longitude;
        }
        else if ('W' == c)
        {
            gprmc_data.longitude =-gprmc_data.longitude;
        }
        else
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gprmc_data.longitude), gprmc_data.longitude);
    }
    else
    {
        gprmc_data.valid_longitude = INVALID;
    }

    // GNSS_GPRMC_SPEED
    p1 = &info[7 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gprmc_data.valid_speed = VALID;
        gprmc_data.speed = atof(p1);

        printf("%s:%f\n", STR(gprmc_data.speed), gprmc_data.speed);
    }
    else
    {
        gprmc_data.valid_speed = INVALID;
    }

    // GNSS_GPRMC_AZIMUTH
    p1 = &info[8 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gprmc_data.valid_azimuth = VALID;
        gprmc_data.azimuth = atof(p1);

        printf("%s:%f\n", STR(gprmc_data.azimuth), gprmc_data.azimuth);
    }
    else
    {
        gprmc_data.valid_azimuth = INVALID;
    }

    // UTC DATE
    p1 = &info[9 * NMEA_INFO_LEN];
    
    if (strlen(p1))
    {
        printf("%s:%d\n", STR(UTC.DATE), atoi(p1));
    }

    // GNSS_GPRMC_MAGNETIC_DECLINATION
    p1 = &info[10* NMEA_INFO_LEN];
    p2 = &info[11* NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gprmc_data.valid_magnetic_declination = VALID;
        gprmc_data.magnetic_declination = atof(p1);

        char c = p2[0];

        if ('E' == c)
        {
            ;
        }
        else if ('W' == c)
        {
            ;
        }
        else
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gprmc_data.magnetic_declination), gprmc_data.magnetic_declination);
    }
    else
    {
        gprmc_data.valid_magnetic_declination = INVALID;
    }
}

struct gpvtg
{
    uint32_t valid_t_motion_angle;
    double t_motion_angle;
    uint32_t valid_m_motion_angle;
    double m_motion_angle;

    uint32_t valid_n_hor_movement_speed;
    double n_hor_movement_speed;
    uint32_t valid_k_hor_movement_speed;
    double k_hor_movement_speed;
};

struct gpvtg gpvtg_data;

void gpvtg_parse(char *info)
{
    printf("%s\n", __FUNCTION__);

    memset(&gpvtg_data, sizeof(gpvtg_data), 0);

    char *p1, *p2;

    // GNSS_GPVTG_T_MOTION_ANGLE
    p1 = &info[1 * NMEA_INFO_LEN];
    p2 = &info[2 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpvtg_data.valid_t_motion_angle = VALID;
        gpvtg_data.t_motion_angle = atof(p1);

        char c = p2[0];

        if ('T' == c)
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpvtg_data.t_motion_angle), gpvtg_data.t_motion_angle);
    }
    else
    {
        gpvtg_data.valid_t_motion_angle = INVALID;
    }

    // GNSS_GPVTG_M_MOTION_ANGLE
    p1 = &info[3 * NMEA_INFO_LEN];
    p2 = &info[4 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpvtg_data.valid_m_motion_angle = VALID;
        gpvtg_data.m_motion_angle = atof(p1);

        char c = p2[0];

        if ('M' == c)
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpvtg_data.m_motion_angle), gpvtg_data.m_motion_angle);
    }
    else
    {
        gpvtg_data.valid_m_motion_angle = INVALID;
    }

    // GNSS_GPVTG_N_HOR_MOVEMENT_SPEED
    p1 = &info[5 * NMEA_INFO_LEN];
    p2 = &info[6 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpvtg_data.valid_n_hor_movement_speed = VALID;
        gpvtg_data.n_hor_movement_speed = atof(p1);

        char c = p2[0];

        if ('N' == c)
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpvtg_data.n_hor_movement_speed), gpvtg_data.n_hor_movement_speed);
    }
    else
    {
        gpvtg_data.valid_n_hor_movement_speed = INVALID;
    }

    // GNSS_GPVTG_K_HOR_MOVEMENT_SPEED
    p1 = &info[7 * NMEA_INFO_LEN];
    p2 = &info[8 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpvtg_data.valid_k_hor_movement_speed = VALID;
        gpvtg_data.k_hor_movement_speed = atof(p1);

        char c = p2[0];

        if ('N' == c)
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpvtg_data.k_hor_movement_speed), gpvtg_data.k_hor_movement_speed);
    }
    else
    {
        gpvtg_data.valid_k_hor_movement_speed = INVALID;
    }
}

struct gpgll
{
    uint32_t valid_latitude;
    double latitude;
    uint32_t valid_longitude;
    double longitude;
    uint32_t valid_utc;
    double utc;
    uint32_t valid_fix_quality;
    uint32_t fix_quality;
};

struct gpgll gpgll_data;

void gpgll_parse(char *info)
{
    printf("%s\n", __FUNCTION__);

    memset(&gpgll_data, sizeof(gpgll_data), 0);

    char *p1, *p2;

    // GNSS_GPGLL_LATITUDE
    p1 = &info[1 * NMEA_INFO_LEN];
    p2 = &info[2 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgll_data.valid_latitude = VALID;
        gpgll_data.latitude = atof(p1);

        char c = p2[0];

        if ('N' == c)
        {
            gpgll_data.latitude = gpgll_data.latitude;
        }
        else if ('S' == c)
        {
            gpgll_data.latitude =-gpgll_data.latitude;
        }
        else
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpgll_data.latitude), gpgll_data.latitude);
    }
    else
    {
        gpgll_data.valid_latitude = INVALID;
    }

    // GNSS_GPGLL_LONGITUDE
    p1 = &info[3 * NMEA_INFO_LEN];
    p2 = &info[4 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgll_data.valid_longitude = VALID;
        gpgll_data.longitude = atof(p1);

        char c = p2[0];

        if ('E' == c)
        {
            gpgll_data.longitude = gpgll_data.longitude;
        }
        else if ('W' == c)
        {
            gpgll_data.longitude =-gpgll_data.longitude;
        }
        else
        {
            // do_nothing();
        }

        printf("%s:%f\n", STR(gpgll_data.longitude), gpgll_data.longitude);
    }
    else
    {
        gpgll_data.valid_longitude = INVALID;
    }

    // GNSS_GPGLL_UTC
    p1 = &info[5 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgll_data.valid_utc = VALID;
        gpgll_data.utc = atof(p1);

        printf("%s:%f\n", STR(gpgll_data.utc), gpgll_data.utc);
    }
    else
    {
        gpgll_data.valid_utc = INVALID;
    }

    // GNSS_GPGLL_FIX_QUALITY
    p1 = &info[6 * NMEA_INFO_LEN];

    if (strlen(p1))
    {
        gpgll_data.valid_fix_quality = VALID;
        gpgll_data.fix_quality = p1[0];

        printf("%s:%c\n", STR(gpgll_data.fix_quality), gpgll_data.fix_quality);
    }
    else
    {
        gpgll_data.valid_fix_quality = INVALID;
    }
}
  • 0
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值