nmea_parse.c

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

	session->gpsdata.satellites = 0;
	return ERROR_SET;
    }
    if (count % 4 != 3){
	gpsd_report(LOG_WARN, "malformed GPGSV - fieldcount %d %% 4 != 3\n", count);
	gpsd_zero_satellites(&session->gpsdata);
	session->gpsdata.satellites = 0;
	return ERROR_SET;
    }

    session->driver.nmea.await = atoi(field[1]);
    if (sscanf(field[2], "%d", &session->driver.nmea.part) < 1) {
	gpsd_zero_satellites(&session->gpsdata);
	return ERROR_SET;
    } else if (session->driver.nmea.part == 1)
	gpsd_zero_satellites(&session->gpsdata);

    for (fldnum = 4; fldnum < count; ) {
	if (session->gpsdata.satellites >= MAXCHANNELS) {
	    gpsd_report(LOG_ERROR, "internal error - too many satellites!\n");
	    gpsd_zero_satellites(&session->gpsdata);
	    break;
	}
	session->gpsdata.PRN[session->gpsdata.satellites]       = atoi(field[fldnum++]);
	session->gpsdata.elevation[session->gpsdata.satellites] = atoi(field[fldnum++]);
	session->gpsdata.azimuth[session->gpsdata.satellites]   = atoi(field[fldnum++]);
	session->gpsdata.ss[session->gpsdata.satellites]	= atoi(field[fldnum++]);
	/*
	 * Incrementing this unconditionally falls afoul of chipsets like
	 * the Motorola Oncore GT+ that emit empty fields at the end of the
	 * last sentence in a GPGSV set if the number of satellites is not
	 * a multiple of 4.
	 */
	if (session->gpsdata.PRN[session->gpsdata.satellites] != 0)
	    session->gpsdata.satellites++;
    }
    if (session->driver.nmea.part == session->driver.nmea.await && atoi(field[3]) != session->gpsdata.satellites)
	gpsd_report(LOG_WARN, "GPGSV field 3 value of %d != actual count %d\n",
		    atoi(field[3]), session->gpsdata.satellites);

    /* not valid data until we've seen a complete set of parts */
    if (session->driver.nmea.part < session->driver.nmea.await) {
	gpsd_report(LOG_PROG, "Partial satellite data (%d of %d).\n", session->driver.nmea.part, session->driver.nmea.await);
	return ERROR_SET;
    }
    /*
     * This sanity check catches an odd behavior of SiRFstarII receivers.
     * 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 < session->gpsdata.satellites; n++)
	if (session->gpsdata.azimuth[n] != 0)
	    goto sane;
    gpsd_report(LOG_WARN, "Satellite data no good (%d of %d).\n", session->driver.nmea.part, session->driver.nmea.await);
    gpsd_zero_satellites(&session->gpsdata);
    return ERROR_SET;
  sane:
    gpsd_report(LOG_PROG, "Satellite data OK (%d of %d).\n", session->driver.nmea.part, session->driver.nmea.await);
    return SATELLITE_SET;
    }

static gps_mask_t processPGRME(int c UNUSED, char *field[], struct gps_device_t *session)
/* Garmin Estimated Position Error */
{
    /*
       $PGRME,15.0,M,45.0,M,25.0,M*22
	1    = horizontal error estimate
	2    = units
	3    = vertical error estimate
	4    = units
	5    = spherical error estimate
	6    = units
     *
     * Garmin won't say, but the general belief is that these are 50% CEP.
     * We follow the advice at <http://gpsinformation.net/main/errors.htm>.
     * If this assumption changes here, it should also change in garmin.c
     * where we scale error estimates from Garmin binary packets.
     */
    if ((strcmp(field[2], "M")!=0) ||
	(strcmp(field[4], "M")!=0) ||
	(strcmp(field[6], "M")!=0)){
	    session->gpsdata.fix.eph =
	    session->gpsdata.fix.epv =
	    session->gpsdata.epe = 100;
	    return ERROR_SET;
    }

    session->gpsdata.fix.eph = atof(field[1]) * (GPSD_CONFIDENCE/CEP50_SIGMA);
    session->gpsdata.fix.epv = atof(field[3]) * (GPSD_CONFIDENCE/CEP50_SIGMA);
    session->gpsdata.epe = atof(field[5]) * (GPSD_CONFIDENCE/CEP50_SIGMA);

    return HERR_SET | VERR_SET | PERR_SET;
}

static gps_mask_t processGPZDA(int c UNUSED, char *field[], struct gps_device_t *session)
/* Time & Date */
{
    gps_mask_t mask = TIME_SET;
    /*
      $GPZDA,160012.71,11,03,2004,-1,00*7D
      1) UTC time (hours, minutes, seconds, may have fractional subsecond)
      2) Day, 01 to 31
      3) Month, 01 to 12
      4) Year (4 digits)
      5) Local zone description, 00 to +- 13 hours
      6) Local zone minutes description, apply same sign as local hours
      7) Checksum
     */
    merge_hhmmss(field[1], session);
    session->driver.nmea.date.tm_year = atoi(field[4]) - 1900;
    session->driver.nmea.date.tm_mon = atoi(field[3])-1;
    session->driver.nmea.date.tm_mday = atoi(field[2]);
    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, "GPZDA starts a reporting cycle.\n");
    }
    session->gpsdata.sentence_time = session->gpsdata. fix.time;
    return mask;
}

#ifdef TNT_ENABLE
static gps_mask_t processTNTHTM(int c UNUSED, char *field[], struct gps_device_t *session)
{
    /*
     * Proprietary sentence for True North Technologies Magnetic Compass.
     * This may also apply to some Honeywell units since they may have been
     * designed by True North.

	HTM,x.x,a,x.x,a,x.x,a,x.x,x.x*hh<cr><lf>
	Fields in order:
	1. True heading in degrees
	2. magnetometer status character:
		C = magnetometer calibration alarm
		L = low alarm
		M = low warning
		N = normal
		O = high warning
		P = high alarm
		V = magnetometer voltage level alarm
	3. pitch angle
	4. pitch status character - see field 2 above
	5. roll angle
	6. roll status character - see field 2 above
	7. dip angle
	8. relative magnitude horizontal component of earth's magnetic field
	*hh	  mandatory nmea_checksum
     */
    gps_mask_t mask;
    mask = ONLINE_SET;

    //gpsd_zero_satellites(&session->gpsdata);

    /*
     * Heading maps to track.
     * Pitch maps to climb.
     * Roll maps to speed.
     * Dip maps to altitude.
     */
    session->gpsdata.fix.time = timestamp();
    session->gpsdata.fix.track = atof(field[1]);
    session->gpsdata.headingStatus = *field[2];
    session->gpsdata.fix.climb = atof(field[3]);
    session->gpsdata.pitchStatus = *field[4];
    session->gpsdata.fix.speed = atof(field[5]);
    session->gpsdata.rollStatus = *field[6];
    session->gpsdata.fix.altitude = atof(field[7]);
    session->gpsdata.horzField = atof(field[8]);
    session->gpsdata.fix.mode = MODE_3D;
    mask |= (STATUS_SET | MODE_SET | TRACK_SET | SPEED_SET | CLIMB_SET | ALTITUDE_SET);
    session->gpsdata.status = STATUS_FIX;	/* could be DGPS_FIX */

    gpsd_report(LOG_RAW, "Heading %lf  %c.\n", session->gpsdata.fix.track, session->gpsdata.headingStatus);
    return mask;
}
#endif /* TNT_ENABLE */

#ifdef ASHTECH_ENABLE
static gps_mask_t processPASHR(int c UNUSED, char *field[], struct gps_device_t *session)
{
	gps_mask_t mask;
	mask = ONLINE_SET;

	if (0 == strcmp("RID", field[1])){ /* Receiver ID */
		(void)snprintf(session->subtype, sizeof(session->subtype)-1,
			       "%s ver %s", field[2], field[3]);
		return 0;
	} else if (0 == strcmp("POS", field[1])){ /* 3D Position */
		mask |= MODE_SET | STATUS_SET | CYCLE_START_SET;
		if (0 == strlen(field[2])){
			/* empty first field means no 3D fix is available */
			session->gpsdata.status = STATUS_NO_FIX;
			session->gpsdata.fix.mode = MODE_NO_FIX;
			return mask;
		}

		/* if we make it this far, we at least have a 3D fix */
		session->gpsdata.fix.mode = MODE_3D;
		if (1 == atoi(field[2]))
			session->gpsdata.status = STATUS_DGPS_FIX;
		else
			session->gpsdata.status = STATUS_FIX;

		session->gpsdata.satellites_used = atoi(field[3]);
		merge_hhmmss(field[4], session);
		do_lat_lon(&field[5], &session->gpsdata);
		session->gpsdata.fix.altitude = atof(field[9]);
		session->gpsdata.fix.track = atof(field[11]);
		session->gpsdata.fix.speed = atof(field[12]) / MPS_TO_KPH;
		session->gpsdata.fix.climb = atof(field[13]);
		session->gpsdata.pdop = atof(field[14]);
		session->gpsdata.hdop = atof(field[15]);
		session->gpsdata.vdop = atof(field[16]);
		session->gpsdata.tdop = atof(field[17]);
		mask |= (TIME_SET | LATLON_SET | ALTITUDE_SET);
		mask |= (SPEED_SET | TRACK_SET | CLIMB_SET);
		mask |= (PDOP_SET | HDOP_SET | VDOP_SET | TDOP_SET);
	} else if (0 == strcmp("SAT", field[1])){ /* Satellite Status */
		int i, n, p, u;
		n = session->gpsdata.satellites = atoi(field[2]);
		u = 0;
		for (i = 0; i < n; i++){
			session->gpsdata.PRN[i] = p = atoi(field[3+i*5+0]);
			session->gpsdata.azimuth[i] = atoi(field[3+i*5+1]);
			session->gpsdata.elevation[i] = atoi(field[3+i*5+2]);
			session->gpsdata.ss[i] = atoi(field[3+i*5+3]);
			if (field[3+i*5+4][0] == 'U')
				session->gpsdata.used[u++] = p;
		}
		session->gpsdata.satellites_used = u;
		mask |= SATELLITE_SET | USED_SET;
	}
	return mask;
}
#endif /* ASHTECH_ENABLE */

#ifdef __UNUSED__
static short nmea_checksum(char *sentence, unsigned char *correct_sum)
/* 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;
    if (correct_sum)
	*correct_sum = sum;
    (void)snprintf(csum, sizeof(csum), "%02X", sum);
    return(csum[0]==toupper(p[0])) && (csum[1]==toupper(p[1]));
}
#endif /* __ UNUSED__ */

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

gps_mask_t nmea_parse(char *sentence, struct gps_device_t *session)
/* parse an NMEA sentence, unpack it into a session structure */
{
    typedef gps_mask_t (*nmea_decoder)(int count, char *f[], struct gps_device_t *session);
    static struct {
	char *name;
	int nf;		/* minimum number of fields required to parse */
	nmea_decoder decoder;
    } nmea_phrase[] = {
	{"RMC", 8,	processGPRMC},
	{"GGA", 13,	processGPGGA},
	{"GLL", 7, 	processGPGLL},
	{"GSA", 17,	processGPGSA},
	{"GSV", 0,	processGPGSV},
	{"VTG", 0, 	NULL},		/* ignore Velocity Track made Good */
	{"ZDA", 7, 	processGPZDA},
	{"PGRMC", 0,	NULL},		/* ignore Garmin Sensor Config */
	{"PGRME", 7,	processPGRME},
	{"PGRMI", 0,	NULL},		/* ignore Garmin Sensor Init */
	{"PGRMO", 0,	NULL},		/* ignore Garmin Sentence Enable */
#ifdef TNT_ENABLE
	{"PTNTHTM", 9,	processTNTHTM},
#endif /* TNT_ENABLE */
#ifdef ASHTECH_ENABLE
	{"PASHR", 3,	processPASHR},	/* general handler for Ashtech */
#endif /* TNT_ENABLE */
    };
    volatile unsigned char buf[NMEA_MAX+1];

    int count;
    gps_mask_t retval = 0;
    unsigned int i;
    char *p, *field[NMEA_MAX], *s;
#ifndef USE_OLD_SPLIT
    volatile char *t;
#endif
#ifdef __UNUSED__
    unsigned char sum;

    if (!nmea_checksum(sentence+1, &sum)) {
	gpsd_report(LOG_ERROR, "Bad NMEA checksum: '%s' should be %02X\n",
		   sentence, sum);
	return 0;
    }
#endif /* __ UNUSED__ */

    /*
     * We've had reports that on the Garmin GPS-10 the device sometimes
     * (1:1000 or so) sends garbage packets that have a valid checksum
     * but are like 2 successive NMEA packets merged together in one
     * with some fields lost.  Usually these are much longer than the
     * legal limit for NMEA, so we can cope by just tossing out overlong
     * packets.  This may be a generic bug of all Garmin chipsets.
     */
    if (strlen(sentence) > NMEA_MAX) {
	gpsd_report(LOG_WARN, "Overlong packet rejected.\n");
	return ONLINE_SET;
    }

#ifdef BREAK_REGRESSIONS
    /* trim trailing CR/LF */
    for (i = 0; i < strlen(sentence); i++)
	if ((sentence[i] == '\r') || (sentence[i] == '\n')){
	    sentence[i] = '\0';
	    break;
	}
#endif
    /*@ -usedef @*//* splint 3.1.1 seems to have a bug here */
    /* make an editable copy of the sentence */
    strncpy((char *)buf, sentence, NMEA_MAX);
    /* discard the checksum part */
    for (p = (char *)buf; (*p != '*') && (*p >= ' '); ) ++p;
    *p = '\0';
    /* split sentence copy on commas, filling the field array */
#ifdef USE_OLD_SPLIT
    for (count = 0, p = (char *)buf; p != NULL && *p != '\0'; ++count, p = strchr(p, ',')) {
	*p = '\0';
	field[count] = ++p;
    }
#else
    count = 0;
    t = p;  /* end of sentence */
    p = (char *)buf + 1; /* beginning of tag, 'G' not '$' */
    /* while there is a search string and we haven't run off the buffer... */
    while((p != NULL) && (p <= t)){
	field[count] = p; /* we have a field. record it */
	/*@ -compdef @*/
	if ((p = strchr(p, ',')) != NULL){ /* search for the next delimiter */
	    *p = '\0'; /* replace it with a NUL */
	    count++; /* bump the counters and continue */
	    p++;
	}
	/*@ +compdef @*/
    }
#endif
    /* dispatch on field zero, the sentence tag */
    for (i = 0; i < (unsigned)(sizeof(nmea_phrase)/sizeof(nmea_phrase[0])); ++i) {
	s = field[0];
	if (strlen(nmea_phrase[i].name) == 3)
	    s += 2;	/* skip talker ID */
	if (strcmp(nmea_phrase[i].name, s) == 0) {
	    if (nmea_phrase[i].decoder!=NULL && (count >= nmea_phrase[i].nf)) {
		retval = (nmea_phrase[i].decoder)(count, field, session);
		strncpy(session->gpsdata.tag, nmea_phrase[i].name, MAXTAGLEN);
		session->gpsdata.sentence_length = strlen(sentence);
	    } else
		retval = ONLINE_SET;		/* unknown sentence */
	    break;
	}
    }
    /*@ +usedef @*/
    return retval;
}
#endif /* NMEA_ENABLE */

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

    if (*p == '$') {
	p++;
    } else {
	gpsd_report(LOG_ERROR, "Bad NMEA sentence: '%s'\n", sentence);
    }
    while ( ((c = *p) != '*') && (c != '\0')) {
	sum ^= c;
	p++;
    }
    *p++ = '*';
    (void)snprintf(p, 5, "%02X\r\n", (unsigned)sum);
}

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

    va_start(ap, fmt) ;
    (void)vsnprintf(buf, sizeof(buf)-5, fmt, ap);
    va_end(ap);
    if (fmt[0] == '$') {
	(void)strlcat(buf, "*", BUFSIZ);
	nmea_add_checksum(buf);
    } else
	(void)strlcat(buf, "\r\n", BUFSIZ);
    status = (int)write(fd, buf, strlen(buf));
    (void)tcdrain(fd);
    if (status == (int)strlen(buf)) {
	gpsd_report(LOG_IO, "=> GPS: %s\n", buf);
	return status;
    } else {
	gpsd_report(LOG_WARN, "=> GPS: %s FAILED\n", buf);
	return -1;
    }
}