gpsd.c

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

#include "config.h"
#include <unistd.h>
#include <stdlib.h>
#include <syslog.h>
#include <signal.h>
#include <errno.h>
#include <ctype.h>
#include <fcntl.h>
#include <string.h>
#include <netdb.h>
#include <stdarg.h>
#include <setjmp.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>

#if defined (HAVE_PATH_H)
#include <paths.h>
#else
#if !defined (_PATH_DEVNULL)
#define _PATH_DEVNULL    "/dev/null"
#endif
#endif

#if defined (HAVE_SYS_SELECT_H)
#include <sys/select.h>
#endif
#if defined(HAVE_SYS_TIME_H)
#include <sys/time.h>
#endif

#include "gpsd.h"

#define QLEN			5

struct gps_session_t *session;
static char *device_name = DEFAULT_DEVICE_NAME;
static fd_set all_fds, nmea_fds, watcher_fds;
static int debuglevel, nfds, in_background = 0;

static jmp_buf	restartbuf;
#define THROW_SIGHUP	1

static void restart(int sig)
{
    longjmp(restartbuf, THROW_SIGHUP);
}

static void onsig(int sig)
{
    gpsd_wrap(session);
    gpsd_report(1, "Received signal %d. Exiting...\n", sig);
    exit(10 + sig);
}

static int daemonize(void)
{
    int fd;
    pid_t pid;

    switch (pid = fork()) {
    case -1:
	return -1;
    case 0:
	break;
    default:
	_exit(pid);
    }

    if (setsid() == -1)
	return -1;
    chdir("/");
    if ((fd = open(_PATH_DEVNULL, O_RDWR, 0)) != -1) {
	dup2(fd, STDIN_FILENO);
	dup2(fd, STDOUT_FILENO);
	dup2(fd, STDERR_FILENO);
	if (fd > 2)
	    close(fd);
    }
    in_background = 1;
    return 0;
}

void gpsd_report(int errlevel, const char *fmt, ... )
/* assemble command in printf(3) style, use stderr or syslog */
{
    char buf[BUFSIZ];
    va_list ap;

    strcpy(buf, "gpsd: ");
    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);

    if (errlevel > debuglevel)
	return;

    if (in_background)
	syslog((errlevel == 0) ? LOG_ERR : LOG_NOTICE, buf);
    else
	fputs(buf, stderr);
}

static void usage(void)
{
    printf("usage:  gpsd [options] \n\
  Options include: \n\
  -p string (default %s)   = set GPS device name \n"
#ifdef NON_NMEA_ENABLE
"  -T devtype (default 'n')       = set GPS device type \n"
#endif /* NON_NMEA_ENABLE */
"  -S integer (default %4s)      = set port for daemon \n"
#ifdef TRIPMATE_ENABLE
"  -i %%f[NS]:%%f[EW]               = set initial latitude/longitude \n"
#endif /* TRIPMATE_ENABLE */
"  -s baud_rate                   = set baud rate on gps device \n\
  -d host[:port]                 = set DGPS server \n\
  -D integer (default 0)         = set debug level \n\
  -h                             = help message \n",
	   DEFAULT_DEVICE_NAME, DEFAULT_GPSD_PORT);
#ifdef NON_NMEA_ENABLE
    {
    struct gps_type_t **dp;
    printf("Here are the available driver types:\n"); 
    for (dp = gpsd_drivers; *dp; dp++)
	if ((*dp)->typekey)
	    printf("   %c -- %s\n", (*dp)->typekey, (*dp)->typename);
    }
#else
    printf("This gpsd was compiled with support for NMEA only.\n");
#endif /* NON_NMEA_ENABLE */
}

static int throttled_write(int fd, char *buf, int len)
/* write to client -- throttle if it's gone or we're close to buffer overrun */
{
    int status;

    gpsd_report(3, "=> client(%d): %s", fd, buf);
    if ((status = write(fd, buf, len)) > -1)
	return status;
    if (errno == EBADF)
	gpsd_report(3, "Client on %d has vanished.\n", fd);
    else if (errno == EWOULDBLOCK)
	gpsd_report(3, "Dropped client on %d to avoid overrun.\n", fd);
    else
	gpsd_report(3, "Client write to %d: %s\n", fd, strerror(errno));
    FD_CLR(fd, &all_fds); FD_CLR(fd, &nmea_fds); FD_CLR(fd, &watcher_fds);
    return status;
}

static int validate(int fd)
{
#define VALIDATION_COMPLAINT(level, legend) \
        gpsd_report(level, legend " (status=%d, mode=%d).\r\n", \
		    session->gNMEAdata.status, session->gNMEAdata.mode)
    if ((session->gNMEAdata.status == STATUS_NO_FIX) != (session->gNMEAdata.mode == MODE_NO_FIX)) {
	VALIDATION_COMPLAINT(3, "GPS is confused about whether it has a fix");
	return 0;
    }
    else if (session->gNMEAdata.status > STATUS_NO_FIX && session->gNMEAdata.mode > MODE_NO_FIX) {
	VALIDATION_COMPLAINT(3, "GPS has a fix");
	return session->gNMEAdata.mode;
    }
    VALIDATION_COMPLAINT(3, "GPS has no fix");
    return 0;
#undef VALIDATION_CONSTRAINT
}

static int handle_request(int fd, char *buf, int buflen)
/* interpret a client request; fd is the socket back to the client */
{
    char reply[BUFSIZE], *p;
    int i, j;
    struct gps_data_t *ud = &session->gNMEAdata;

    sprintf(reply, "GPSD");
    p = buf;
    while (*p) {
	switch (toupper(*p++)) {
	case 'A':
	    if (!validate(fd))
		strcat(reply, ",A=?");
	    else
		sprintf(reply + strlen(reply), ",A=%f", ud->altitude);
	    break;
	case 'D':
	    if (ud->utc[0])
		sprintf(reply + strlen(reply), ",D=%s", ud->utc);
	    else
		strcat(reply, ",D=?");
	    break;
	case 'L':
	    sprintf(reply + strlen(reply), ",l=1 " VERSION " admpqrstvwxy");
	    break;
	case 'M':
	    sprintf(reply + strlen(reply), ",M=%d", ud->mode);
	    break;
	case 'P':
	    if (!validate(fd))
		strcat(reply, ",P=?");
	    else
		sprintf(reply + strlen(reply), ",P=%f %f", 
			ud->latitude, ud->longitude);
	    break;
	case 'Q':
	    if (!validate(fd))
		strcat(reply, ",Q=?");
	    else
		sprintf(reply + strlen(reply), ",Q=%d %f %f %f",
			ud->satellites_used, ud->pdop, ud->hdop, ud->vdop);
	    break;
	case 'R':
	    if (*p == '1' || *p == '+') {
		FD_SET(fd, &nmea_fds);
		gpsd_report(3, "%d turned on raw mode\n", fd);
		sprintf(reply + strlen(reply), ",R=1");
		p++;
	    } else if (*p == '0' || *p == '-') {
		FD_CLR(fd, &nmea_fds);
		gpsd_report(3, "%d turned off raw mode\n", fd);
		sprintf(reply + strlen(reply), ",R=0");
		p++;
	    } else if (FD_ISSET(fd, &nmea_fds)) {
		FD_CLR(fd, &nmea_fds);
		gpsd_report(3, "%d turned off raw mode\n", fd);
		sprintf(reply + strlen(reply), ",R=0");
	    } else {
		FD_SET(fd, &nmea_fds);
		gpsd_report(3, "%d turned on raw mode\n", fd);
		sprintf(reply + strlen(reply), ",R=1");
	    }
	    break;
	case 'S':
	    sprintf(reply + strlen(reply), ",S=%d", ud->status);
	    break;
	case 'T':
	    if (!validate(fd))
		strcat(reply, ",T=?");
	    else
		sprintf(reply + strlen(reply), ",T=%f", ud->track);
	    break;
	case 'V':
	    if (!validate(fd))
		strcat(reply, ",V=?");
	    else
		sprintf(reply + strlen(reply), ",V=%f", ud->speed);
	    break;
	case 'W':
	    if (*p == '1' || *p == '+') {
		FD_SET(fd, &watcher_fds);
		gpsd_report(3, "%d turned on watching\n", fd);
		sprintf(reply + strlen(reply), ",W=1");
		p++;
	    } else if (*p == '0' || *p == '-') {
		FD_CLR(fd, &watcher_fds);
		gpsd_report(3, "%d turned off watching\n", fd);
		sprintf(reply + strlen(reply), ",W=0");
		p++;
	    } else if (FD_ISSET(fd, &watcher_fds)) {
		FD_CLR(fd, &watcher_fds);
		gpsd_report(3, "%d turned off watching\n", fd);
		sprintf(reply + strlen(reply), ",W=0");
	    } else {
		FD_SET(fd, &watcher_fds);
		gpsd_report(3, "%d turned on watching\n", fd);
		sprintf(reply + strlen(reply), ",W=1");
	    }
	    break;
        case 'X':
	    sprintf(reply + strlen(reply), ",X=%d", ud->gps_fd != -1);
	    break;
	case 'Y':
	    if (!ud->satellites)
		strcat(reply, ",Y=?");
	    else {
		int used;
		sprintf(reply + strlen(reply), ",Y=%d:", ud->satellites);
		if (SEEN(ud->satellite_stamp))
		    for (i = 0; i < ud->satellites; i++) {
			used = 0;
			for (j = 0; j < ud->satellites_used; j++)
			    if (ud->used[j] == ud->PRN[i]) {
				used = 1;
				break;
			    }
			if (ud->PRN[i])
			    sprintf(reply + strlen(reply), "%d %d %d %d %d:", 
				    ud->PRN[i], 
				    ud->elevation[i],ud->azimuth[i],
				    ud->ss[i],
				    used);
		    }
		}
	    break;
	case '\r': case '\n':
	    goto breakout;
	}
    }
 breakout:
    strcat(reply, "\r\n");

    return throttled_write(fd, reply, strlen(reply));
}

static void notify_watchers(char *sentence)
/* notify all watching clients of an event */
{