libgpsd_core.c

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

	    tm.tm_mday,
	    tm.tm_mon,
	    tm.tm_year);
    /*@ +usedef @*/
    nmea_add_checksum(bufp);
}

static void gpsd_binary_fix_dump(struct gps_device_t *session,
				 char bufp[], size_t len)
{
    gpsd_position_fix_dump(session, bufp, len);
    gpsd_transit_fix_dump(session, bufp + strlen(bufp), len - strlen(bufp));
}

static void gpsd_binary_satellite_dump(struct gps_device_t *session, 
				char bufp[], size_t len)
{
    int i;
    char *bufp2 = bufp;
    bufp[0] = '\0';

    for( i = 0 ; i < session->gpsdata.satellites; i++ ) {
	if (i % 4 == 0) {
	    bufp += strlen(bufp);
            bufp2 = bufp;
	    len -= snprintf(bufp, len,
		    "$GPGSV,%d,%d,%02d", 
		    ((session->gpsdata.satellites-1) / 4) + 1, 
		    (i / 4) + 1,
		    session->gpsdata.satellites);
	}
	bufp += strlen(bufp);
	if (i < session->gpsdata.satellites)
	    len -= snprintf(bufp, len, 
		    ",%02d,%02d,%03d,%02d", 
		    session->gpsdata.PRN[i],
		    session->gpsdata.elevation[i], 
		    session->gpsdata.azimuth[i], 
		    session->gpsdata.ss[i]);
	if (i % 4 == 3 || i == session->gpsdata.satellites-1) {
	    nmea_add_checksum(bufp2);
	    len -= 5;
	}
    }

#ifdef ZODIAC_ENABLE
    if (session->packet.type == ZODIAC_PACKET && session->driver.zodiac.Zs[0] != 0) {
	bufp += strlen(bufp);
	bufp2 = bufp;
	(void)strlcpy(bufp, "$PRWIZCH", len);
	for (i = 0; i < ZODIAC_CHANNELS; i++) {
	    len -= snprintf(bufp+strlen(bufp), len,
			  ",%02u,%X", 
			    session->driver.zodiac.Zs[i], 
			    session->driver.zodiac.Zv[i] & 0x0f);
	}
	nmea_add_checksum(bufp2);
    }
#endif /* ZODIAC_ENABLE */
}

static void gpsd_binary_quality_dump(struct gps_device_t *session, 
			      char bufp[], size_t len)
{
    int	i, j;
    char *bufp2 = bufp;

    (void)snprintf(bufp, len-strlen(bufp),
		   "$GPGSA,%c,%d,", 'A', session->gpsdata.fix.mode);
    j = 0;
    for (i = 0; i < session->device_type->channels; i++) {
	if (session->gpsdata.used[i]) {
	    bufp += strlen(bufp);
	    (void)snprintf(bufp, len-strlen(bufp),
			   "%02d,", session->gpsdata.used[i]);
	    j++;
	}
    }
    for (i = j; i < session->device_type->channels; i++) {
	bufp += strlen(bufp);
	(void)strlcpy(bufp, ",", len);
    }
    bufp += strlen(bufp);
#define ZEROIZE(x)	(isnan(x)!=0 ? 0.0 : x)  
    if (session->gpsdata.fix.mode == MODE_NO_FIX)
	(void)strlcat(bufp, ",,,", len);
    else
	(void)snprintf(bufp, len-strlen(bufp),
		       "%.1f,%.1f,%.1f*", 
		       ZEROIZE(session->gpsdata.pdop), 
		       ZEROIZE(session->gpsdata.hdop),
		       ZEROIZE(session->gpsdata.vdop));
    nmea_add_checksum(bufp2);
    bufp += strlen(bufp);
    if (finite(session->gpsdata.fix.eph)
	|| finite(session->gpsdata.fix.epv)
	|| finite(session->gpsdata.epe)) {
        /*
	 * Output PGRME only if realistic.  Note: we're converting back to
	 * our guess about Garmin's confidence units here, make sure this
	 * stays consistent with the in-conversion in nmea_parse.c!
	 */
        (void)snprintf(bufp, len-strlen(bufp),
	    "$PGRME,%.2f,M,%.2f,M,%.2f,M",
	    ZEROIZE(session->gpsdata.fix.eph * (CEP50_SIGMA/GPSD_CONFIDENCE)), 
	    ZEROIZE(session->gpsdata.fix.epv * (CEP50_SIGMA/GPSD_CONFIDENCE)), 
	    ZEROIZE(session->gpsdata.epe * (CEP50_SIGMA/GPSD_CONFIDENCE)));
        nmea_add_checksum(bufp);
     }
#undef ZEROIZE
}

static void gpsd_binary_dump(struct gps_device_t *session, 
			      char bufp[], size_t len)
{
    if ((session->gpsdata.set & LATLON_SET) != 0)
	gpsd_binary_fix_dump(session, bufp+strlen(bufp), len-strlen(bufp));
    if ((session->gpsdata.set & (DOP_SET | ERR_SET)) != 0)
	gpsd_binary_quality_dump(session, bufp+strlen(bufp), len-strlen(bufp));
    if ((session->gpsdata.set & SATELLITE_SET) != 0)
	gpsd_binary_satellite_dump(session,bufp+strlen(bufp),len-strlen(bufp));
}

#endif /* BINARY_ENABLE */


void gpsd_error_model(struct gps_device_t *session, 
		      struct gps_fix_t *fix, struct gps_fix_t *oldfix)
/* compute errors and derived quantities */
{
    /*
     * Now we compute derived quantities.  This is where the tricky error-
     * modeling stuff goes. Presently we don't know how to derive 
     * time error.
     *
     * Some drivers set the position-error fields.  Only the Zodiacs 
     * report speed error.  Nobody reports track error or climb error.
     */
#define UERE_NO_DGPS	8.0	/* meters, 95% confidence */
#define UERE_WITH_DGPS	2.0	/* meters, 95% confidence */
    double uere;

    if (NULL == session)
	return;

    uere = (session->gpsdata.status == STATUS_DGPS_FIX ? UERE_WITH_DGPS : UERE_NO_DGPS);

    /*
     * Field reports match the theoretical prediction that
     * expected time error should be half the resolution of
     * the GPS clock, so we put the bound of the error
     * in as a constant pending getting it from each driver.
     */
    if (isnan(fix->ept)!=0)
	fix->ept = 0.005;
    /* Other error computations depend on having a valid fix */
    if (fix->mode >= MODE_2D) {
	if (isnan(fix->eph)!=0 && finite(session->gpsdata.hdop)!=0)
		fix->eph = session->gpsdata.hdop * uere;

	if ((fix->mode >= MODE_3D)
		&& isnan(fix->epv)!=0 && finite(session->gpsdata.vdop)!=0)
	    fix->epv = session->gpsdata.vdop * uere;

	if (isnan(session->gpsdata.epe)!=0 && finite(session->gpsdata.pdop)!=0)
	    session->gpsdata.epe = session->gpsdata.pdop * uere;
	else
	    session->gpsdata.epe = NAN;

	/*
	 * If we have a current fix and an old fix, and the packet handler 
	 * didn't set the speed error and climb error members itself, 
	 * try to compute them now.
	 */
	if (isnan(fix->eps)!=0)
	{
	    if (oldfix->mode > MODE_NO_FIX && fix->mode > MODE_NO_FIX
			&& isnan(oldfix->eph)==0 && isnan(oldfix->eph)==0 
			&& isnan(oldfix->time)==0 && isnan(oldfix->time)==0 
			&& fix->time > oldfix->time) {
		double t = fix->time-oldfix->time;
		double e = oldfix->eph + fix->eph;
		fix->eps = e/t;
	    } else
		fix->eps = NAN;
	}
	if ((fix->mode >= MODE_3D) 
	    	&& isnan(fix->epc)!=0 && fix->time > oldfix->time) {
	    if (oldfix->mode > MODE_3D && fix->mode > MODE_3D) {
		double t = fix->time-oldfix->time;
		double e = oldfix->epv + fix->epv;
		/* if vertical uncertainties are zero this will be too */
		fix->epc = e/t;
	    }
	    /*
	     * We compute a track error estimate solely from the
	     * position of this fix and the last one.  The maximum
	     * track error, as seen from the position of last fix, is
	     * the angle subtended by the two most extreme possible
	     * error positions of the current fix; the expected track
	     * error is half that.  Let the position of the old fix be
	     * A and of the new fix B.  We model the view from A as
	     * two right triangles ABC and ABD with BC and BD both
	     * having the length of the new fix's estimated error.
	     * adj = len(AB), opp = len(BC) = len(BD), hyp = len(AC) =
	     * len(AD). This leads to spurious uncertainties
	     * near 180 when we're moving slowly; to avoid reporting
	     * garbage, throw back NaN if the distance from the previous
	     * fix is less than the error estimate.
	     */
	    fix->epd = NAN;
	    if (oldfix->mode >= MODE_2D) {
		double adj = earth_distance(
		    oldfix->latitude, oldfix->longitude, 
		    fix->latitude, fix->longitude);
		if (isnan(adj)==0 && adj > fix->eph) {
		    double opp = fix->eph;
		    double hyp = sqrt(adj*adj + opp*opp);
		    fix->epd = RAD_2_DEG * 2 * asin(opp / hyp);
		}
	    }
	}
    }

    /* save old fix for later error computations */
    /*@ -mayaliasunique @*/
    if (fix->mode >= MODE_2D)
	(void)memcpy(oldfix, fix, sizeof(struct gps_fix_t));
    /*@ +mayaliasunique @*/
}

gps_mask_t gpsd_poll(struct gps_device_t *session)
/* update the stuff in the scoreboard structure */
{
    ssize_t newlen;

    gps_clear_fix(&session->gpsdata.fix);

    if (session->packet.inbuflen==0)
	session->gpsdata.d_xmit_time = timestamp();

    /* can we get a full packet from the device? */
    if (session->device_type) {
	newlen = session->device_type->get_packet(session);
	session->gpsdata.d_xmit_time = timestamp();
	if (session->packet.outbuflen>0 && session->device_type->probe_subtype!=NULL)
	    session->device_type->probe_subtype(session, ++session->packet.counter);
    } else {
	newlen = generic_get(session);
	session->gpsdata.d_xmit_time = timestamp();
	gpsd_report(LOG_RAW, 
		    "packet sniff finds type %d\n", 
		    session->packet.type);
	if (session->packet.type != BAD_PACKET) {
	    switch (session->packet.type) {
	    case COMMENT_PACKET:
		break;
#ifdef SIRF_ENABLE
	    case SIRF_PACKET:
		(void)gpsd_switch_driver(session, "SiRF binary");
		break;
#endif /* SIRF_ENABLE */
#ifdef TSIP_ENABLE
	    case TSIP_PACKET:
		(void)gpsd_switch_driver(session, "Trimble TSIP");
		break;
#endif /* TSIP_ENABLE */
#ifdef GARMIN_ENABLE
	    case GARMIN_PACKET:
		(void)gpsd_switch_driver(session, "Garmin Serial binary");
		break;
#endif /* GARMIN_ENABLE */
#ifdef NMEA_ENABLE
	    case NMEA_PACKET:
		(void)gpsd_switch_driver(session, "Generic NMEA");
		break;
#endif /* NMEA_ENABLE */
#ifdef ZODIAC_ENABLE
	    case ZODIAC_PACKET:
		(void)gpsd_switch_driver(session, "Zodiac binary");
		break;
#endif /* ZODIAC_ENABLE */
#ifdef UBX_ENABLE
	    case UBX_PACKET:
		(void)gpsd_switch_driver(session, "uBlox UBX");
		break;
#endif /* UBX_ENABLE */
#ifdef NAVCOM_ENABLE
 	    case NAVCOM_PACKET:
         	(void)gpsd_switch_driver(session, "Navcom binary");
         	break;
#endif /* NAVCOM_ENABLE */
#ifdef EVERMORE_ENABLE
	    case EVERMORE_PACKET:
		(void)gpsd_switch_driver(session, "EverMore binary");
		break;
#endif /* EVERMORE_ENABLE */
#ifdef ITRAX_ENABLE
	    case ITALK_PACKET:
		(void)gpsd_switch_driver(session, "iTalk binary");
		break;
#endif /* ITRAX_ENABLE */
#ifdef RTCM104_ENABLE
	    case RTCM_PACKET:
		(void)gpsd_switch_driver(session, "RTCM104");
		break;
#endif /* RTCM104_ENABLE */
	    }
	} else if (!gpsd_next_hunt_setting(session))
	    return ERROR_SET;
    }

    /* update the scoreboard structure from the GPS */
    gpsd_report(LOG_RAW+2, "GPS sent %d new characters\n", newlen);
    if (newlen == -1)	{		/* read error */
	session->gpsdata.online = 0;
	return 0;
    } else if (newlen == 0) {		/* no new data */
	if (session->device_type != NULL && timestamp()>session->gpsdata.online+session->device_type->cycle+1){
		gpsd_report(LOG_PROG, "GPS is offline (%lf sec since data)\n", 
			timestamp() - session->gpsdata.online);
	    session->gpsdata.online = 0;
	    return 0;
	} else
	    return ONLINE_SET;
    } else if (session->packet.outbuflen == 0) {   /* got new data, but no packet */
	    gpsd_report(LOG_RAW+3, "New data, not yet a packet\n");
	    return ONLINE_SET;
    } else {
	gps_mask_t received, dopmask = 0;
	session->gpsdata.online = timestamp();

	/*@ -nullstate @*/
	if (session->gpsdata.raw_hook)
	    session->gpsdata.raw_hook(&session->gpsdata, 
				      (char *)session->packet.outbuffer,
				      (size_t)session->packet.outbuflen, 2);
	/*@ -nullstate @*/
	session->gpsdata.sentence_length = session->packet.outbuflen;
	session->gpsdata.d_recv_time = timestamp();

	/* Get data from current packet into the fix structure */
	received = 0;
	if (session->packet.type != COMMENT_PACKET)
	    if (session->device_type != NULL && session->device_type->parse_packet!=NULL)
		received = session->device_type->parse_packet(session);

	/*
	 * Compute fix-quality data from the satellite positions.
	 * These will not overwrite DOPs reported from the packet we just got.
	 */
	if (session->gpsdata.fix.mode > MODE_NO_FIX 
		    && (session->gpsdata.set & SATELLITE_SET) != 0
		    && session->gpsdata.satellites > 0) {
	    dopmask = dop(&session->gpsdata);
	    session->gpsdata.epe = NAN;
	}
	session->gpsdata.set = ONLINE_SET | dopmask | received;

	/*
	 * Count good fixes. We used to check 
	 *	session->gpsdata.status > STATUS_NO_FIX
	 * here, but that wasn't quite right.  That tells us whether
	 * we think we have a valid fix for the current cycle, but remains
	 * true while following non-fix packets are received.  What we
	 * really want to know is whether the last packet received was a
	 * fix package AND held a valid fix. We must ignore non-fix packages
	 * AND packages which have fix data but are flagged as invalid. Some
	 * devices output fix packets on a regular basis, even when unable
	 * to derive a good fix. Such packets should set STATUS_NO_FIX.
	 */
	if ((session->gpsdata.set & LATLON_SET )!=0 && session->gpsdata.status > STATUS_NO_FIX)
	    session->context->fixcnt++;

	session->gpsdata.d_decode_time = timestamp();

	/* also copy the sentence up to clients in raw mode */
	if (session->packet.type == NMEA_PACKET) {
	    if (session->gpsdata.raw_hook)
		session->gpsdata.raw_hook(&session->gpsdata,
					  (char *)session->packet.outbuffer,
					  strlen((char *)session->packet.outbuffer),
					  1);
	} else {
	    char buf2[MAX_PACKET_LENGTH*3+2];

	    buf2[0] = '\0';
#ifdef RTCM104_ENABLE
	    if ((session->gpsdata.set & RTCM_SET) != 0)
		rtcm_dump(&session->gpsdata.rtcm, 
			  buf2+strlen(buf2), 
			  (sizeof(buf2)-strlen(buf2)));
	    else {
#endif /* RTCM104_ENABLE */
#ifdef BINARY_ENABLE
		gpsd_binary_dump(session, buf2, sizeof(buf2));
#endif /* BINARY_ENABLE */
#ifdef RTCM104_ENABLE
	    }
#endif /* RTCM104_ENABLE */
	    if (buf2[0] != '\0') {
		gpsd_report(LOG_IO, "<= GPS: %s", buf2);
		if (session->gpsdata.raw_hook)
		    session->gpsdata.raw_hook(&session->gpsdata, 
					      buf2, strlen(buf2), 1);
	    }
	}

	if (session->gpsdata.fix.mode == MODE_3D)
	    dgnss_report(session);

	return session->gpsdata.set;
    }
}

void gpsd_wrap(struct gps_device_t *session)
/* end-of-session wrapup */
{
    if (session->gpsdata.gps_fd != -1)
	gpsd_deactivate(session);
}

void gpsd_zero_satellites(/*@out@*/struct gps_data_t *out)
{
    (void)memset(out->PRN,       0, sizeof(out->PRN));
    (void)memset(out->elevation, 0, sizeof(out->elevation));
    (void)memset(out->azimuth,   0, sizeof(out->azimuth));
    (void)memset(out->ss,        0, sizeof(out->ss));
    out->satellites = 0;
}