sirfmon.c

gpsd, a popular GPS daemon.

/* $Id: sirfmon.c 4365 2007-05-31 09:52:27Z esr $ */
/*
 * SiRF packet monitor, originally by Rob Janssen, PE1CHL.
 * Heavily hacked by Eric S. Raymond for use with the gpsd project.
 *
 * Autobauds.  Takes a SiRF chip in NMEA mode to binary mode, if needed.
 * The autobauding code is fairly primitive and can sometimes fail to
 * sync properly.  If that happens, just kill and restart sirfmon.
 *
 * Useful commands:
 *	n -- switch device to NMEA at current speed and exit.
 *	l -- toggle packet logging
 *	a -- toggle receipt of 50BPS subframe data.
 *	b -- change baud rate.
 *	c -- set or clear static navigation mode
 *	s -- send hex bytes to device.
 *	t -- toggle navigation-parameter display mode
 *	q -- quit, leaving device in binary mode.
 *      Ctrl-S -- freeze display.
 *      Ctrl-Q -- unfreeze display.
 *
 * Note: one of the goals of sirfmon.c is *not* to use the gpsdata structure.  
 * sirfmon is intended to be an independent sanity check on SiRF decoding,
 * so it deliberately doesn't use much of the library. 
 */
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <unistd.h>
#include <assert.h>
/* Cygwin has only _timezone and not timezone unless the following is set */
#if defined(__CYGWIN__)
#define timezonevar
#endif /* defined(__CYGWIN__) */
#include <time.h>
#include <termios.h>
#include <fcntl.h>	/* for O_RDWR */
#include <stdarg.h>
#include <stdbool.h>

#include "gpsd_config.h"
#ifdef HAVE_NCURSES_H
#include <ncurses.h>
#else
#include <curses.h>
#endif /* HAVE_NCURSES_H */
#include "gps.h"	/* for DEFAULT_GPSD_PORT; brings in PI as well */

#define PUT_ORIGIN	-4
#include "bits.h"

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

#ifdef S_SPLINT_S
extern struct tm *localtime_r(const time_t *,/*@out@*/struct tm *tp)/*@modifies tp@*/;
#endif /* S_SPLINT_S */

extern int netlib_connectsock(const char *, const char *, const char *);

#define BUFLEN		2048

#define START1		0xa0
#define START2		0xa2
#define END1		0xb0
#define END2		0xb3

#define RAD2DEG (180.0/PI)

/* how many characters to look at when trying to find baud rate lock */
#define SNIFF_RETRIES	1200

static int devicefd = -1, controlfd = -1;
static int nfix,fix[20];
static int gmt_offset;
static bool dispmode = false;
static bool serial, subframe_enabled = false;
static unsigned int stopbits, bps;

/*@ -nullassign @*/
static char *verbpat[] =
{
    "#Time:",
    "@R Time:",
    "CSTD: New almanac for",
    "NOTICE: DOP Q Boost",
    "RTC not set",
    "numOfSVs = 0",
    "rtcaj tow ",
    NULL
};
/*@ +nullassign @*/

static char *dgpsvec[] =
{
    "None",
    "SBAS",
    "Serial",
    "Beacon",
    "Software",
};

static struct termios ttyset;
static WINDOW *mid2win, *mid4win, *mid6win, *mid7win, *mid9win, *mid13win;
static WINDOW *mid19win, *mid27win, *cmdwin, *debugwin;
static FILE *logfile;

#define NO_PACKET	0
#define SIRF_PACKET	1
#define NMEA_PACKET	2

#define display	(void)mvwprintw

/*****************************************************************************
 *
 * NMEA command composition
 *
 *****************************************************************************/

static void nmea_add_checksum(char *sentence)
/* add NMEA checksum to a possibly  *-terminated sentence */
{
    unsigned char sum = '\0';
    char c, *p = sentence;

    if (*p == '$') {
	p++;
	while ( ((c = *p) != '*') && (c != '\0')) {
	    sum ^= c;
	    p++;
	}
	*p++ = '*';
	(void)snprintf(p, 5, "%02X\r\n", (unsigned int)sum);
    }
}

static int nmea_send(int fd, const char *fmt, ... )
/* ship a command to the GPS, adding * and correct checksum */
{
    size_t status;
    char buf[BUFLEN];
    va_list ap;

    va_start(ap, fmt) ;
    (void)vsnprintf(buf, sizeof(buf)-5, fmt, ap);
    va_end(ap);
    (void)strlcat(buf, "*", BUFLEN);
    nmea_add_checksum(buf);
    (void)fputs(buf, stderr);		/* so user can watch the baud hunt */
    status = (size_t)write(fd, buf, strlen(buf));
    if (status == strlen(buf)) {
	return (int)status;
    } else {
	perror("nmea_send");
	return -1;
    }
}

/*****************************************************************************
 *
 * SiRF packet-decoding routines
 *
 *****************************************************************************/

static void decode_time(int week, int tow)
{
    int day = tow / 8640000;
    int tod = tow % 8640000;
    int h = tod / 360000;
    int m = tod % 360000;
    int s = m % 6000;

    m = (m - s) / 6000;

    (void)wmove(mid2win, 3,7);
    (void)wprintw(mid2win, "%4d+%9.2f", week, (double)tow/100);
    (void)wmove(mid2win, 3, 29);
    (void)wprintw(mid2win, "%d %02d:%02d:%05.2f", day, h,m,(double)s/100);
    (void)wmove(mid2win, 4, 8);
    (void)wprintw(mid2win, "%f", timestamp()-gpstime_to_unix(week,tow/100.0));
    (void)wmove(mid2win, 4, 29);
    (void)wprintw(mid2win, "%d", gmt_offset);
}

static void decode_ecef(double x, double y, double z,
			double vx, double vy, double vz)
{
    const double a = 6378137;
    const double f = 1 / 298.257223563;
    const double b = a * (1 - f);
    const double e2 = (a*a - b*b) / (a*a);
    const double e_2 = (a*a - b*b) / (b*b);
    double lambda,p,theta,phi,n,h,vnorth,veast,vup,speed,heading;

    lambda = atan2(y,x);
    /*@ -evalorder @*/ 
    p = sqrt(pow(x,2) + pow(y,2));
    theta = atan2(z*a,p*b);
    phi = atan2(z + e_2*b*pow(sin(theta),3),p - e2*a*pow(cos(theta),3));
    n = a / sqrt(1.0 - e2*pow(sin(phi),2));
    h = p / cos(phi) - n;
    h -= wgs84_separation((double)(RAD2DEG*phi),(double)(RAD2DEG*lambda));
    vnorth = -vx*sin(phi)*cos(lambda)-vy*sin(phi)*sin(lambda)+vz*cos(phi);
    veast = -vx*sin(lambda)+vy*cos(lambda);
    vup = vx*cos(phi)*cos(lambda)+vy*cos(phi)*sin(lambda)+vz*sin(phi);
    speed = sqrt(pow(vnorth,2) + pow(veast,2));
    heading = atan2(veast,vnorth);
    /*@ +evalorder @*/
    if (heading < 0)
	heading += 2 * PI;

    (void)wmove(mid2win, 1,40);
    (void)wprintw(mid2win, "%9.5f %9.5f",(double)(RAD2DEG*phi),
				   (double)(RAD2DEG*lambda));
    (void)mvwaddch(mid2win, 1, 49, ACS_DEGREE);
    (void)mvwaddch(mid2win, 1, 59, ACS_DEGREE);
    (void)wmove(mid2win, 1,61);
    (void)wprintw(mid2win, "%8d",(int)h);

    (void)wmove(mid2win, 2,40);
    (void)wprintw(mid2win, "%9.1f %9.1f",vnorth,veast);
    (void)wmove(mid2win, 2,61);
    (void)wprintw(mid2win, "%8.1f",vup);

    (void)wmove(mid2win, 3,54);
    (void)wprintw(mid2win, "%5.1f",(double)(RAD2DEG*heading));
    (void)mvwaddch(mid2win, 3, 59, ACS_DEGREE);
    (void)wmove(mid2win, 3,61);
    (void)wprintw(mid2win, "%8.1f",speed);
}

static void decode_sirf(unsigned char buf[], int len)
{
    int i,j,ch,off,cn;

    assert(mid27win != NULL);
    switch (buf[0])
    {
    case 0x02:		/* Measured Navigation Data */
	(void)wmove(mid2win, 1,6);	/* ECEF position */
	(void)wprintw(mid2win, "%8d %8d %8d",getsl(buf, 1),getsl(buf, 5),getsl(buf, 9));
	(void)wmove(mid2win, 2,6);	/* ECEF velocity */
	(void)wprintw(mid2win, "%8.1f %8.1f %8.1f",
		(double)getsw(buf, 13)/8,(double)getsw(buf, 15)/8,(double)getsw(buf, 17)/8);
	decode_ecef((double)getsl(buf, 1),(double)getsl(buf, 5),(double)getsl(buf, 9),
		(double)getsw(buf, 13)/8,(double)getsw(buf, 15)/8,(double)getsw(buf, 17)/8);
	decode_time((int)getuw(buf, 22),getsl(buf, 24));
	/* line 4 */
	(void)wmove(mid2win, 4,49);
	(void)wprintw(mid2win, "%4.1f",(double)getub(buf, 20)/5);	/* HDOP */
	(void)wmove(mid2win, 4,58);
	(void)wprintw(mid2win, "%02x",getub(buf, 19));		/* Mode 1 */
	(void)wmove(mid2win, 4,70);
	(void)wprintw(mid2win, "%02x",getub(buf, 21));		/* Mode 2 */
	(void)wmove(mid2win, 5,7);
	nfix = (int)getub(buf, 28);
	(void)wprintw(mid2win, "%d = ",nfix);		/* SVs in fix */
	for (i = 0; i < SIRF_CHANNELS; i++) {	/* SV list */
	    if (i < nfix)
		(void)wprintw(mid2win, "%3d",fix[i] = (int)getub(buf, 29+i));
	    else
		(void)wprintw(mid2win, "   ");
	}
	(void)wprintw(debugwin, "MND 0x02=");
	break;

    case 0x04:		/* Measured Tracking Data */
	decode_time((int)getuw(buf, 1),getsl(buf, 3));
	ch = (int)getub(buf, 7);
	for (i = 0; i < ch; i++) {
	    int sv,st;
	    
	    off = 8 + 15 * i;
	    (void)wmove(mid4win, i+2, 3);
	    sv = (int)getub(buf, off);
	    (void)wprintw(mid4win, " %3d",sv);

	    (void)wprintw(mid4win, " %3d%3d %04x",((int)getub(buf, off+1)*3)/2,(int)getub(buf, off+2)/2,(int)getsw(buf, off+3));

	    st = ' ';
	    if ((int)getuw(buf, off+3) == 0xbf)
		st = 'T';
	    for (j = 0; j < nfix; j++)
		if (sv == fix[j]) {
		    st = 'N';
		    break;
		}

	    cn = 0;

	    for (j = 0; j < 10; j++)
		cn += (int)getub(buf, off+5+j);

	    (void)wprintw(mid4win, "%5.1f %c",(double)cn/10,st);

	    if (sv == 0)			/* not tracking? */
		(void)wprintw(mid4win, "   ");	/* clear other info */
	}
	(void)wprintw(debugwin, "MTD 0x04=");
    	break;

#ifdef __UNUSED__
    case 0x05:		/* raw track data */
	for (off = 1; off < len; off += 51) {
	    ch = getul(buf, off);
	    (void)wmove(mid4win, ch+2, 19);
	    cn = 0;

	    for (j = 0; j < 10; j++)
		cn += getub(buf, off+34+j);

	    printw("%5.1f",(double)cn/10);

	    printw("%9d%3d%5d",getul(buf, off+8),(int)getuw(buf, off+12),(int)getuw(buf, off+14));
	    printw("%8.5f %10.5f",
	    	(double)getul(buf, off+16)/65536,(double)getul(buf, off+20)/1024);
	}
	(void)wprintw(debugwin, "RTD 0x05=");
    	break;
#endif /* __UNUSED */

    case 0x06:		/* firmware version */
	display(mid6win, 1, 10, "%s",buf + 1);
	(void)wprintw(debugwin, "FV  0x06=");
    	break;

    case 0x07:		/* Response - Clock Status Data */
	decode_time((int)getuw(buf, 1),getsl(buf, 3));
	display(mid7win, 1, 5,  "%2d", getub(buf, 7));	/* SVs */
	display(mid7win, 1, 16, "%lu", getul(buf, 8));	/* Clock drift */
	display(mid7win, 1, 29, "%lu", getul(buf, 12));	/* Clock Bias */
	display(mid7win, 2, 21, "%lu", getul(buf, 16));	/* Estimated Time */
	(void)wprintw(debugwin, "CSD 0x07=");
	break;

    case 0x08:		/* 50 BPS data */
	ch = (int)getub(buf, 1);
	display(mid4win, ch+2, 27, "Y");
	(void)wprintw(debugwin, "50B 0x08=");
	subframe_enabled = true;
    	break;

    case 0x09:		/* Throughput */
	display(mid9win, 1, 6,  "%.3f",(double)getuw(buf, 1)/186);	/*SegStatMax*/
	display(mid9win, 1, 18, "%.3f",(double)getuw(buf, 3)/186);	/*SegStatLat*/
	display(mid9win, 1, 31, "%.3f",(double)getuw(buf, 5)/186);	/*SegStatTime*/
	display(mid9win, 1, 42, "%3d",(int)getuw(buf, 7));	/* Last Millisecond */
	(void)wprintw(debugwin, "THR 0x09=");
    	break;

    case 0x0b:		/* Command Acknowledgement */
	(void)wprintw(debugwin, "ACK 0x0b=");
    	break;

    case 0x0c:		/* Command NAcknowledgement */
	(void)wprintw(debugwin, "NAK 0x0c=");
    	break;

    case 0x0d:		/* Visible List */
	display(mid13win, 1, 6, "%d",getub(buf, 1));
	(void)wmove(mid13win, 1, 10);
	for (i = 0; i < SIRF_CHANNELS; i++) {
	    if (i < (int)getub(buf, 1))
		(void)wprintw(mid13win, " %2d",getub(buf, 2 + 5 * i));
	    else
		(void)wprintw(mid13win, "   ");

	}
	(void)wprintw(debugwin, "VL  0x0d=");
    	break;

    case 0x13:
#define YESNO(n)	(((int)getub(buf, n) != 0)?'Y':'N')
	display(mid19win, 1, 20, "%d", getub(buf, 5));	/* Alt. hold mode */
	display(mid19win, 2, 20, "%d", getub(buf, 6));	/* Alt. hold source*/
	display(mid19win, 3, 20, "%dm", (int)getuw(buf, 7));	/* Alt. source input */
	display(mid19win, 4, 20, "%d", getub(buf, 9));	/* Degraded mode*/
	display(mid19win, 5, 20, "%dsec", getub(buf, 10));	/* Degraded timeout*/
	display(mid19win, 6, 20, "%dsec",getub(buf, 11));	/* DR timeout*/
	display(mid19win, 7, 20, "%c", YESNO(12));/* Track smooth mode*/
	display(mid19win, 8, 20, "%c", YESNO(13)); /* Static Nav.*/
	display(mid19win, 9, 20, "0x%x", getub(buf, 14));	/* 3SV Least Squares*/
	display(mid19win, 10,20, "0x%x", getub(buf, 19));	/* DOP Mask mode*/
	display(mid19win, 11,20, "0x%x", (int)getuw(buf, 20));	/* Nav. Elev. mask*/
	display(mid19win, 12,20, "0x%x", getub(buf, 22));	/* Nav. Power mask*/
	display(mid19win, 13,20, "0x%x", getub(buf, 27));	/* DGPS Source*/
	display(mid19win, 14,20, "0x%x", getub(buf, 28));	/* DGPS Mode*/
	display(mid19win, 15,20, "%dsec",getub(buf, 29));	/* DGPS Timeout*/
	display(mid19win, 1, 42, "%c", YESNO(34));/* LP Push-to-Fix */
	display(mid19win, 2, 42, "%dms", getul(buf, 35));	/* LP On Time */
	display(mid19win, 3, 42, "%d", getul(buf, 39));	/* LP Interval */
	display(mid19win, 4, 42, "%c", YESNO(43));/* User Tasks enabled */
	display(mid19win, 5, 42, "%d", getul(buf, 44));	/* User Task Interval */
	display(mid19win, 6, 42, "%c", YESNO(48));/* LP Power Cycling Enabled */
	display(mid19win, 7, 42, "%d", getul(buf, 49));/* LP Max Acq Search Time */
	display(mid19win, 8, 42, "%d", getul(buf, 53));/* LP Max Off Time */
	display(mid19win, 9, 42, "%c", YESNO(57));/* APM Enabled */
	display(mid19win,10, 42, "%d", (int)getuw(buf, 58));/* # of fixes */
	display(mid19win,11, 42, "%d", (int)getuw(buf, 60));/* Time Between fixes */
	display(mid19win,12, 42, "%d", getub(buf, 62));/* H/V Error Max */
	display(mid19win,13, 42, "%d", getub(buf, 63));/* Response Time Max */
	display(mid19win,14, 42, "%d", getub(buf, 64));/* Time/Accu & Duty Cycle Priority */
#undef YESNO
	break;

    case 0x1b:
	/******************************************************************
	 Not actually documented in any published materials.
	 Here is what Chris Kuethe got from the SiRF folks,
	 (plus some corrections from the GpsPaSsion forums):

	Start of message
	----------------
	Message ID          1 byte    27
	Correction Source   1 byte    0=None, 1=SBAS, 2=Serial, 3=Beacon,
	4=Software

	total:              2 bytes

	Middle part of message varies if using beacon or other:
	-------------------------------------------------------
	If Beacon:
	Receiver Freq Hz    4 bytes
	Bit rate BPS        1 byte
	Status bit map      1 byte    01=Signal Valid,
				      02=Auto frequency detect
				      04=Auto bit rate detect
	Signal Magnitude    4 bytes   Note: in internal units
	Signal Strength dB  2 bytes   derived from Signal Magnitude
	SNR  dB             2 bytes

	total:             14 bytes

	If Not Beacon:
	Correction Age[12]  1 byte x 12  Age in seconds in same order as follows
	Reserved            2 bytes

	total:             14 bytes

	End of Message
	--------------
	Repeated 12 times (pad with 0 if less than 12 SV corrections):
	SVID                1 byte
	Correction (cm)     2 bytes (signed short)

	total               3 x 12 = 36 bytes
	******************************************************************/