libgpsd_core.c

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

/* $Id: libgpsd_core.c 4664 2008-01-21 00:37:35Z garyemiller $ */
/* libgpsd_core.c -- direct access to GPSes on serial or USB devices. */
#include <sys/time.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <netdb.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>

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

#if defined(PPS_ENABLE) && defined(TIOCMIWAIT)
#ifndef S_SPLINT_S
#include <pthread.h>	/* pacifies OpenBSD's compiler */
#endif
#endif


int gpsd_switch_driver(struct gps_device_t *session, char* type_name)
{
    struct gps_type_t **dp;

    gpsd_report(LOG_PROG, "switch_driver(%s) called...\n", type_name);
    if (session->device_type != NULL && 
	strcmp(session->device_type->type_name, type_name) == 0) {
#ifdef ALLOW_RECONFIGURE
	gpsd_report(LOG_PROG, "Reconfiguring for %s...\n", session->device_type->type_name);
	if (session->enable_reconfigure 
	    	&& session->device_type->configurator != NULL)
	    session->device_type->configurator(session, 0);
#endif /* ALLOW_RECONFIGURE */
	return 0;
    }

    /*@ -compmempass @*/
    for (dp = gpsd_drivers; *dp; dp++)
	if (strcmp((*dp)->type_name, type_name) == 0) {
	    gpsd_report(LOG_PROG, "selecting %s driver...\n", (*dp)->type_name);
	    gpsd_assert_sync(session);
	    /*@i@*/session->device_type = *dp;
	    if (!session->context->readonly && session->device_type->probe_subtype != NULL)
		session->device_type->probe_subtype(session, session->packet.counter = 0);
#ifdef ALLOW_RECONFIGURE
	    if (session->enable_reconfigure 
			&& session->device_type->configurator != NULL) {
		gpsd_report(LOG_PROG, "configuring for %s...\n", session->device_type->type_name);
		session->device_type->configurator(session, 0);
	    }
#endif /* ALLOW_RECONFIGURE */
	    return 1;
	}
    gpsd_report(LOG_ERROR, "invalid GPS type \"%s\".\n", type_name);
    return 0;
    /*@ +compmempass @*/
}


void gpsd_init(struct gps_device_t *session, struct gps_context_t *context, char *device)
/* initialize GPS polling */
{
    /*@ -mayaliasunique @*/
    (void)strlcpy(session->gpsdata.gps_device, device, PATH_MAX);
    /*@ -mustfreeonly @*/
    session->device_type = NULL;	/* start by hunting packets */
    session->rtcmtime = 0;
    /*@ -temptrans @*/
    session->context = context;
    /*@ +temptrans @*/
    /*@ +mayaliasunique @*/
    /*@ +mustfreeonly @*/
    gps_clear_fix(&session->gpsdata.fix);
    session->gpsdata.set &=~ (FIX_SET | DOP_SET);
    session->gpsdata.hdop = NAN;
    session->gpsdata.vdop = NAN;
    session->gpsdata.pdop = NAN;
    session->gpsdata.tdop = NAN;
    session->gpsdata.gdop = NAN;
    session->gpsdata.epe = NAN;
    session->mag_var = NAN;

    /* tty-level initialization */
    gpsd_tty_init(session);

    /* necessary in case we start reading in the middle of a GPGSV sequence */
    gpsd_zero_satellites(&session->gpsdata);

    /* initialize things for the packet parser */
    packet_reset(&session->packet);
}

void gpsd_deactivate(struct gps_device_t *session)
/* temporarily release the GPS device */
{
#ifdef NTPSHM_ENABLE
    (void)ntpshm_free(session->context, session->shmindex);
    session->shmindex = -1;
# ifdef PPS_ENABLE
    (void)ntpshm_free(session->context, session->shmTimeP);
    session->shmTimeP = -1;
# endif /* PPS_ENABLE */
#ifdef ALLOW_RECONFIGURE
    if (session->enable_reconfigure 
	&& session->device_type != NULL
	&& session->device_type->revert != NULL) {
	session->device_type->revert(session);
	session->enable_reconfigure = false;
    }
#endif /* ALLOW_RECONFIGURE */
    if (session->device_type!=NULL) {
	if (session->back_to_nmea && session->device_type->mode_switcher!=NULL)
	    session->device_type->mode_switcher(session, 0);
	if (session->device_type->wrapup!=NULL)
	    session->device_type->wrapup(session);
    }
#endif /* NTPSHM_ENABLE */
    gpsd_report(LOG_INF, "closing GPS=%s (%d)\n", 
		session->gpsdata.gps_device, session->gpsdata.gps_fd);
    (void)gpsd_close(session);
}

#if defined(PPS_ENABLE) && defined(TIOCMIWAIT)
static void *gpsd_ppsmonitor(void *arg)
{
    struct gps_device_t *session = (struct gps_device_t *)arg;
    int cycle,duration, state = 0, laststate = -1, unchanged = 0;
    struct timeval tv;
    struct timeval pulse[2] = {{0,0},{0,0}};
    int pps_device = TIOCM_CAR;

#if defined(PPS_ON_CTS)
    pps_device = TIOCM_CTS;
#endif

    gpsd_report(LOG_PROG, "Create Thread gpsd_ppsmonitor\n");

    /* wait for status change on the device's carrier-detect line */
    while (ioctl(session->gpsdata.gps_fd, TIOCMIWAIT, pps_device) == 0) {
	(void)gettimeofday(&tv,NULL);
	/*@ +ignoresigns */
	if (ioctl(session->gpsdata.gps_fd, TIOCMGET, &state) != 0)
	    break;
	/*@ -ignoresigns */

	state = (int)((state & pps_device) != 0);

	if (state == laststate) {
	    if (++unchanged == 10) {
		gpsd_report(LOG_WARN, "TIOCMIWAIT returns unchanged state, ppsmonitor terminates\n");
		break;
	    }
	} else {
            gpsd_report(LOG_RAW, "pps-detect (%s) on %s changed to %d\n",
                        ((pps_device==TIOCM_CAR) ? "DCD" : "CTS"), 
                          session->gpsdata.gps_device, state);
	    laststate = state;
	    unchanged = 0;
	}

	/*@ +boolint @*/
	if ( session->context->fixcnt > 3 ) {
	    /* Garmin doc says PPS is valid after four good fixes. */
	    /*
	     * The PPS pulse is normally a short pulse with a frequency of
	     * 1 Hz, and the UTC second is defined by the front edge. But we
	     * don't know the polarity of the pulse (different receivers
	     * emit different polarities). The duration variable is used to
	     * determine which way the pulse is going. The code assumes
	     * that the UTC second is changing when the signal has not
	     * been changing for at least 800ms, i.e. it assumes the duty
	     * cycle is at most 20%.
	     *
	     * Some GPS instead output a square wave that is 0.5 Hz and each
	     * edge denotes the start of a second.
	     *
             * A few stupid GPS, like the Furuno GPSClock, output a 1.0 Hz 
             * square wave where the leading edge is the start of a second
	     *
	     * 5Hz GPS (Garmin 18-5Hz) pulses at 5Hz. Set the pulse length to 
	     * 40ms which gives a 160ms pulse before going high.
             *
	     */
#define timediff(x, y)	(int)((x.tv_sec-y.tv_sec)*1000000+x.tv_usec-y.tv_usec)
	    cycle = timediff(tv, pulse[state]);
	    duration = timediff(tv, pulse[state == 0]);
#undef timediff

	    if (cycle > 199000 && cycle < 201000 ) {
		/* 5Hz cycle */
		/* looks like 5hz PPS pulse */
	        if (duration > 45000)
		    (void)ntpshm_pps(session, &tv);
		gpsd_report(LOG_RAW, "5Hz PPS pulse. cycle: %d, duration: %d\n",
			cycle, duration);
	    } else if (cycle > 999000 && cycle < 1001000 ) {
		/* looks like PPS pulse or square wave */
                if (duration > 499000 && duration < 501000 
		  && session->driver.nmea.ignore_trailing_edge) {
                    /* looks like 1.0 Hz square wave, ignore trailing edge */
                    if (state == 1) {
                         (void)ntpshm_pps(session, &tv);
                    }
                } else {
                    /* looks like PPS pulse */
                    (void)ntpshm_pps(session, &tv);
		}
		gpsd_report(LOG_RAW, "PPS pulse. cycle: %d, duration: %d\n",
			cycle, duration);

	    } else if (cycle > 1999000 && cycle < 2001000) {
		/* looks like 0.5 Hz square wave */
		(void)ntpshm_pps(session, &tv);
                gpsd_report(LOG_RAW, "PPS square wave. cycle: %d, duration: %d\n",
			cycle, duration);
	    } else {
                gpsd_report(LOG_INF, "PPS pulse rejected.  cycle: %d, duration: %d\n",
			cycle, duration);
	    }
	} else {
                gpsd_report(LOG_INF, "PPS pulse rejected. No fix.\n");
	}
	/*@ -boolint @*/

	pulse[state] = tv;
    }

    return NULL;
}
#endif /* PPS_ENABLE */

/*@ -branchstate @*/
int gpsd_activate(struct gps_device_t *session, bool reconfigurable)
/* acquire a connection to the GPS device */
{
    if (gpsd_open(session) < 0)
	return -1;
    else {
#ifdef NON_NMEA_ENABLE
	struct gps_type_t **dp;

	/*@ -mustfreeonly @*/
	for (dp = gpsd_drivers; *dp; dp++) {
	    (void)tcflush(session->gpsdata.gps_fd, TCIOFLUSH);  /* toss stale data */
	    if ((*dp)->probe_detect!=NULL && (*dp)->probe_detect(session)!=0) {
		gpsd_report(LOG_PROG, "probe found %s driver...\n", (*dp)->type_name);
		session->device_type = *dp;
		gpsd_assert_sync(session);
		goto foundit;
	    }
 	}
	/*@ +mustfreeonly @*/
	gpsd_report(LOG_PROG, "no probe matched...\n");
    foundit:
#ifdef ALLOW_RECONFIGURE
	session->enable_reconfigure = reconfigurable;
#endif /* ALLOW_RECONFIGURE */
#endif /* NON_NMEA_ENABLE */
	session->gpsdata.online = timestamp();
#ifdef SIRF_ENABLE
	session->driver.sirf.satcounter = 0;
#endif /* SIRF_ENABLE */
	session->packet.char_counter = 0;
	session->packet.retry_counter = 0;
	gpsd_report(LOG_INF, "gpsd_activate(%d): opened GPS (%d)\n", reconfigurable, session->gpsdata.gps_fd);
	// session->gpsdata.online = 0;
	session->gpsdata.fix.mode = MODE_NOT_SEEN;
	session->gpsdata.status = STATUS_NO_FIX;
	session->gpsdata.fix.track = NAN;
	session->gpsdata.separation = NAN;
	session->mag_var = NAN;

	/* clear driver subtype field and private data union */
	session->subtype[0] = '\0';
	memset(&session->driver, '\0', sizeof(session->driver));
	/* if we know the device type, probe for subtype and configure it */
	if (session->device_type != NULL) {
	    if (session->device_type->probe_subtype !=NULL)
		session->device_type->probe_subtype(session, session->packet.counter = 0);
#ifdef ALLOW_RECONFIGURE
	    if (reconfigurable) {
		if (session->device_type->configurator != NULL)
		    session->device_type->configurator(session, session->packet.counter);
	    }
#endif /* ALLOW_RECONFIGURE */
	}
    }

    return session->gpsdata.gps_fd;
}
/*@ +branchstate @*/

void ntpd_link_activate(struct gps_device_t *session)
{
#if defined(PPS_ENABLE) && defined(TIOCMIWAIT)
    pthread_t pt;
#endif /* defined(PPS_ENABLE) && defined(TIOCMIWAIT) */

#ifdef NTPSHM_ENABLE
    /* If we are talking to ntpd, allocate a shared-memory segment for "NMEA" time data */
    if (session->context->enable_ntpshm)
        session->shmindex = ntpshm_alloc(session->context);

#if defined(PPS_ENABLE) && defined(TIOCMIWAIT)
    /* If we also have the 1pps capability, allocate a shared-memory segment for
     * the 1pps time data and launch a thread to capture the 1pps transitions
     */
    if (session->shmindex >= 0 && session->context->shmTimePPS)
        if ((session->shmTimeP = ntpshm_alloc(session->context)) >= 0)
            /*@i1@*/(void)pthread_create(&pt,NULL,gpsd_ppsmonitor,(void *)session);

#endif /* defined(PPS_ENABLE) && defined(TIOCMIWAIT) */
#endif /* NTPSHM_ENABLE */
}

char /*@observer@*/ *gpsd_id(/*@in@*/struct gps_device_t *session)
/* full ID of the device for reports, including subtype */
{
    static char buf[128];
    if ((session == NULL) || (session->device_type == NULL) || 
	(session->device_type->type_name == NULL))
	return "unknown,";
    (void)strlcpy(buf, session->device_type->type_name, sizeof(buf));
    if (session->subtype[0] != '\0') {
	(void)strlcat(buf, " ", sizeof(buf));
	(void)strlcat(buf, session->subtype, sizeof(buf));
    }
    return(buf);
}

#if defined(BINARY_ENABLE) || defined(RTCM_ENABLE) || defined(NTRIP_ENABLE)
/*
 * Support for generic binary drivers.  These functions dump NMEA for passing
 * to the client in raw mode.  They assume that (a) the public gps.h structure 
 * members are in a valid state, (b) that the private members hours, minutes, 
 * and seconds have also been filled in, (c) that if the private member
 * mag_var is not NAN it is a magnetic variation in degrees that should be
 * passed on, and (d) if the private member separation does not have the
 * value NAN, it is a valid WGS84 geoidal separation in 
 * meters for the fix.
 */

static double degtodm(double a)
{
    double m, t;
    m = modf(a, &t);
    t = floor(a) * 100 + m * 60;
    return t;
}

/*@ -mustdefine @*/
void gpsd_position_fix_dump(struct gps_device_t *session,
			    /*@out@*/char bufp[], size_t len)
{
    struct tm tm;
    time_t intfixtime;

    intfixtime = (time_t)session->gpsdata.fix.time;
    (void)gmtime_r(&intfixtime, &tm);
    if (session->gpsdata.fix.mode > 1) {
	(void)snprintf(bufp, len,
		"$GPGGA,%02d%02d%02d,%09.4f,%c,%010.4f,%c,%d,%02d,",
		tm.tm_hour,
		tm.tm_min,
		tm.tm_sec,
		degtodm(fabs(session->gpsdata.fix.latitude)),
		((session->gpsdata.fix.latitude > 0) ? 'N' : 'S'),
		degtodm(fabs(session->gpsdata.fix.longitude)),
		((session->gpsdata.fix.longitude > 0) ? 'E' : 'W'),
		session->gpsdata.status,
		session->gpsdata.satellites_used);
	if (isnan(session->gpsdata.hdop))
	    (void)strlcat(bufp, ",", len);
	else
	    (void)snprintf(bufp+strlen(bufp), len-strlen(bufp),
			   "%.2f,",session->gpsdata.hdop);
	if (isnan(session->gpsdata.fix.altitude))
	    (void)strlcat(bufp, ",", len);
	else
	    (void)snprintf(bufp+strlen(bufp), len-strlen(bufp), 
			   "%.2f,M,", session->gpsdata.fix.altitude);
	if (isnan(session->gpsdata.separation))
	    (void)strlcat(bufp, ",", len);
	else
	    (void)snprintf(bufp+strlen(bufp), len-strlen(bufp), 
			   "%.3f,M,", session->gpsdata.separation);
	if (isnan(session->mag_var)) 
	    (void)strlcat(bufp, ",", len);
	else {
	    (void)snprintf(bufp+strlen(bufp),
			   len-strlen(bufp),
			   "%3.2f,", fabs(session->mag_var));
	    (void)strlcat(bufp, (session->mag_var > 0) ? "E": "W", len);
	}
	nmea_add_checksum(bufp);
    }
}
/*@ +mustdefine @*/

static void gpsd_transit_fix_dump(struct gps_device_t *session,
				  char bufp[], size_t len)
{
    struct tm tm;
    time_t intfixtime;

    tm.tm_mday = tm.tm_mon = tm.tm_year = tm.tm_hour = tm.tm_min = tm.tm_sec = 0;
    if (isnan(session->gpsdata.fix.time)==0) {
	intfixtime = (time_t)session->gpsdata.fix.time;
	(void)gmtime_r(&intfixtime, &tm);
	tm.tm_mon++;
	tm.tm_year %= 100;
    }
    /*@ -usedef @*/
    (void)snprintf(bufp, len,
	    "$GPRMC,%02d%02d%02d,%c,%09.4f,%c,%010.4f,%c,%.4f,%.3f,%02d%02d%02d,,",
	    tm.tm_hour, 
	    tm.tm_min, 
	    tm.tm_sec,
	    session->gpsdata.status ? 'A' : 'V',
	    degtodm(fabs(session->gpsdata.fix.latitude)),
	    ((session->gpsdata.fix.latitude > 0) ? 'N' : 'S'),
	    degtodm(fabs(session->gpsdata.fix.longitude)),
	    ((session->gpsdata.fix.longitude > 0) ? 'E' : 'W'),
#define ZEROIZE(x)	(isnan(x)!=0 ? 0.0 : x)  
	    ZEROIZE(session->gpsdata.fix.speed * MPS_TO_KNOTS),
	    ZEROIZE(session->gpsdata.fix.track),
#undef ZEROIZE