sirfmon.c

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

    (void)wrefresh(mid9win);
    (void)wrefresh(mid13win);
    (void)wrefresh(mid27win);
}
/*@ +nullpass +globstate @*/

static void command(char buf[], size_t len, const char *fmt, ... )
/* assemble command in printf(3) style, use stderr or syslog */
{
    va_list ap;
    ssize_t n;

    va_start(ap, fmt) ;
    (void)vsnprintf(buf, len, fmt, ap);
    va_end(ap);

    (void)write(devicefd, buf, strlen(buf));
    n = read(devicefd, buf, len);
    if (n >= 0) {
	buf[n] = '\0';
	while (isspace(buf[strlen(buf)-1]))
	    buf[strlen(buf)-1] = '\0';
    }
}

int main (int argc, char **argv)
{
    unsigned int i, v;
    int len, option;
    char *p, *arg = NULL, *colon1 = NULL, *colon2 = NULL, *slash = NULL;
    char *server=NULL, *port = DEFAULT_GPSD_PORT, *device = NULL;
    char *controlsock = "/var/run/gpsd.sock";
    fd_set select_set;
    unsigned char buf[BUFLEN];
    char line[80];

    gmt_offset = (int)tzoffset();

    /*@ -branchstate @*/
    while ((option = getopt(argc, argv, "F:Vh")) != -1) {
	switch (option) {
	case 'F':
	    controlsock = optarg;
	    break;
	case 'V':
	    (void)printf("sirfmon %s\n", VERSION);
	    exit(0);
	case 'h': case '?': default:
	    (void)fputs("usage:  sirfmon [-?hv] [-F controlsock] [server[:port:[device]]]\n", stderr);
	    exit(1);
	}
    }
    /*@ +branchstate @*/
    /*@ -nullpass -branchstate @*/
    if (optind < argc) {
	arg = strdup(argv[optind]);
	colon1 = strchr(arg, ':');
	slash = strchr(arg, '/');
	server = arg;
	if (colon1 != NULL) {
	    if (colon1 == arg)
		server = NULL;
	    else
		*colon1 = '\0';
	    port = colon1 + 1;
	    colon2 = strchr(port, ':');
	    if (colon2 != NULL) {
		if (colon2 == port)
		    port = NULL;
	        else
		    *colon2 = '\0';
		device = colon2 + 1;
	    }
	}
    }
    /*@ +nullpass +branchstate @*/

    /*@ -boolops */
    if (!arg || (arg && !slash) || (arg && colon1 && slash)) {	
	if (!server)
	    server = "127.0.0.1";
	devicefd = netlib_connectsock(server, port, "tcp");
	if (devicefd < 0) {
	    (void)fprintf(stderr, 
			  "%s: connection failure on %s:%s, error %d.\n", 
			  argv[0], server, port, devicefd);
	    exit(1);
	}
	controlfd = open(controlsock, O_RDWR);
	/*@ -compdef @*/
	if (device)
	    command((char *)buf, sizeof(buf), "F=%s\r\n", device);
	else
	    command((char *)buf, sizeof(buf), "O\r\n");	/* force device allocation */
	command((char *)buf, sizeof(buf), "F\r\n");
	device = strdup((char *)buf+7);
	command((char *)buf, sizeof(buf), "R=2\r\n");
	/*@ +compdef @*/
	serial = false;
    } else {
	serial_initialize(device = arg);
	serial = true;
    }
    /*@ +boolops */

    (void)initscr();
    (void)cbreak();
    (void)noecho();
    (void)intrflush(stdscr, FALSE);
    (void)keypad(stdscr, true);

    /*@ -onlytrans @*/
    mid2win   = newwin(7,  80,  0, 0);
    mid4win   = newwin(15, 30,  7, 0);
    mid6win   = newwin(3,  50,  7, 30);
    mid7win   = newwin(4,  50, 10, 30);
    mid9win   = newwin(3,  50, 14, 30);
    mid13win  = newwin(3,  50, 17, 30);
    mid19win  = newwin(17, 50,  7, 30);
    mid27win  = newwin(4,  50, 20, 30);
    cmdwin    = newwin(2,  30, 22, 0);
    if (mid2win==NULL || mid4win==NULL || mid6win==NULL || mid9win==NULL
	|| mid13win==NULL || mid19win==NULL || mid27win==NULL || cmdwin==NULL)
	goto quit;

    debugwin  = newwin(0,   0, 24, 0);
    (void)scrollok(debugwin, true);
    (void)wsetscrreg(debugwin, 0, LINES-21);
    /*@ +onlytrans @*/

    /*@ -nullpass @*/
    (void)wborder(mid2win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid2win, A_BOLD);
    (void)wmove(mid2win, 0,1);
    display(mid2win, 0, 12, " X "); 
    display(mid2win, 0, 21, " Y "); 
    display(mid2win, 0, 30, " Z "); 
    display(mid2win, 0, 43, " North "); 
    display(mid2win, 0, 54, " East "); 
    display(mid2win, 0, 65, " Alt "); 

    (void)wmove(mid2win, 1,1);
    (void)wprintw(mid2win, "Pos:                            m                                    m");
    (void)wmove(mid2win, 2,1);
    (void)wprintw(mid2win, "Vel:                            m/s                                  climb m/s");
    (void)wmove(mid2win, 3,1);
    (void)wprintw(mid2win, "Time:                  GPS:                Heading:                  speed m/s");
    (void)wmove(mid2win, 4,1);
    (void)wprintw(mid2win, "Skew:                   TZ:                HDOP:      M1:        M2:    ");
    (void)wmove(mid2win, 5,1);
    (void)wprintw(mid2win, "Fix:");
    display(mid2win, 6, 24, " Packet type 2 (0x02) ");
    (void)wattrset(mid2win, A_NORMAL);

    (void)wborder(mid4win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid4win, A_BOLD);
    display(mid4win, 1, 1, " Ch SV  Az El Stat  C/N ? A");
    for (i = 0; i < SIRF_CHANNELS; i++) {
	display(mid4win, (int)(i+2), 1, "%2d",i);
    }
    display(mid4win, 14, 4, " Packet Type 4 (0x04) ");
    (void)wattrset(mid4win, A_NORMAL);

    (void)wborder(mid19win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid19win, A_BOLD);
    display(mid19win, 1, 1, "Alt. hold mode:");
    display(mid19win, 2, 1, "Alt. hold source:");
    display(mid19win, 3, 1, "Alt. source input:");
    display(mid19win, 4, 1, "Degraded mode:");
    display(mid19win, 5, 1, "Degraded timeout:");
    display(mid19win, 6, 1, "DR timeout:");
    display(mid19win, 7, 1, "Track smooth mode:");
    display(mid19win, 8, 1, "Static Navigation:");
    display(mid19win, 9, 1, "3SV Least Squares:");
    display(mid19win, 10,1, "DOP Mask mode:");
    display(mid19win, 11,1, "Nav. Elev. mask:");
    display(mid19win, 12,1, "Nav. Power mask:");
    display(mid19win, 13,1, "DGPS Source:");
    display(mid19win, 14,1, "DGPS Mode:");
    display(mid19win, 15,1, "DGPS Timeout:");
    display(mid19win, 1, 26,"LP Push-to-Fix:");
    display(mid19win, 2, 26,"LP On Time:");
    display(mid19win, 3, 26,"LP Interval:");
    display(mid19win, 4, 26,"U. Tasks Enab.:");
    display(mid19win, 5, 26,"U. Task Inter.:");
    display(mid19win, 6, 26,"LP Pwr Cyc En:");
    display(mid19win, 7, 26,"LP Max Acq Srch:");
    display(mid19win, 8, 26,"LP Max Off Time:");
    display(mid19win, 9, 26,"APM enabled:");
    display(mid19win,10, 26,"# of Fixes:");
    display(mid19win,11, 26,"Time btw Fixes:");
    display(mid19win,12, 26,"H/V Error Max:");
    display(mid19win,13, 26,"Rsp Time Max:");
    display(mid19win,14, 26,"Time/Accu:");

    display(mid19win, 16, 8, " Packet type 19 (0x13) ");
    (void)wattrset(mid19win, A_NORMAL);

    (void)wborder(mid6win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid6win, A_BOLD);
    display(mid6win, 1, 1, "Version:");
    display(mid6win, 2, 8, " Packet Type 6 (0x06) ");
    (void)wattrset(mid6win, A_NORMAL);

    (void)wborder(mid7win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid7win, A_BOLD);
    display(mid7win, 1, 1,  "SVs: ");
    display(mid7win, 1, 9,  "Drift: ");
    display(mid7win, 1, 23, "Bias: ");
    display(mid7win, 2, 1,  "Estimated GPS Time: ");
    display(mid7win, 3, 8, " Packet type 7 (0x07) ");
    (void)wattrset(mid7win, A_NORMAL);

    (void)wborder(mid9win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid9win, A_BOLD);
    display(mid9win, 1, 1,  "Max: ");
    display(mid9win, 1, 13, "Lat: ");
    display(mid9win, 1, 25, "Time: ");
    display(mid9win, 1, 39, "MS: ");
    display(mid9win, 2, 8, " Packet type 9 (0x09) ");
    (void)wattrset(mid9win, A_NORMAL);

    (void)wborder(mid13win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid13win, A_BOLD);
    display(mid13win, 1, 1, "SVs: ");
    display(mid13win, 1, 9, "=");
    display(mid13win, 2, 8, " Packet type 13 (0x0D) ");
    (void)wattrset(mid13win, A_NORMAL);

    (void)wborder(mid27win, 0, 0, 0, 0, 0, 0, 0, 0),
    (void)wattrset(mid27win, A_BOLD);
    display(mid27win, 1, 1, "DGPS source: ");
    display(mid27win, 1, 31, "Corrections: ");
    display(mid27win, 3, 8, " Packet type 27 (0x1B) ");
    (void)wattrset(mid27win, A_NORMAL);

    (void)wattrset(cmdwin, A_BOLD);
    if (serial)
    	display(cmdwin, 1, 0, "%s %4d N %d", device, bps, stopbits);
    else
	display(cmdwin, 1, 0, "%s:%s:%s", server, port, device);
    (void)wattrset(cmdwin, A_NORMAL);

    (void)wmove(debugwin,0, 0);

    FD_ZERO(&select_set);

    /* probe for version */
    putbyte(buf, 0, 0x84);
    putbyte(buf, 1, 0x0);
    /*@ -compdef @*/
    (void)sendpkt(buf, 2, device);
    /*@ +compdef @*/

    for (;;) {
	(void)wmove(cmdwin, 0,0);
	(void)wprintw(cmdwin, "cmd> ");
	(void)wclrtoeol(cmdwin);
	(void)refresh();
	(void)wrefresh(mid2win);
	(void)wrefresh(mid4win);
	if (dispmode == 0) {
	    refresh_rightpanel1();
	} else {
	    (void)touchwin(mid19win);
	    (void)wrefresh(mid19win);
	    (void)redrawwin(mid19win);
	}
	(void)wrefresh(debugwin);
	(void)wrefresh(cmdwin);

	FD_SET(0,&select_set);
	FD_SET(devicefd,&select_set);

	if (select(FD_SETSIZE, &select_set, NULL, NULL, NULL) < 0)
	    break;

	if (FD_ISSET(0,&select_set)) {
	    (void)wmove(cmdwin, 0,5);
	    (void)wrefresh(cmdwin);
	    (void)echo();
	    /*@ -usedef -compdef @*/
	    (void)wgetnstr(cmdwin, line, 80);
	    (void)noecho();
	    //(void)move(0,0);
	    //(void)clrtoeol();
	    //(void)refresh();
	    (void)wrefresh(mid2win);
	    (void)wrefresh(mid4win);
	    if (dispmode == 0) {
		refresh_rightpanel1();
	    } else {
		(void)touchwin(mid19win);
		(void)wrefresh(mid19win);
		(void)redrawwin(mid19win);
	    }
	    (void)wrefresh(mid19win);
	    (void)wrefresh(debugwin);
	    (void)wrefresh(cmdwin);

	    if ((p = strchr(line,'\r')) != NULL)
		*p = '\0';

	    if (line[0] == '\0')
		continue;
	    /*@ +usedef +compdef @*/

	    p = line;

	    while (*p != '\0' && !isspace(*p))
		p++;
	    while (*p != '\0' && isspace(*p))
		p++;

	    switch (line[0])
	    {
	    case 'a':		/* toggle 50bps subframe data */
		(void)memset(buf, '\0', sizeof(buf));
		putbyte(buf, 0, 0x80);
		putbyte(buf, 23, 12);
		putbyte(buf, 24, subframe_enabled ? 0x00 : 0x10);
		(void)sendpkt(buf, 25, device);
		break;

	    case 'b':
		if (serial) {
		    v = (unsigned)atoi(line+1);
		    for (ip=rates; ip<rates+sizeof(rates)/sizeof(rates[0]);ip++)
			if (v == *ip)
			    goto goodspeed;
		    break;
		goodspeed:
		    putbyte(buf, 0, 0x86);
		    putlong(buf, 1, v);		/* new baud rate */
		    putbyte(buf, 5, 8);		/* 8 data bits */
		    putbyte(buf, 6, stopbits);	/* 1 stop bit */
		    putbyte(buf, 7, 0);		/* no parity */
		    putbyte(buf, 8, 0);		/* reserved */
		    (void)sendpkt(buf, 9, device);
		    (void)usleep(50000);
		    (void)set_speed(bps = v, stopbits);
		    display(cmdwin, 1, 0, "%s %d N %d", device,bps,stopbits);
		} else {
		    line[0] = 'b';
		    (void)write(devicefd, line, strlen(line));
		    (void)read(devicefd, buf, sizeof(buf));	/* discard response */
		}
		break;

	    case 'c':				/* static navigation */
		putbyte(buf, 0,0x8f);			/* id */
		putbyte(buf, 1, atoi(line+1));
		(void)sendpkt(buf, 2, device);
		break;

	    case 'd':		/* MID 4 rate change -- not documented */
		v = (unsigned)atoi(line+1);
		if (v > 30)
		    break;
		putbyte(buf, 0,0xa6);
		putbyte(buf, 1,0);
		putbyte(buf, 2, 4);	/* satellite picture */
		putbyte(buf, 3, v);
		putbyte(buf, 4, 0);
		putbyte(buf, 5, 0);
		putbyte(buf, 6, 0);
		putbyte(buf, 7, 0);
		(void)sendpkt(buf, 8, device);
		break;

	    case 'l':				/* open logfile */
		if (logfile != NULL) {
		    (void)wprintw(debugwin, ">>> Logging to %s off", logfile);
		    (void)fclose(logfile);
		}

		logfile = fopen(line+1,"a");
		(void)wprintw(debugwin, ">>> Logging to %s on", logfile);
		break;

	    case 'n':				/* switch to NMEA */
		putbyte(buf, 0,0x81);			/* id */
		putbyte(buf, 1,0x02);			/* mode */
		putbyte(buf, 2,0x01);			/* GGA */
		putbyte(buf, 3,0x01);
		putbyte(buf, 4,0x01);			/* GLL */
		putbyte(buf, 5,0x01);
		putbyte(buf, 6,0x01);		  	/* GSA */
		putbyte(buf, 7,0x01);
		putbyte(buf, 8,0x05);			/* GSV */
		putbyte(buf, 9,0x01);
		putbyte(buf, 10,0x01);			/* RNC */
		putbyte(buf, 11,0x01);
		putbyte(buf, 12,0x01);			/* VTG */
		putbyte(buf, 13,0x01);
		putbyte(buf, 14,0x00);			/* unused fields */
		putbyte(buf, 15,0x01);
		putbyte(buf, 16,0x00);
		putbyte(buf, 17,0x01);
		putbyte(buf, 18,0x00);
		putbyte(buf, 19,0x01);
		putbyte(buf, 20,0x00);
		putbyte(buf, 21,0x01);
		putword(buf, 22,bps);
		(void)sendpkt(buf, 24, device);
		goto quit;

	    case 't':				/* poll navigation params */
		dispmode = !dispmode;
		break;

	    case 'q':
		goto quit;

	    case 's':
		len = 0;
		while (*p != '\0')
		{
		    /*@i1@*/(void)sscanf(p,"%x",&v);
		    putbyte(buf, len,v);
		    len++;
		    while (*p != '\0' && !isspace(*p))
			p++;
		    while (*p != '\0' && isspace(*p))
			p++;
		}

		(void)sendpkt(buf, (size_t)len, device);
		break;
	    }
	}

	if (dispmode && (time(NULL) % 10 == 0)){
	    putbyte(buf, 0,0x98);
	    putbyte(buf, 1,0x00);
	    (void)sendpkt(buf, 2, device);
	}

	if ((len = readpkt(buf, sizeof(buf))) != EOF) {
	    decode_sirf(buf,len);
	}
    }
    /*@ +nullpass @*/

 quit:
    if (logfile)
	(void)fclose(logfile);
    (void)endwin();
    exit(0);
}

/* sirfmon.c ends here */