nmea_parse.c

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

	5    = Speed over ground in knots
	6    = Fixed text 'N' indicates that speed over ground in in knots
	7    = Speed over ground in kilometers/hour
	8    = Fixed text 'K' indicates that speed over ground is in km/h.
	9    = Checksum

     * which means we want to extract field 5.  We cope with both.
     */
    out->track_stamp.changed = update_field_f(sentence, 1, &out->track);;
    REFRESH(out->track_stamp);
    if (field(sentence, 2)[0] == 'T')
	out->speed_stamp.changed = update_field_f(sentence, 5, &out->speed);
    else
	out->speed_stamp.changed = update_field_f(sentence, 3, &out->speed);
    REFRESH(out->speed_stamp);
}

static void processGPGGA(char *sentence, struct gps_data_t *out)
/* 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
           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)
    */
    out->status_stamp.changed = update_field_i(sentence, 6, &out->status);
    REFRESH(out->status_stamp);
    gpsd_report(3, "GPGGA sets status %d\n", out->status);
    if (out->status > STATUS_NO_FIX) {
	char	*altitude;

	fake_mmddyyyy(out);
	merge_hhmmss(field(sentence, 1), out);
	do_lat_lon(sentence, 2, out);
        out->satellites_used = atoi(field(sentence, 7));
	altitude = field(sentence, 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]) {
	    if (out->mode == MODE_3D) {
		out->mode = out->status ? MODE_2D : MODE_NO_FIX; 
		out->mode_stamp.changed = 1;
		REFRESH(out->mode_stamp);
	    }
	} else {
	    double newaltitude = atof(altitude);

	    out->altitude_stamp.changed = (newaltitude != out->altitude);
	    out->altitude = newaltitude;
	    REFRESH(out->altitude_stamp);
	}
    }
}

static void processGPGSA(char *sentence, struct gps_data_t *out)
/* 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
     */
    int i, changed = 0;
    
    out->mode_stamp.changed = update_field_i(sentence, 2, &out->mode);
    REFRESH(out->mode_stamp);
    gpsd_report(3, "GPGSA sets mode %d\n", out->mode);
    changed |= update_field_f(sentence, 15, &out->pdop);
    changed |= update_field_f(sentence, 16, &out->hdop);
    changed |= update_field_f(sentence, 17, &out->vdop);
    for (i = 0; i < MAXCHANNELS; i++)
	out->used[i] = 0;
    out->satellites_used = 0;
    for (i = 0; i < MAXCHANNELS; i++) {
        out->used[i] = atoi(field(sentence, i+3));
        if (out->used[i] > 0)
                out->satellites_used++;
    }
    out->fix_quality_stamp.changed = changed;
    REFRESH(out->fix_quality_stamp);
}

int nmea_sane_satellites(struct gps_data_t *out)
{
    int n;

    /* data may be incomplete */
    if (out->part < out->await)
	return 0;

    /*
     * This sanity check catches an odd behavior SiRF-II based GPSes.
     * When they can't see any satellites at all (like, inside a
     * building) they sometimes cough up a hairball in the form of a
     * GSV packet with all the azimuth entries 0 (but nonzero
     * elevations).  This behavior was observed under SiRF firmware
     * revision 231.000.000_A2.
     */
    for (n = 0; n < out->satellites; n++)
	if (out->azimuth[n])
	    return 1;
    return 0;
}

static void processGPGSV(char *sentence, struct gps_data_t *out)
/* 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 changed, lower, upper, fldnum = 4;

    out->await = atoi(field(sentence, 1));
    if (sscanf(field(sentence, 2), "%d", &out->part) < 1)
        return;

    changed = update_field_i(sentence, 3, &out->satellites);
    if (out->satellites > MAXCHANNELS) out->satellites = MAXCHANNELS;

    lower = (out->part - 1) * 4;
    upper = lower + 4;
    while (lower < out->satellites && lower < upper) {
	changed |= update_field_i(sentence, fldnum++, &out->PRN[lower]);
	changed |= update_field_i(sentence, fldnum++, &out->elevation[lower]);
	changed |= update_field_i(sentence, fldnum++, &out->azimuth[lower]);
	if (*(field(sentence, fldnum)))
	    changed |= update_field_i(sentence, fldnum, &out->ss[lower]);
	fldnum++; lower++;
    }

    /* not valid data until we've seen a complete set of parts */
    if (out->part < out->await)
	gpsd_report(3, "Partial satellite data (%d of %d).\n", out->part, out->await);
    else {
	/* trim off PRNs with spurious data attached */
	while (out->satellites
		    && !out->elevation[out->satellites-1]
		    && !out->azimuth[out->satellites-1]
		    && !out->ss[out->satellites-1])
	    out->satellites--;
	if (nmea_sane_satellites(out)) {
	    gpsd_report(3, "Satellite data OK.\n");
	    out->satellite_stamp.changed = changed;
	    REFRESH(out->satellite_stamp);
	}
	else
	    gpsd_report(3, "Satellite data no good.\n");
    }
}

static short nmea_checksum(char *sentence)
/* is the checksum on the specified sentence good? */
{
    unsigned char sum = '\0';
    char c, *p = sentence, csum[3];

    while ((c = *p++) != '*' && c != '\0')
	sum ^= c;
    sprintf(csum, "%02X", sum);
    return (strncmp(csum, p, 2) == 0);
}

/**************************************************************************
 *
 * Entry points begin here
 *
 **************************************************************************/

void nmea_add_checksum(char *sentence)
/* add NMEA to a *-terminated sentence */
{
    unsigned char sum = '\0';
    char c, *p = sentence;

    while ((c = *p++) != '*' && c != '\0')
	sum ^= c;
    sprintf(p, "%02X\r\n", sum);
}

int nmea_parse(char *sentence, struct gps_data_t *outdata)
/* parse an NMEA sentence, unpack it into a session structure */
{
    if (nmea_checksum(sentence+1)) {
	if (PREFIX("$GPRMC", sentence)) {
	    processGPRMC(sentence, outdata);
	} else if (PREFIX("$GPGGA", sentence)) {
	    processGPGGA(sentence, outdata);
	} else if (PREFIX("$GPGLL", sentence)) {
	    processGPGLL(sentence, outdata);
	} else if (PREFIX("$GPVTG", sentence)) {
	    processGPVTG(sentence, outdata);
	} else if (PREFIX("$GPGSA", sentence)) {
	    processGPGSA(sentence, outdata);
	} else if (PREFIX("$GPGSV", sentence)) {
	    processGPGSV(sentence, outdata);
	} else if (PREFIX("$PRWIZCH", sentence)) {
	    /* do nothing */;
	} else
	    return -1;
    }
    return 0;
}

void nmea_send(int fd, const char *fmt, ... )
/* ship a command to the GPS, adding * and correct checksum */
{
    char buf[BUFSIZ];
    va_list ap;

    va_start(ap, fmt) ;
#ifdef HAVE_VSNPRINTF
    vsnprintf(buf + strlen(buf), sizeof(buf)-strlen(buf), fmt, ap);
#else
    vsprintf(buf + strlen(buf), fmt, ap);
#endif
    va_end(ap);
    strcat(buf, "*");
    nmea_add_checksum(buf + 1);
    write(fd, buf, strlen(buf));
}