sirf.c

gpsd, a popular GPS daemon.

/* $Id: sirf.c 4661 2008-01-19 22:54:23Z garyemiller $ */
/*
 * This is the gpsd driver for SiRF GPSes operating in binary mode.
 * It also handles uBlox, a SiRF derivative.
 *
 * The advantage: Reports climb/sink rate (raw-mode clients won't see this).
 * The disadvantages: Doesn't return PDOP or VDOP, just HDOP.
 *
 * Chris Kuethe, our SiRF expert, tells us:
 * 
 * "I don't see any indication in any of my material that PDOP, GDOP
 * or VDOP are output. There are quantities called Estimated
 * {Horizontal Position, Vertical Position, Time, Horizonal Velocity}
 * Error, but those are apparently only valid when SiRFDRive is
 * active."
 *
 * "(SiRFdrive is their Dead Reckoning augmented firmware. It
 * allows you to feed odometer ticks, gyro and possibly 
 * accelerometer inputs to the chip to allow it to continue 
 * to navigate in the absence of satellite information, and 
 * to improve fixes when you do have satellites.)"
 *
 * "[When we need RINEX data, we can get it from] SiRF Message #5.
 *  If it's no longer implemented on your receiver, messages
 * 7, 28, 29 and 30 will give you the same information."
 */

#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <unistd.h>
#include <time.h>
#include <stdio.h>

#include "gpsd_config.h"
#include "gpsd.h"
#include "bits.h"
#if defined(SIRF_ENABLE) && defined(BINARY_ENABLE)

#define HI(n)		((n) >> 8)
#define LO(n)		((n) & 0xff)

#ifdef ALLOW_RECONFIGURE
/*@ +charint @*/
static unsigned char enablesubframe[] = {0xa0, 0xa2, 0x00, 0x19,
				 0x80, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x0C,
				 0x10,
				 0x00, 0x00, 0xb0, 0xb3};

static unsigned char disablesubframe[] = {0xa0, 0xa2, 0x00, 0x19,
				  0x80, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x0C,
				  0x00,
				  0x00, 0x00, 0xb0, 0xb3};

static unsigned char modecontrol[] = {0xa0, 0xa2, 0x00, 0x0e,
			      0x88,
			      0x00, 0x00,	/* pad bytes */
			      0x00,		/* degraded mode off */
			      0x00, 0x00,	/* pad bytes */
			      0x00, 0x00,	/* altitude */
			      0x00,		/* altitude hold auto */
			      0x00,		/* use last computed alt */
			      0x00,		/* reserved */
			      0x00,		/* disable degraded mode */
			      0x00,		/* disable dead reckoning */
			      0x01,		/* enable track smoothing */
			      0x00, 0x00, 0xb0, 0xb3};

static unsigned char enablemid52[] = {
	    0xa0, 0xa2, 0x00, 0x08,
	    0xa6, 0x00, 0x34, 0x01, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0xdb, 0xb0, 0xb3};
/*@ -charint @*/
#endif /* ALLOW_RECONFIGURE */


static gps_mask_t sirf_msg_debug(unsigned char *, size_t );
static gps_mask_t sirf_msg_errors(unsigned char *, size_t );

static gps_mask_t sirf_msg_swversion(struct gps_device_t *, unsigned char *, size_t );
static gps_mask_t sirf_msg_navdata(struct gps_device_t *, unsigned char *, size_t );
static gps_mask_t sirf_msg_svinfo(struct gps_device_t *, unsigned char *, size_t );
static gps_mask_t sirf_msg_navsol(struct gps_device_t *, unsigned char *, size_t );
static gps_mask_t sirf_msg_geodetic(struct gps_device_t *, unsigned char *, size_t );
static gps_mask_t sirf_msg_sysparam(struct gps_device_t *, unsigned char *, size_t );
static gps_mask_t sirf_msg_ublox(struct gps_device_t *, unsigned char *, size_t );
static gps_mask_t sirf_msg_ppstime(struct gps_device_t *, unsigned char *, size_t );


bool sirf_write(int fd, unsigned char *msg) {
   unsigned int       crc;
   size_t    i, len;
   bool      ok;

   len = (size_t)((msg[2] << 8) | msg[3]);

   /* calculate CRC */
   crc = 0;
   for (i = 0; i < len; i++)
	crc += (int)msg[4 + i];
   crc &= 0x7fff;

   /* enter CRC after payload */
   msg[len + 4] = (unsigned char)((crc & 0xff00) >> 8);
   msg[len + 5] = (unsigned char)( crc & 0x00ff);

   gpsd_report(LOG_IO, "Writing SiRF control type %02x:%s\n", msg[4], gpsd_hexdump(msg, len+8));
   ok = (write(fd, msg, len+8) == (ssize_t)(len+8));
   (void)tcdrain(fd);
   return(ok);
}

static bool sirf_speed(int ttyfd, speed_t speed)
/* change speed in binary mode */
{
    /*@ +charint @*/
   static unsigned char msg[] = {0xa0, 0xa2, 0x00, 0x09,
		     0x86,
		     0x0, 0x0, 0x12, 0xc0,	/* 4800 bps */
		     0x08,			/* 8 data bits */
		     0x01,			/* 1 stop bit */
		     0x00,			/* no parity */
		     0x00,			/* reserved */
		     0x00, 0x00, 0xb0, 0xb3};
   /*@ -charint @*/

   msg[7] = (unsigned char)HI(speed);
   msg[8] = (unsigned char)LO(speed);
   return (sirf_write(ttyfd, msg));
}

static bool sirf_to_nmea(int ttyfd, speed_t speed)
/* switch from binary to NMEA at specified baud */
{
    /*@ +charint @*/
   static unsigned char msg[] = {0xa0, 0xa2, 0x00, 0x18,
		     0x81, 0x02,
		     0x01, 0x01, /* GGA */
		     0x00, 0x00, /* suppress GLL */
		     0x01, 0x01, /* GSA */
		     0x05, 0x01, /* GSV */
		     0x01, 0x01, /* RMC */
		     0x00, 0x00, /* suppress VTG */
		     0x00, 0x01, 0x00, 0x01,
		     0x00, 0x01, 0x00, 0x01,
		     0x12, 0xc0, /* 4800 bps */
		     0x00, 0x00, 0xb0, 0xb3};
   /*@ -charint @*/

   msg[26] = (unsigned char)HI(speed);
   msg[27] = (unsigned char)LO(speed);
   return (sirf_write(ttyfd, msg));
}

static void sirfbin_mode(struct gps_device_t *session, int mode)
{
    if (mode == 0) {
	(void)sirf_to_nmea(session->gpsdata.gps_fd,session->gpsdata.baudrate);
	session->gpsdata.driver_mode = 0;	/* NMEA */
	(void)gpsd_switch_driver(session, "Generic NMEA");
    } else {
	session->back_to_nmea = false;
	session->gpsdata.driver_mode = 1;	/* binary */
    }
}

static gps_mask_t sirf_msg_debug(unsigned char *buf, size_t len)
{
    char msgbuf[MAX_PACKET_LENGTH*3 + 2];
    int i;

    bzero(msgbuf, (int)sizeof(msgbuf));

    /*@ +charint @*/
    if (0xe1 == buf[0]) {		/* Development statistics messages */
	for (i = 2; i < (int)len; i++)
		(void)snprintf(msgbuf+strlen(msgbuf),
			       sizeof(msgbuf)-strlen(msgbuf),
			       "%c", buf[i]^0xff);
	gpsd_report(LOG_PROG, "DEV 0xe1: %s\n", msgbuf);
    } else if (0xff == (unsigned char)buf[0]) {		/* Debug messages */
	for (i = 1; i < (int)len; i++)
	    if (isprint(buf[i]))
		(void)snprintf(msgbuf+strlen(msgbuf),
			       sizeof(msgbuf)-strlen(msgbuf),
			       "%c", buf[i]);
	    else
		(void)snprintf(msgbuf+strlen(msgbuf),
			       sizeof(msgbuf)-strlen(msgbuf),
			       "\\x%02x", (unsigned int)buf[i]);
	gpsd_report(LOG_PROG, "DBG 0xff: %s\n", msgbuf);
    }
    /*@ -charint @*/
    return 0;
}

static gps_mask_t sirf_msg_errors(unsigned char *buf, size_t len UNUSED)
{
    switch (getuw(buf, 1)) {
    case 2:
	gpsd_report(LOG_PROG, "EID 0x0a type 2: Subframe %d error on PRN %ld\n", getul(buf, 9), getul(buf, 5));
	break;

    case 4107:
	gpsd_report(LOG_PROG, "EID 0x0a type 4107: neither KF nor LSQ fix.\n", getul(buf, 5));
	break;

    default:
	gpsd_report(LOG_PROG, "EID 0x0a: Error ID type %d\n", getuw(buf, 1));
	break;
    }
    return 0;
}

static gps_mask_t sirf_msg_swversion(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    double fv;

    if (len < 20)
	return 0;

    gpsd_report(LOG_INF, "FV  0x06: Firmware version: %s\n", buf+1);
    (void)strlcpy(session->subtype, (char *)buf+1, sizeof(session->subtype));
    fv = atof((char *)(buf+1));
    if (fv < 231) {
	session->driver.sirf.driverstate |= SIRF_LT_231;
	if (fv > 200)
	    sirfbin_mode(session, 0);
    } else if (fv < 232) {
	session->driver.sirf.driverstate |= SIRF_EQ_231;
    } else {
#ifdef ALLOW_RECONFIGURE
	gpsd_report(LOG_PROG, "Enabling PPS message...\n");
	(void)sirf_write(session->gpsdata.gps_fd, enablemid52);
#endif /* ALLOW_RECONFIGURE */
	session->driver.sirf.driverstate |= SIRF_GE_232;
	session->context->valid |= LEAP_SECOND_VALID;
    }
    if (strstr((char *)(buf+1), "ES"))
	gpsd_report(LOG_INF, "Firmware has XTrac capability\n");
    gpsd_report(LOG_PROG, "Driver state flags are: %0x\n", session->driver.sirf.driverstate);
#ifdef NTPSHM_ENABLE
    session->driver.sirf.time_seen = 0;
#endif /* NTPSHM_ENABLE */
#ifdef ALLOW_RECONFIGURE
    if (session->gpsdata.baudrate >= 38400){
	gpsd_report(LOG_PROG, "Enabling subframe transmission...\n");
	(void)sirf_write(session->gpsdata.gps_fd, enablesubframe);
    }
#endif /* ALLOW_RECONFIGURE */
    return DEVICEID_SET;
}

static gps_mask_t sirf_msg_navdata(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    unsigned int words[10];

    if (len != 43)
	return 0;

    //unsigned int chan = (unsigned int)getub(buf, 1);
    //unsigned int svid = (unsigned int)getub(buf, 2);
    words[0] = (unsigned int)getul(buf, 3);
    words[1] = (unsigned int)getul(buf, 7);
    words[2] = (unsigned int)getul(buf, 11);
    words[3] = (unsigned int)getul(buf, 15);
    words[4] = (unsigned int)getul(buf, 19);
    words[5] = (unsigned int)getul(buf, 23);
    words[6] = (unsigned int)getul(buf, 27);
    words[7] = (unsigned int)getul(buf, 31);
    words[8] = (unsigned int)getul(buf, 35);
    words[9] = (unsigned int)getul(buf, 39);
    gpsd_report(LOG_PROG, "50B 0x08\n");
    gpsd_interpret_subframe(session, words);

#ifdef ALLOW_RECONFIGURE
    if (session->gpsdata.baudrate < 38400){
	gpsd_report(LOG_PROG, "Disabling subframe transmission...\n");
	(void)sirf_write(session->gpsdata.gps_fd, disablesubframe);
    }
#endif /* ALLOW_RECONFIGURE */
    return 0;
}

static gps_mask_t sirf_msg_svinfo(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    int	st, i, j, cn;

    if (len != 188)
	return 0;

    gpsd_zero_satellites(&session->gpsdata);
    session->gpsdata.sentence_time
	    = gpstime_to_unix(getsw(buf, 1), getul(buf, 3)*1e-2) - session->context->leap_seconds;
    for (i = st = 0; i < SIRF_CHANNELS; i++) {
	int off = 8 + 15 * i;
	bool good;
	session->gpsdata.PRN[st]       = (int)getub(buf, off);
	session->gpsdata.azimuth[st]   = (int)(((unsigned)getub(buf, off+1)*3)/2.0);
	session->gpsdata.elevation[st] = (int)((unsigned)getub(buf, off+2)/2.0);
	cn = 0;
	for (j = 0; j < 10; j++)
	    cn += (int)getub(buf, off+5+j);

	session->gpsdata.ss[st] = cn/10;
	good = session->gpsdata.PRN[st]!=0 &&
	    session->gpsdata.azimuth[st]!=0 &&
	    session->gpsdata.elevation[st]!=0;