nmea_parse.c

很好的一个gps daemon驱动,简单实用

#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <stdarg.h>

#include "config.h"
#include "gpsd.h"

/**************************************************************************
 *
 * Parser helpers begin here
 *
 **************************************************************************/

static char *field(char *sentence, short n)
/* return the nth comma-delimited field from the sentence */
{
    static char result[100];
    char c, *p = sentence;
    int i;

    while (n-- > 0)
        while ((c = *p++) != ',' && c != '\0')
	    continue;
    strncpy(result, p, sizeof(result)-1);
    p = result;
    i = 0;
    while (*p && *p != ',' && *p != '*' && *p != '\r' && ++i < sizeof(result))
	p++;
    *p = '\0';
    return result;
}

static void do_lat_lon(char *sentence, int begin, 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(sentence, begin + 0)) != '\0') {
	strncpy(str, p, 20);
	sscanf(p, "%lf", &lat);
	m = 100.0 * modf(lat / 100.0, &d);
	lat = d + m / 60.0;
	p = field(sentence, begin + 1);
	if (*p == 'S')
	    lat = -lat;
	if (out->latitude != lat)
	    out->latitude = lat;
	updated++;
    }
    if (*(p = field(sentence, begin + 2)) != '\0') {
	strncpy(str, p, 20);
	sscanf(p, "%lf", &lon);
	m = 100.0 * modf(lon / 100.0, &d);
	lon = d + m / 60.0;

	p = field(sentence, begin + 3);
	if (*p == 'W')
	    lon = -lon;
	if (out->longitude != lon)
	    out->longitude = lon;
	updated++;
    }
    if (updated == 2)
	out->latlon_stamp.changed = 1;
    REFRESH(out->latlon_stamp);
}

static int update_field_i(char *sentence, int fld, int *dest)
/* update an integer-valued field */
{
    int tmp, changed;

    tmp = atoi(field(sentence, fld));
    changed = (tmp != *dest);
    *dest = tmp;
    return changed;
}

static int update_field_f(char *sentence, int fld, double *dest)
/* update a float-valued field */
{
    int changed;
    double tmp;

    tmp = atof(field(sentence, fld));
    changed = (tmp != *dest);
    *dest = tmp;
    return changed;
}

/**************************************************************************
 *
 * Scary timestamp fudging begins here
 *
 **************************************************************************/

/*
   Three sentences, GGA and GGL and RMC, contain timestamps. Timestamps 
   always look like hhmmss.ss, with the trailing .ss part optional.
   RMC alone has a date field, in the format ddmmyy.  But we want the 
   output to be in ISO 8601 format:

   yyyy-mm-ddThh:mm:ss.sssZ
   012345678901234567890123

   (where part or all of the decimal second suffix may be omitted).
   This means that for GPRMC we must supply a century and for GGA and
   GGL we must supply a century, year, and day.

   We get the missing data from the host machine's clock time.  That is, 
   the machine where this *daemon* is running -- which is probably hooked
   to the GPS by a link short enough that it doesn't cross the International
   Date Line.  Even if it does, this hack could only screw the year number 
   up for two hours around the first midnight of a new century.
 */

static void merge_ddmmyy(char *ddmmyy, struct gps_data_t *out)
/* sentence supplied ddmmyy, but no century part */
{
    time_t now = time(NULL);
    struct tm *tm = localtime(&now);

    strftime(out->utc, 3, "%C", tm);
    strncpy(out->utc+2, ddmmyy + 4, 2);	/* copy year */
    out->utc[4] = '-';
    strncpy(out->utc+5, ddmmyy + 2, 2);	/* copy month */
    out->utc[7] = '-';
    strncpy(out->utc + 8, ddmmyy, 2);	/* copy date */
    out->utc[10] = 'T';
}

static void fake_mmddyyyy(struct gps_data_t *out)
/* sentence didn't sypply mm/dd/yyy, so we have to fake it */
{
    time_t now = time(NULL);
    struct tm *tm = localtime(&now);

    strftime(out->utc, sizeof(out->utc), "%Y-%m-%dT", tm);
}

static void merge_hhmmss(char *hhmmss, struct gps_data_t *out)
/* update last-fix field from a UTC time */
{
    strncpy(out->utc+11, hhmmss, 2);	/* copy hours */
    out->utc[13] = ':';
    strncpy(out->utc+14, hhmmss+2, 2);	/* copy minutes */
    out->utc[16] = ':';
    strncpy(out->utc+17 , hhmmss+4, sizeof(out->utc)-17);	/* copy seconds */
    strcat(out->utc, "Z");
}

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

static void processGPRMC(char *sentence, struct gps_data_t *out)
/* Recommend Minimum Specific GPS/TRANSIT Data */
{
    /*
        RMC,225446.33,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E,A*68
           225446.33    Time of fix 22:54:46 UTC
           A            Navigation receiver warning A = OK, V = warning
           4916.45,N    Latitude 49 deg. 16.45 min North
           12311.12,W   Longitude 123 deg. 11.12 min West
           000.5        Speed over ground, Knots
           054.7        Course Made Good, True
           191194       Date of fix  19 November 1994
           020.3,E      Magnetic variation 20.3 deg East
	   A            FAA mode indicator (NMEA 2.3 and later)
           *68          mandatory nmea_checksum

     * SiRF chipsets don't return either Mode Indicator or magnetic variation.
     */
    if (!strcmp(field(sentence, 2), "A")) {
	merge_ddmmyy(field(sentence, 9), out);
	merge_hhmmss(field(sentence, 1), out);
	do_lat_lon(sentence, 3, out);
	out->speed_stamp.changed = update_field_f(sentence, 7, &out->speed);
	REFRESH(out->speed_stamp);
	out->track_stamp.changed = update_field_f(sentence, 8, &out->track);
	REFRESH(out->track_stamp);
    }
}

static void processGPGLL(char *sentence, struct gps_data_t *out)
/* 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.
     */
    char *status = field(sentence, 7);

    if (!strcmp(field(sentence, 6), "A") && status[0] != 'N') {
	int newstatus = out->status;

	do_lat_lon(sentence, 1, out);
	fake_mmddyyyy(out);
	merge_hhmmss(field(sentence, 5), out);
	if (status[0] == 'D')
	    newstatus = STATUS_DGPS_FIX;	/* differential */
	else
	    newstatus = STATUS_FIX;
	out->status_stamp.changed = (out->status != newstatus);
	out->status = newstatus;
	REFRESH(out->status_stamp);
	gpsd_report(3, "GPGLL sets status %d\n", out->status);
    }
}

static void processGPVTG(char *sentence, struct gps_data_t *out)
/* Track Made Good and Ground Speed */
{
    /* There are two variants of GPVTG.  One looks like this:

	(1) True course over ground (degrees) 000 to 359
	(2) Magnetic course over ground 000 to 359
	(3) Speed over ground (knots) 00.0 to 99.9
	(4) Speed over ground (kilometers) 00.0 to 99.9

     * Up to and including 1.10, gpsd assumed this and extracted field 3.
     * But I'm told the NMEA spec, version 3.01, dated 1/1/2002, gives this:

	1    = Track made good
	2    = Fixed text 'T' indicates that track made good is relative to 
	       true north
	3    = Magnetic course over ground
	4    = Fixed text 'M' indicates course is relative to magnetic north.