sirfmon.c

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

	(void)touchwin(mid27win);
	display(mid27win, 1, 14, "                                                                                     ");
	display(mid27win, 1, 14, "%s", dgpsvec[(int)getub(buf, 1)]);
	for (i = j = 0; i < 12; i++) {
	    (void)touchwin(mid27win);
	    if (/*@i1@*/getub(buf, 16+3*i) != '\0') {
		(void)wprintw(mid27win, "  %d=%d", getub(buf, 16+3*i), getsw(buf, 16+3*i+1));
		j++;
	    }
	}
	display(mid27win, 1, 44, "%d", j);
	(void)wprintw(debugwin, "DST 0x1b=");
	break;

    case 0x1C:	/* NL Measurement Data */
    case 0x1D:	/* DGPS Data */
    case 0x1E:	/* SV State Data */
    case 0x1F:	/* NL Initialized Data */
	subframe_enabled = true;
	break;
    case 0x29:	/* Geodetic Navigation Message */
	(void)wprintw(debugwin, "GNM 0x29=");
	break;
    case 0x32:	/* SBAS Parameters */
	(void)wprintw(debugwin, "SBP 0x32=");
	break;
    case 0x34:	/* PPS Time */
	(void)wprintw(debugwin, "PPS 0x34=");
	break;

#ifdef __UNUSED__
    case 0x62:
	attrset(A_BOLD);
	move(2,40);
	printw("%9.5f %9.5f",(double)(RAD2DEG*1e8*getsl(buf, 1)),
			     (double)(RAD2DEG*1e8*getsl(buf, 5)));
	move(2,63);
	printw("%8d",getsl(buf, 9)/1000);

	move(3,63);

	printw("%8.1f",(double)getsl(buf, 17)/1000);

	move(4,54);
	if (getul(buf, 13) > 50) {
	    double heading = RAD2DEG*1e8*getsl(buf, 21);
	    if (heading < 0)
		heading += 360;
	    printw("%5.1f",heading);
	} else
	    printw("  0.0");

	move(4,63);
	printw("%8.1f",(double)getsl(buf, 13)/1000);
	attrset(A_NORMAL);

	move(5,13);
	printw("%04d-%02d-%02d %02d:%02d:%02d.%02d",
		(int)getuw(buf, 26),getub(buf, 28),getub(buf, 29),getub(buf, 30),getub(buf, 31),
		(unsigned short)getuw(buf, 32)/1000,
		((unsigned short)getuw(buf, 32)%1000)/10);
	{
	    struct timeval clk,gps;
	    struct tm tm;

	    gettimeofday(&clk,NULL);

	    memset(&tm,0,sizeof(tm));
	    tm.tm_sec = (unsigned short)getuw(buf, 32)/1000;
	    tm.tm_min = (int)getub(buf, 31);
	    tm.tm_hour = (int)getub(buf, 30);
	    tm.tm_mday = (int)getub(buf, 29);
	    tm.tm_mon = (int)getub(buf, 28) - 1;
	    tm.tm_year = (int)getuw(buf, 26) - 1900;

	    gps.tv_sec = mkgmtime(&tm);
	    gps.tv_usec = (((unsigned short)getuw(buf, 32)%1000)/10) * 10000;

	    move(5,2);
	    printw("           ");
	    move(5,2);
#if 1	    
	    printw("%ld",(gps.tv_usec - clk.tv_usec) +
	    		 ((gps.tv_sec - clk.tv_sec) % 3600) * 1000000);
#else
	    printw("%ld %ld %ld %ld",gps.tv_sec % 3600,gps.tv_usec,
	    			     clk.tv_sec % 3600,clk.tv_usec);
#endif
	}
		(void)wprintw(debugwin, "??? 0x62=");
    	break;
#endif /* __UNUSED__ */

    case 0xff:		/* Development Data */
	/*@ +ignoresigns @*/
	while (len > 0 && buf[len-1] == '\n')
	    len--;
	while (len > 0 && buf[len-1] == ' ')
	    len--;
	/*@ -ignoresigns @*/
	buf[len] = '\0';
	j = 1;
	for (i = 0; verbpat[i] != NULL; i++)
	    if (strncmp((char *)(buf+1),verbpat[i],strlen(verbpat[i])) == 0) {
		j = 0;
		break;
	    }
	if (j != 0)
	    (void)wprintw(debugwin, "%s\n",buf+1);
	(void)wprintw(debugwin, "DD  0xff=");
	break;

    default:
	(void)wprintw(debugwin, "    0x%02x=", buf[0]);
	break;
    }

    (void)wprintw(debugwin, "(%d) ", len);
    for (i = 1; i < len; i++)
	(void)wprintw(debugwin, "%02x",buf[i]);
    (void)wprintw(debugwin, "\n");
}

/*****************************************************************************
 *
 * Serial-line handling
 *
 *****************************************************************************/

static unsigned int get_speed(struct termios* ttyctl)
{
    speed_t code = cfgetospeed(ttyctl);
    switch (code) {
    case B0:     return(0);
    case B300:   return(300);
    case B1200:  return(1200);
    case B2400:  return(2400);
    case B4800:  return(4800);
    case B9600:  return(9600);
    case B19200: return(19200);
    case B38400: return(38400);
    case B57600: return(57600);
    default: return(115200);
    }
}

static int set_speed(unsigned int speed, unsigned int stopbits)
{
    unsigned int	rate, count, state;
    int st;
    unsigned char	c;

    (void)tcflush(devicefd, TCIOFLUSH);	/* toss stale data */

    if (speed != 0) {
	/*@ +ignoresigns @*/
	if (speed < 300)
	    rate = 0;
	else if (speed < 1200)
	    rate =  B300;
	else if (speed < 2400)
	    rate =  B1200;
	else if (speed < 4800)
	    rate =  B2400;
	else if (speed < 9600)
	    rate =  B4800;
	else if (speed < 19200)
	    rate =  B9600;
	else if (speed < 38400)
	    rate =  B19200;
	else if (speed < 57600)
	    rate =  B38400;
	else
	    rate =  B57600;
	/*@ -ignoresigns @*/

	/*@ ignore @*/
	(void)cfsetispeed(&ttyset, (speed_t)rate);
	(void)cfsetospeed(&ttyset, (speed_t)rate);
	/*@ end @*/
    }
    ttyset.c_cflag &=~ CSIZE;
    ttyset.c_cflag |= (CSIZE & (stopbits==2 ? CS7 : CS8));
    if (tcsetattr(devicefd, TCSANOW, &ttyset) != 0)
	return NO_PACKET;
    (void)tcflush(devicefd, TCIOFLUSH);

    (void)fprintf(stderr, "Hunting at speed %u, %dN%u\n",
	    get_speed(&ttyset), 9-stopbits, stopbits);

    /* sniff for NMEA or SiRF packet */
    state = 0;
    for (count = 0; count < SNIFF_RETRIES; count++) {
	if ((st = (int)read(devicefd, &c, 1)) < 0)
	    return 0;
	else
	    count += st;
	/*@ +charint @*/
	if (state == 0) {
	    if (c == START1)
		state = 1;
	    else if (c == '$')
		state = 2;
	} else if (state == 1) {
	    if (c == START2)
		return SIRF_PACKET;
	    else if (c == '$')
		state = 2;
	    else
		state = 0;
	} else if (state == 2) {
	    if (c == 'G')
		state = 3;
	    else if (c == START1)
		state = 1;
	    else
		state = 0;
	} else if (state == 3) {
	    if (c == 'P')
		return NMEA_PACKET;
	    else if (c == START1)
		state = 1;
	    else
		state = 0;
	}
	/*@ -charint @*/
    }
    
    return NO_PACKET;
}

static unsigned int *ip, rates[] = {0, 4800, 9600, 19200, 38400, 57600};

static unsigned int hunt_open(unsigned int *pstopbits)
{
    unsigned int trystopbits;
    int st;
    /*
     * Tip from Chris Kuethe: the FTDI chip used in the Trip-Nav
     * 200 (and possibly other USB GPSes) gets completely hosed
     * in the presence of flow control.  Thus, turn off CRTSCTS.
     */
    ttyset.c_cflag &= ~(PARENB | CRTSCTS);
    ttyset.c_cflag |= CREAD | CLOCAL;
    ttyset.c_iflag = ttyset.c_oflag = ttyset.c_lflag = (tcflag_t) 0;
    ttyset.c_oflag = (ONLCR);

    for (trystopbits = 1; trystopbits <= 2; trystopbits++) {
	*pstopbits = trystopbits;
	for (ip = rates; ip < rates + sizeof(rates)/sizeof(rates[0]); ip++) {
	    if ((st = set_speed(*ip, trystopbits)) == SIRF_PACKET)
		return get_speed(&ttyset);
	    else if (st == NMEA_PACKET) {
		(void)fprintf(stderr, "Switching to SiRF mode...\n");
		(void)nmea_send(controlfd,"$PSRF100,0,%d,8,1,0", *ip);
		return *ip;
	    }
	}
    }
    return 0;
}

static void serial_initialize(char *device)
{
    if ((controlfd = devicefd = open(device,O_RDWR)) < 0) {
	perror(device);
	exit(1);
    }

    /* Save original terminal parameters */
    if (tcgetattr(devicefd, &ttyset) != 0 || (bps = hunt_open(&stopbits))==0) {
	(void)fputs("Can't sync up with device!\n", stderr);
	exit(1);
    }
}


/******************************************************************************
 *
 * Device-independent I/O routines
 *
 ******************************************************************************/

static int readbyte(void)
{
    /*@ -type -shiftnegative -compdef -nullpass @*/
    static int cnt = 0,pos = 0;
    static unsigned char inbuf[BUFLEN];
    struct timeval timeval;

    if (pos >= cnt) {
	fd_set select_set;

	FD_ZERO(&select_set);
	FD_SET(devicefd,&select_set);
	if (controlfd < -1)
	    FD_SET(controlfd,&select_set);
	timeval.tv_sec = 0;
	timeval.tv_usec = 500000;
	if (select(devicefd + 1,&select_set,NULL,NULL,&timeval) < 0)
	    return EOF;

	if (!FD_ISSET(devicefd,&select_set))
	    return EOF;

	(void)usleep(100000);

	if ((cnt = (int)read(devicefd,inbuf,BUFLEN)) <= 0)
	    return EOF;

	pos = 0;
    }
    /*@ +type +shiftnegative +compdef +nullpass @*/

    return (int)inbuf[pos++];
}

static int readword(void)
{
    int byte1,byte2;

    if ((byte1 = readbyte()) == EOF || (byte2 = readbyte()) == EOF)
	return EOF;

    /*@i@*/return (byte1 << 8) | byte2;
}

/*@ -globstate @*/
static int readpkt(unsigned char *buf, size_t buflen)
{
    int byte,len,csum,cnt;
    unsigned char *cp = buf;

    do {
	while ((byte = readbyte()) != START1)
	    if (byte == EOF)
		return EOF;
    } while ((byte = readbyte()) != START2);

    if ((len = readword()) == EOF || len > BUFLEN)
	return EOF;

    csum = 0;
    assert(len < (int)buflen);
    cnt = len;

    while (cnt-- > 0) {
	if ((byte = readbyte()) == EOF)
	    return EOF;
	*cp++ = (unsigned char)byte;
	csum += byte;
    }

    csum &= 0x7fff;

    if (readword() != csum)
	return EOF;

    if (readbyte() != END1 || readbyte() != END2)
	return EOF;

    if (logfile != NULL) {
	/*@ -shiftimplementation @*/
	(void)fwrite("\xa0\xa2", (size_t)2,  sizeof(char), logfile);
	(void)fputc(len >> 8, logfile);
	(void)fputc(len & 0xff, logfile);
	(void)fwrite(buf, (size_t)len, sizeof(char), logfile);
	(void)fputc(csum >> 8, logfile);
	(void)fputc(csum & 0xff, logfile);
	(void)fwrite("\xb0\xb3", (size_t)2,  sizeof(char), logfile);
	/*@ +shiftimplementation @*/
    }
    return len;
}
/*@ +globstate @*/

static bool sendpkt(unsigned char *buf, size_t len, char *device)
{
    unsigned int csum;
    ssize_t st;
    size_t i;

    putbyte(buf, -4, START1);			/* start of packet */
    putbyte(buf, -3, START2);
    putword(buf, -2, len);			/* length */

    csum = 0;
    for (i = 0; i < len; i++)
	csum += (int)buf[4 + i];

    csum &= 0x7fff;
    putword(buf, len, csum);			/* checksum */
    putbyte(buf, len + 2,END1);			/* end of packet */
    putbyte(buf, len + 3,END2);
    len += 8;

    (void)wprintw(debugwin, ">>>");
    for (i = 0; i < len; i++)
	(void)wprintw(debugwin, " %02x",buf[i]);
    (void)wprintw(debugwin, "\n");

    if (controlfd == -1) 
	return false;
    else {
	if (!serial) {
	    (void)write(controlfd, "!", 1);
	    (void)write(controlfd, device, strlen(device));
	    (void)write(controlfd, "=", 1);
	}
	st = write(controlfd, buf,len);
	if (!serial)
	    (void)read(controlfd, buf, 8);	/* enough room for "ERROR\r\n\0" */
	return ((size_t)st == len);
    }
}

/*****************************************************************************
 *
 * Main sequence and display machinery
 *
 *****************************************************************************/

static long tzoffset(void)
{
    time_t now = time(NULL);
    struct tm tm;
    long res = 0;

    tzset();
#ifdef HAVE_TIMEZONE
    res = timezone;
#else
    res = localtime_r(&now, &tm)->tm_gmtoff;
#endif
#ifdef HAVE_DAYLIGHT
    if (daylight != 0 && localtime_r(&now, &tm)->tm_isdst != 0)
	res -= 3600;
#else
    if (localtime_r(&now, &tm)->tm_isdst)
	res -= 3600;
#endif
    return res;
}

/*@ -nullpass -globstate @*/
static void refresh_rightpanel1(void)
{
    (void)touchwin(mid6win);
    (void)touchwin(mid7win);
    (void)touchwin(mid9win);
    (void)touchwin(mid13win);
    (void)touchwin(mid27win);
    (void)wrefresh(mid6win);
    (void)wrefresh(mid7win);