nmea_parse.c

来自「gpsd, a popular GPS daemon.」· C语言 代码 · 共 842 行 · 第 1/2 页

/* $Id: nmea_parse.c 4629 2007-12-26 02:16:05Z ckuethe $ */
#include <sys/types.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <stdarg.h>
#include <ctype.h>
#include <time.h>

#include "gpsd_config.h"
#include "gpsd.h"
#include "timebase.h"

#ifdef NMEA_ENABLE
/**************************************************************************
 *
 * Parser helpers begin here
 *
 **************************************************************************/

static void do_lat_lon(char *field[], struct gps_data_t *out)
/* process a pair of latitude/longitude fields starting at field index BEGIN */
{
    double lat, lon, d, m;
    char str[20], *p;
    int updated = 0;

    if (*(p = field[0]) != '\0') {
	strncpy(str, p, 20);
	(void)sscanf(p, "%lf", &lat);
	m = 100.0 * modf(lat / 100.0, &d);
	lat = d + m / 60.0;
	p = field[1];
	if (*p == 'S')
	    lat = -lat;
	if (out->fix.latitude != lat)
	    out->fix.latitude = lat;
	updated++;
    }
    if (*(p = field[2]) != '\0') {
	strncpy(str, p, 20);
	(void)sscanf(p, "%lf", &lon);
	m = 100.0 * modf(lon / 100.0, &d);
	lon = d + m / 60.0;

	p = field[3];
	if (*p == 'W')
	    lon = -lon;
	if (out->fix.longitude != lon)
	    out->fix.longitude = lon;
	updated++;
    }
}

/**************************************************************************
 *
 * Scary timestamp fudging begins here
 *
 * Four sentences, GGA and GLL and RMC and ZDA, contain timestamps.
 * Timestamps always look like hhmmss.ss, with the trailing .ss part
 * optional.  RMC has a date field, in the format ddmmyy.  ZDA has
 * separate fields for day/month/year, with a 4-digit year.  This
 * means that for RMC we must supply a century and for GGA and GLL we
 * must supply a century, year, and day.  We get the missing data from
 * a previous RMC or ZDA; century in RMC is supplied by a constant if
 * there has been no previous RMC.
 *
 **************************************************************************/

#define DD(s)	((int)((s)[0]-'0')*10+(int)((s)[1]-'0'))

static void merge_ddmmyy(char *ddmmyy, struct gps_device_t *session)
/* sentence supplied ddmmyy, but no century part */
{
    if (session->driver.nmea.date.tm_year == 0)
	session->driver.nmea.date.tm_year = (CENTURY_BASE + DD(ddmmyy+4)) - 1900;
    session->driver.nmea.date.tm_mon = DD(ddmmyy+2)-1;
    session->driver.nmea.date.tm_mday = DD(ddmmyy);
}

static void merge_hhmmss(char *hhmmss, struct gps_device_t *session)
/* update from a UTC time */
{
    int old_hour = session->driver.nmea.date.tm_hour;

    session->driver.nmea.date.tm_hour = DD(hhmmss);
	if (session->driver.nmea.date.tm_hour < old_hour)	/* midnight wrap */
	session->driver.nmea.date.tm_mday++;
    session->driver.nmea.date.tm_min = DD(hhmmss+2);
    session->driver.nmea.date.tm_sec = DD(hhmmss+4);
    session->driver.nmea.subseconds = atof(hhmmss+4) - session->driver.nmea.date.tm_sec;
}

#undef DD

/**************************************************************************
 *
 * Compare GPS timestamps for equality.  Depends on the fact that the
 * timestamp granularity of GPS is 1/100th of a second.  Use this to avoid
 * naive float comparisons.
 *
 **************************************************************************/

#define GPS_TIME_EQUAL(a, b) (fabs((a) - (b)) < 0.01)

/**************************************************************************
 *
 * NMEA sentence handling begins here
 *
 **************************************************************************/

static gps_mask_t processGPRMC(int count, char *field[], struct gps_device_t *session)
/* Recommend Minimum Course Specific GPS/TRANSIT Data */
{
    /*
	RMC,225446.33,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E,A*68
     1     225446.33    Time of fix 22:54:46 UTC
     2     A	    Status of Fix A = Autonomous, valid; D = Differential, valid; V = invalid
     3,4   4916.45,N    Latitude 49 deg. 16.45 min North
     5,6   12311.12,W   Longitude 123 deg. 11.12 min West
     7     000.5	Speed over ground, Knots
     8     054.7	Course Made Good, True north
     9     191194       Date of fix  19 November 1994
     10,11 020.3,E      Magnetic variation 20.3 deg East
     12    A	    FAA mode indicator (NMEA 2.3 and later)
			A=autonomous, D=differential, E=Estimated,
			N=not valid, S=Simulator, M=Manual input mode
	   *68	  mandatory nmea_checksum

     * SiRF chipsets don't return either Mode Indicator or magnetic variation.
     */
    gps_mask_t mask = 0;

    if (strcmp(field[2], "V")==0) {
	/* copes with Magellan EC-10X, see below */
	if (session->gpsdata.status != STATUS_NO_FIX) {
	    session->gpsdata.status = STATUS_NO_FIX;
	    mask |= STATUS_SET;
	}
	if (session->gpsdata.fix.mode >= MODE_2D) {
	    session->gpsdata.fix.mode = MODE_NO_FIX;
	    mask |= MODE_SET;
	}
	/* set something nz, so it won't look like an unknown sentence */
	mask |= ONLINE_SET;
    } else if (strcmp(field[2], "A")==0) {
	if (count > 9) {
	    merge_ddmmyy(field[9], session);
	    merge_hhmmss(field[1], session);
	    mask |= TIME_SET;
	    session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds;
	    if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) {
		mask |= CYCLE_START_SET;
		gpsd_report(LOG_PROG, "GPRMC starts a reporting cycle.\n");
	    }
	    session->gpsdata.sentence_time = session->gpsdata.fix.time;
	}
	do_lat_lon(&field[3], &session->gpsdata);
	mask |= LATLON_SET;
	session->gpsdata.fix.speed = atof(field[7]) * KNOTS_TO_MPS;
	session->gpsdata.fix.track = atof(field[8]);
	mask |= (TRACK_SET | SPEED_SET);
	/*
	 * This copes with GPSes like the Magellan EC-10X that *only* emit
	 * GPRMC. In this case we set mode and status here so the client
	 * code that relies on them won't mistakenly believe it has never
	 * received a fix.
	 */
	if (session->gpsdata.status == STATUS_NO_FIX) {
	    session->gpsdata.status = STATUS_FIX;	/* could be DGPS_FIX, we can't tell */
	    mask |= STATUS_SET;
	}
	if (session->gpsdata.fix.mode < MODE_2D) {
	    session->gpsdata.fix.mode = MODE_2D;
	    mask |= MODE_SET;
	}
    }

    gpsd_report(LOG_PROG, "GPRMC sets mode %d\n", session->gpsdata.fix.mode);
    return mask;
}

static gps_mask_t processGPGLL(int count, char *field[], struct gps_device_t *session)
/* Geographic position - Latitude, Longitude */
{
    /* Introduced in NMEA 3.0.  Here are the fields:
     *
     * 1,2 Latitude, N (North) or S (South)
     * 3,4 Longitude, E (East) or W (West)
     * 5 UTC of position
     * 6 A=Active, V=Void
     * 7 Mode Indicator
     *   A = Autonomous mode
     *   D = Differential Mode
     *   E = Estimated (dead-reckoning) mode
     *   M = Manual Input Mode
     *   S = Simulated Mode
     *   N = Data Not Valid
     *
     * I found a note at <http://www.secoh.ru/windows/gps/nmfqexep.txt>
     * indicating that the Garmin 65 does not return time and status.
     * SiRF chipsets don't return the Mode Indicator.
     * This code copes gracefully with both quirks.
     *
     * Unless you care about the FAA indicator, this sentence supplies nothing
     * that GPRMC doesn't already.  But at least one Garmin GPS -- the 48
     * actually ships updates in GPLL that aren't redundant.
     */
    char *status = field[7];
    gps_mask_t mask = ERROR_SET;

    if (strcmp(field[6], "A")==0 && (count < 8 || *status != 'N')) {
	int newstatus = session->gpsdata.status;

	mask = 0;
	merge_hhmmss(field[5], session);
	if (session->driver.nmea.date.tm_year == 0)
	    gpsd_report(LOG_WARN, "can't use GGL time until after ZDA or RMC has supplied a year.\n");
	else {
	    mask = TIME_SET;
	    session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds;
	    if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) {
		mask |= CYCLE_START_SET;
		gpsd_report(LOG_PROG, "GPGLL starts a reporting cycle.\n");
	    }
	    session->gpsdata.sentence_time = session->gpsdata.fix.time;
	}
	do_lat_lon(&field[1], &session->gpsdata);
	mask |= LATLON_SET;
	if (count >= 8 && *status == 'D')
	    newstatus = STATUS_DGPS_FIX;	/* differential */
	else
	    newstatus = STATUS_FIX;
	/*
	 * This is a bit dodgy.  Technically we shouldn't set the mode
	 * bit until we see GSA.  But it may be later in the cycle,
	 * some devices like the FV-18 don't send it by default, and
	 * elsewhere in the code we want to be able to test for the
	 * presence of a valid fix with mode > MODE_NO_FIX.
	 */
	if (session->gpsdata.fix.mode < MODE_2D) {
	    session->gpsdata.fix.mode = MODE_2D;
	    mask |= MODE_SET;
	}
	session->gpsdata.status = newstatus;
	mask |= STATUS_SET;
	gpsd_report(LOG_PROG, "GPGLL sets status %d\n", session->gpsdata.status);
    }

    return mask;
}

static gps_mask_t processGPGGA(int c UNUSED, char *field[], struct gps_device_t *session)
/* Global Positioning System Fix Data */
{
    /*
	GGA,123519,4807.038,N,01131.324,E,1,08,0.9,545.4,M,46.9,M, , *42
	   123519       Fix taken at 12:35:19 UTC
	   4807.038,N   Latitude 48 deg 07.038' N
	   01131.324,E  Longitude 11 deg 31.324' E
	   1	    Fix quality: 0 = invalid, 1 = GPS fix, 2 = DGPS fix,
	   		3=PPS (Precise Position Service),
			4=RTK (Real Time Kinematic) with fixed integers,
			5=Float RTK, 6=Estimated, 7=Manual, 8=Simulator
	   08	   Number of satellites being tracked
	   0.9	  Horizontal dilution of position
	   545.4,M      Altitude, Metres above mean sea level
	   46.9,M       Height of geoid (mean sea level) above WGS84
			ellipsoid, in Meters
	   (empty field) time in seconds since last DGPS update
	   (empty field) DGPS station ID number (0000-1023)
    */
    gps_mask_t mask;

    session->gpsdata.status = atoi(field[6]);
    mask = STATUS_SET;
    if (session->gpsdata.status > STATUS_NO_FIX) {
	char *altitude;
	double oldfixtime = session->gpsdata.fix.time;

	merge_hhmmss(field[1], session);
	if (session->driver.nmea.date.tm_year == 0)
	    gpsd_report(LOG_WARN, "can't use GGA time until after ZDA or RMC has supplied a year.\n");
	else {
	    mask |= TIME_SET;
	    session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds;
	    if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) {
		mask |= CYCLE_START_SET;
		gpsd_report(LOG_PROG, "GPGGA starts a reporting cycle.\n");
	    }
	    session->gpsdata.sentence_time = session->gpsdata.fix.time;
	}
	do_lat_lon(&field[2], &session->gpsdata);
	mask |= LATLON_SET;
	session->gpsdata.satellites_used = atoi(field[7]);
	altitude = field[9];
	/*
	 * SiRF chipsets up to version 2.2 report a null altitude field.
	 * See <http://www.sirf.com/Downloads/Technical/apnt0033.pdf>.
	 * If we see this, force mode to 2D at most.
	 */
	if (altitude[0] == '\0') {
	    if (session->gpsdata.fix.mode == MODE_3D) {
		session->gpsdata.fix.mode = session->gpsdata.status ? MODE_2D : MODE_NO_FIX;
		mask |= MODE_SET;
	    }
	} else {
	    double oldaltitude = session->gpsdata.fix.altitude;

	    session->gpsdata.fix.altitude = atof(altitude);
	    mask |= ALTITUDE_SET;
	    /*
	     * This is a bit dodgy.  Technically we shouldn't set the mode
	     * bit until we see GSA.  But it may be later in the cycle,
	     * some devices like the FV-18 don't send it by default, and
	     * elsewhere in the code we want to be able to test for the
	     * presence of a valid fix with mode > MODE_NO_FIX.
	     */
	    if (session->gpsdata.fix.mode < MODE_3D) {
		session->gpsdata.fix.mode = MODE_3D;
		mask |= MODE_SET;
	    }

	    /*
	     * Compute climb/sink in the simplest possible way.
	     * This substitutes for the climb report provided by
	     * SiRF and Garmin chips, which might have some smoothing
	     * going on.
	     */
	    if (isnan(oldaltitude)!=0 || session->gpsdata.fix.time==oldfixtime)
		session->gpsdata.fix.climb = 0;
	    else {
		session->gpsdata.fix.climb = (session->gpsdata.fix.altitude-oldaltitude)/(session->gpsdata.fix.time-oldfixtime);
	    }
	    mask |= CLIMB_SET;
	}
	if (strlen(field[11]) > 0) {
	   session->gpsdata.separation = atof(field[11]);
	} else {
	   session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude,session->gpsdata.fix.longitude);
	}
    }
    gpsd_report(LOG_PROG, "GPGGA sets status %d and mode %d (%s)\n", session->gpsdata.status, session->gpsdata.fix.mode, ((mask&MODE_SET)!=0) ? "changed" : "unchanged");
    return mask;
}

static gps_mask_t processGPGSA(int count, char *field[], struct gps_device_t *session)
/* GPS DOP and Active Satellites */
{
    /*
	eg1. $GPGSA,A,3,,,,,,16,18,,22,24,,,3.6,2.1,2.2*3C
	eg2. $GPGSA,A,3,19,28,14,18,27,22,31,39,,,,,1.7,1.0,1.3*35
	1    = Mode:
	       M=Manual, forced to operate in 2D or 3D
	       A=Automatic, 3D/2D
	2    = Mode: 1=Fix not available, 2=2D, 3=3D
	3-14 = PRNs of satellites used in position fix (null for unused fields)
	15   = PDOP
	16   = HDOP
	17   = VDOP
     */
    gps_mask_t mask;
    int i;

    /*
     * One chipset called the i.Trek M3 issues GPGSA lines that look like
     * this: "$GPGSA,A,1,,,,*32" when it has no fix.  This is broken
     * in at least two ways: it's got the wrong number of fields, and
     * it claims to be a valid sentence (A flag) when it isn't.
     * Alarmingly, it's possible this error may be generic to SiRFstarIII.
     */
    if (count < 17)
	return ONLINE_SET;

    session->gpsdata.fix.mode = atoi(field[2]);
    /*
     * The first arm of this conditional ignores dead-reckoning
     * fixes from an Antaris chipset. which returns E in field 2
     * for a dead-reckoning estimate.  Fix by Andreas Stricker.
     */
    if (session->gpsdata.fix.mode == 0 && field[2][0] == 'E')
	mask = 0;
    else
	mask = MODE_SET;
    gpsd_report(LOG_PROG, "GPGSA sets mode %d\n", session->gpsdata.fix.mode);
    session->gpsdata.pdop = atof(field[count-3]);
    session->gpsdata.hdop = atof(field[count-2]);
    session->gpsdata.vdop = atof(field[count-1]);
    session->gpsdata.satellites_used = 0;
    memset(session->gpsdata.used,0,sizeof(session->gpsdata.used));
    /* the magic 6 here counts the tag, two mode fields, and the DOP fields */
    for (i = 0; i < count - 6; i++) {
	int prn = atoi(field[i+3]);
	if (prn > 0)
	    session->gpsdata.used[session->gpsdata.satellites_used++] = prn;
    }
    mask |= HDOP_SET | VDOP_SET | PDOP_SET | USED_SET;

    return mask;
}

static gps_mask_t processGPGSV(int count, char *field[], struct gps_device_t *session)
/* GPS Satellites in View */
{
    /*
	GSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75
	   2	    Number of sentences for full data
	   1	    sentence 1 of 2
	   08	   Total number of satellites in view
	   01	   Satellite PRN number
	   40	   Elevation, degrees
	   083	  Azimuth, degrees
	   46	   Signal-to-noise ratio in decibels
	   <repeat for up to 4 satellites per sentence>
		There my be up to three GSV sentences in a data packet
     */
    int n, fldnum;
    if (count <= 3) {
	gpsd_zero_satellites(&session->gpsdata);