gpsd.c

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

    int fd;

    for (fd = 0; fd < nfds; fd++)
	if (FD_ISSET(fd, &watcher_fds))
	    throttled_write(fd, sentence, strlen(sentence));
}

static void raw_hook(char *sentence)
/* hook to be executed on each incoming sentence */
{
    int fd;

    for (fd = 0; fd < nfds; fd++) {
	/* copy raw NMEA sentences from GPS to clients in raw mode */
	if (FD_ISSET(fd, &nmea_fds))
	    throttled_write(fd, sentence, strlen(sentence));

	/* some listeners may be in watcher mode */
	if (FD_ISSET(fd, &watcher_fds)) {
#define PUBLISH(fd, cmds)	handle_request(fd, cmds, sizeof(cmds)-1)
	    if (PREFIX("$GPRMC", sentence)) {
		PUBLISH(fd, "pdtvs");
	    } else if (PREFIX("$GPGGA", sentence)) {
		PUBLISH(fd, "pdas");	
	    } else if (PREFIX("$GPGLL", sentence)) {
		PUBLISH(fd, "pd");
	    } else if (PREFIX("$GPVTG", sentence)) {
		PUBLISH(fd, "tv");
	    } else if (PREFIX("$GPGSA", sentence)) {
		PUBLISH(fd, "qm");
	    } else if (PREFIX("$GPGSV", sentence)) {
		if (nmea_sane_satellites(&session->gNMEAdata))
		    PUBLISH(fd, "y");
	    }
#undef PUBLISH
	}
    }
}

static int passivesock(char *service, char *protocol, int qlen)
{
    struct servent *pse;
    struct protoent *ppe;
    struct sockaddr_in sin;
    int s, type, one = 1;

    memset((char *) &sin, '\0', sizeof(sin));
    sin.sin_family = AF_INET;
    sin.sin_addr.s_addr = INADDR_ANY;

    if ( (pse = getservbyname(service, protocol)) )
	sin.sin_port = htons(ntohs((u_short) pse->s_port));
    else if ((sin.sin_port = htons((u_short) atoi(service))) == 0) {
	gpsd_report(0, "Can't get \"%s\" service entry.\n", service);
	return -1;
    }
    if ((ppe = getprotobyname(protocol)) == 0) {
	gpsd_report(0, "Can't get \"%s\" protocol entry.\n", protocol);
	return -1;
    }
    if (strcmp(protocol, "udp") == 0)
	type = SOCK_DGRAM;
    else
	type = SOCK_STREAM;
    if ((s = socket(PF_INET, type, ppe->p_proto)) < 0) {
	gpsd_report(0, "Can't create socket\n");
	return -1;
    }
    if (setsockopt(s,SOL_SOCKET,SO_REUSEADDR,(char *)&one,sizeof(one)) == -1) {
	gpsd_report(0, "Error: SETSOCKOPT SO_REUSEADDR\n");
	return -1;
    }
    if (bind(s, (struct sockaddr *) &sin, sizeof(sin)) < 0) {
	gpsd_report(0, "Can't bind to port %s\n", service);
	return -1;
    }
    if (type == SOCK_STREAM && listen(s, qlen) < 0) {
	gpsd_report(0, "Can't listen on %s port%s\n", service);
	return -1;
    }
    return s;
}

int main(int argc, char *argv[])
{
    char *service = NULL, *dgpsserver = NULL;
    struct sockaddr_in fsin;
    fd_set rfds;
    int option, msock, fd, need_gps, nowait = 0, gpsd_speed = 0;
    extern char *optarg;
    char gpstype = 'n';

    debuglevel = 1;
    while ((option = getopt(argc, argv, "D:S:d:hnp:s:"
#if TRIPMATE_ENABLE || defined(ZODIAC_ENABLE)
			    "i:"
#endif /* TRIPMATE_ENABLE || defined(ZODIAC_ENABLE) */
#ifdef NON_NMEA_ENABLE
			    "T:"
#endif /* NON_NMEA_ENABLE */
		)) != -1) {
	switch (option) {
#ifdef NON_NMEA_ENABLE
	case 'T':
	    gpstype = *optarg;
	    break;
#endif /* NON_NMEA_ENABLE */
	case 'D':
	    debuglevel = (int) strtol(optarg, 0, 0);
	    break;
	case 'S':
	    service = optarg;
	    break;
	case 'd':
	    dgpsserver = optarg;
	    break;
#if TRIPMATE_ENABLE || defined(ZODIAC_ENABLE)
	case 'i': {
	    char *colon;
	    if (!(colon = strchr(optarg, ':')) || colon == optarg)
		fprintf(stderr, 
			"gpsd: required format is latitude:longitude.\n");
	    else if (!strchr("NSns", colon[-1]))
		fprintf(stderr,
			"gpsd: latitude field is invalid; must end in N or S.\n");
	    else if (!strchr("EWew", optarg[strlen(optarg)-1]))
		fprintf(stderr,
			"gpsd: longitude field is invalid; must end in E or W.\n");
	   else {
		*colon = '\0';
		session->latitude = optarg;
 		session->latd = toupper(optarg[strlen(session->latitude) - 1]);
		session->latitude[strlen(session->latitude) - 1] = '\0';
		session->longitude = colon+1;
		session->lond = toupper(session->longitude[strlen(session->longitude)-1]);
		session->longitude[strlen(session->longitude)-1] = '\0';
	    }
	    break;
	}
#endif /* TRIPMATE_ENABLE || defined(ZODIAC_ENABLE) */
	case 'n':
	    nowait = 1;
	    break;
	case 'p':
	    device_name = optarg;
	    break;
	case 's':
	    gpsd_speed = atoi(optarg);
	    break;
	case 'h': case '?':
	default:
	    usage();
	    exit(0);
	}
    }

    if (!service)
	service = getservbyname("gpsd", "tcp") ? "gpsd" : DEFAULT_GPSD_PORT;

    if (debuglevel < 2)
	daemonize();

    /* Handle some signals */
    signal(SIGHUP, restart);
    signal(SIGINT, onsig);
    signal(SIGTERM, onsig);
    signal(SIGQUIT, onsig);
    signal(SIGPIPE, SIG_IGN);

    openlog("gpsd", LOG_PID, LOG_USER);
    gpsd_report(1, "launching (Version %s)\n", VERSION);
    if ((msock = passivesock(service, "tcp", QLEN)) < 0) {
	gpsd_report(0, "startup failed, netlib error %d\n", msock);
	exit(2);
    }
    gpsd_report(1, "listening on port %s\n", service);

    /* user may want to re-initialize the session */
    if (setjmp(restartbuf) == THROW_SIGHUP) {
	gpsd_wrap(session);
	gpsd_report(1, "gpsd restarted by SIGHUP\n");
    }

    FD_ZERO(&all_fds); FD_ZERO(&nmea_fds); FD_ZERO(&watcher_fds);
    FD_SET(msock, &all_fds);
    nfds = getdtablesize();

    session = gpsd_init(gpstype, dgpsserver);
    if (gpsd_speed)
	session->baudrate = gpsd_speed;
    session->gpsd_device = device_name;
    session->gNMEAdata.raw_hook = raw_hook;
    if (session->dsock >= 0)
	FD_SET(session->dsock, &all_fds);
    if (nowait) {
	if (gpsd_activate(session) < 0) {
	    gpsd_report(0, "exiting - GPS device nonexistent or can't be read\n");
	    exit(2);
	}
	FD_SET(session->gNMEAdata.gps_fd, &all_fds);
    }

    while (1) {
	struct timeval tv;

        memcpy((char *)&rfds, (char *)&all_fds, sizeof(rfds));

	/* 
	 * Poll for user commands or GPS data.  The timeout doesn't
	 * actually matter here since select returns whenever one of
	 * the file descriptors in the set goes ready. 
	 */
	tv.tv_sec = 1; tv.tv_usec = 0;
	if (select(nfds, &rfds, NULL, NULL, &tv) < 0) {
	    if (errno == EINTR)
		continue;
	    gpsd_report(0, "select: %s\n", strerror(errno));
	    exit(2);
	}

	/* always be open to new connections */
	if (FD_ISSET(msock, &rfds)) {
	    int alen = sizeof(fsin);
	    int ssock = accept(msock, (struct sockaddr *) &fsin, &alen);

	    if (ssock < 0)
		gpsd_report(0, "accept: %s\n", strerror(errno));
	    else {
		int opts = fcntl(ssock, F_GETFL);

		if (opts >= 0)
		    fcntl(ssock, F_SETFL, opts | O_NONBLOCK);
		gpsd_report(3, "client connect on %d\n", ssock);
		FD_SET(ssock, &all_fds);
	    }
	    FD_CLR(msock, &rfds);
	}

	/* we may need to force the GPS open */
	if (nowait && session->gNMEAdata.gps_fd == -1) {
	    gpsd_deactivate(session);
	    if (gpsd_activate(session) >= 0) {
		notify_watchers("GPSD,X=1\r\n");
		FD_SET(session->gNMEAdata.gps_fd, &all_fds);
	    }
	}

	/* get data from it */
	if (session->gNMEAdata.gps_fd >= 0 && gpsd_poll(session) < 0) {
	    gpsd_report(3, "GPS is offline\n");
	    FD_CLR(session->gNMEAdata.gps_fd, &all_fds);
	    gpsd_deactivate(session);
	    notify_watchers("GPSD,X=0\r\n");
	}

	/* this simplifies a later test */
	if (session->dsock > -1)
	    FD_CLR(session->dsock, &rfds);

	/* accept and execute commands for all clients */
	need_gps = 0;
	for (fd = 0; fd < getdtablesize(); fd++) {
	    if (fd == msock || fd == session->gNMEAdata.gps_fd)
		continue;
	    /*
	     * GPS must be opened if commands are waiting or any client is
	     * streaming (raw or watcher mode).
	     */
	    if (FD_ISSET(fd, &rfds) || FD_ISSET(fd, &nmea_fds) || FD_ISSET(fd, &watcher_fds)) {
		char buf[BUFSIZE];
		int buflen;

		if (session->gNMEAdata.gps_fd == -1) {
		    gpsd_deactivate(session);
		    if (gpsd_activate(session) >= 0) {
			notify_watchers("GPSD,X=1\r\n");
			FD_SET(session->gNMEAdata.gps_fd, &all_fds);
		    }
		}

		if (FD_ISSET(fd, &rfds)) {
		    buflen = read(fd, buf, sizeof(buf) - 1);
		    if (buflen <= 0) {
			(void) close(fd);
			FD_CLR(fd, &all_fds);
		    }
		    buf[buflen] = '\0';
		    gpsd_report(1, "<= client: %s", buf);
		    if (handle_request(fd, buf, buflen) < 0) {
			(void) close(fd);
			FD_CLR(fd, &all_fds);
		    }
		}
	    }
	    if (fd != session->gNMEAdata.gps_fd && fd != msock && FD_ISSET(fd, &all_fds))
		need_gps++;
	}

	if (!nowait && !need_gps && session->gNMEAdata.gps_fd != -1) {
	    FD_CLR(session->gNMEAdata.gps_fd, &all_fds);
	    session->gNMEAdata.gps_fd = -1;
	    gpsd_deactivate(session);
	}
    }

    gpsd_wrap(session);
}